Servo/SR04/Motor shield problem!

Hi,

I'm trying to finish my first Arduino UNO project; an ultrasonic obstacle avoidance rover. I'm having difficulties getting my sensor to operate simultaneously with the servo it is mounted on. The idea is to create a 180-degree scan for nearby objects to navigate a path. Is there a way to make my sensor take readings whilst the servo is rotating the head? Please have a read of my code so far and suggest how I could make this work. The robot is powered by 9v, two DC motors, an Adafruit motor shield, SR04 sensor and a standard 9g micro servo - p.s I only just started learning Arduino two days ago :wink: Please help me! :
#include <NewPing.h>
#include <Servo.h>
#include <AFMotor.h> //
#define trigPin A4
#define echoPin A5
AF_DCMotor motor1(1,MOTOR12_64KHZ); // set up motors.
AF_DCMotor motor2(4, MOTOR12_64KHZ);

Servo myservo; // create servo object to control a servo
int pos = 0; // this sets up variables for use in the sketch (code)

void setup() {

Serial.begin(9600); // begin serial communication
Serial.println("Motor test!");
pinMode(trigPin, OUTPUT);// set the trig pin to output (Send sound waves)
pinMode(echoPin, INPUT);// set the echo pin to input (recieve sound waves)
motor1.setSpeed(105); //set the speed of the motors, between 0-255
motor2.setSpeed (105);

myservo.attach(9); // attaches the servo on pin 9 to the servo object
myservo.write(90); // tells the servo to position at 90-degrees ie. facing forward.

}

void loop() {

long duration, distance; // start the scan
digitalWrite(trigPin, LOW);
delayMicroseconds(2); //
digitalWrite(trigPin, HIGH);

delayMicroseconds(10); //
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 30;//
if (distance < 25)/*if there's an obstacle 25 centimers, ahead, do the following: */ {
Serial.println ("Close Obstacle detected!" );
Serial.println ("Obstacle Details:");
Serial.print ("Distance From Robot is " );
Serial.print ( distance);
Serial.print ( " CM!");// print out the distance in centimeters.

Serial.println (" The obstacle is declared a threat due to close distance. ");
Serial.println (" Turning !");

motor1.run(FORWARD); // Turn as long as there's an obstacle ahead.
motor2.run (BACKWARD);

if (distance < 10)
motor1.run(BACKWARD);
motor2.run(BACKWARD);
delay(400); // run motors this way for 400ms
}

else {
Serial.println ("No obstacle detected. going forward");
delay (15);
motor1.run(FORWARD); //if there's no obstacle ahead, Go Forward!
motor2.run(FORWARD);
}

for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
// in steps of 1 degree
myservo.write(pos); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
myservo.write(pos); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}

}

First, you really should have read the How to use this forum - please read post at the top of the index page and How to use this forum before posting.

ie Your code and any error messages should always be placed between code tags. Posting it inline as you have done makes it much harder to read or copy and paste for diagnosis.

It's still not too late to edit your post and do this. You'll make potential helpers much happier. :slight_smile:

It's also a good idea to format your code correctly when doing this. Ctrl-T in the IDE will format your code, or you can click on >Tools >Auto-Format


Now, to business - what I did with my robot was have the servo move to 5 discrete positions, taking a distance measurement in each position. Left, Mid-left, Centre, Mid-right and Right.
Then when you process the results, you can more easily work out a direction to turn. I have mine turn in the direction with the longest clear path.