//Written by WWW.markssupplies.com, there are many other projects on our website so please take time to visit us //I used an Arduino Uno V3 with version 1.0.4 of the Arduino IDE. This code works well, if it doesn't work for you then please double check your wiring before contacting us. //This code is for the ultrasonic sensor HC-SR04 together with 3 cheap 180 degree servos. The 2 main drive servos I have modified to make them into continuous rotation motors. //The HC-SR04 is mounted on a 180 degree servo, I only used one sensor but I have it looking left, right or forward by rotating the servo. //This is a 3 wheeled robot, the 2 main drive motors as mentioned above together with a swivel castor wheel at the front. //The Arduino Uno is powered by a 9v battery. //All 3 servos are powered by a 4 AA battery pack, also connect the ground of this to the ground pin on your Arduino. This ensures that all grounds are connected together. //I initially found some Arduino code on a website, but it didn't mention what ultrasonic library it used, I tried many libraries but none of them were compatible. //I would mention the website here but the code was dreadful, it had many problems, so best not mention it. But it was a good starting place for a newbie like myself. //I fixed all the problems and rewrote the code to use the library called NewPing, you can download it from our website www.markssupplies.com just search on the website for NewPing. //I also used the standard servo library that comes with the Arduino IDE. //So because I am new to this I had to use the reference section from the main Arduino website, the information there is very good & although I struggled to understand some of the explainantions I was just about able to understand the examples. //With my limited understanding I have commented each line with exactly what it does, or at least what I think it does. I hope this helps other beginners out there. //Please contact Ian & Mark at mark@markssupplies.com to report any problems or send us your improvements. #include //This tells the code to include the NewPing library, you will have to download this library and put it into your library folder inside the main arduino folder. #include //This tells the code to include the standard arduino servo library that comes with the arduino IDE. #define TRIGGER_PIN 12 // Arduino pin tied to trigger pin on the ultrasonic sensor. #define ECHO_PIN 13 // Arduino pin tied to echo pin on the ultrasonic sensor. #define MAX_DISTANCE 400 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400cm. If the distance goes over 400 then the sensor value will be zero. This is not really needed but it's a sensor value that could be used for something in the future. #define LeftLED 2 //Attach LED to pin #define RightLED 4 //Attach LED to pin #define ForwardLED 8 //Attach LED to pin NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance. If over max distance we get a zero measurement. const int RForward = 75; // Motor direction & speed. One motor was slightly faster than the other so I changed this from 0 to 75 to slow this motor down. const int RBackward = 180;// Motor direction & speed const int LForward = 180;// Motor direction & speed const int LBackward = 0;// Motor direction & speed const int RNeutral = 89; //90 should stop the motor in between forward & reverse, but this may need tweeking to ensure the motor has definitelt stopped. const int LNeutral = 88; //90 should stop the motor in between forward & reverse, but this may need tweeking to ensure the motor has definitelt stopped. const int dangerThresh = 15; //threshold for obstacles (in cm) int leftDistance = 0; //initial distances for left sensor int rightDistance = 0; //initial distances for right sensor int distanceFwd = 0; //initial distance for forward sensor, I'm trying this out to make the robot go straight when it is first switched on Servo HeadServo; //declare servos Servo LeftServo; //declare servos Servo RightServo; //declare servos void setup() { pinMode(LeftLED, OUTPUT); //Making this pin an output & not an input pinMode(RightLED, OUTPUT); //Making this pin an output & not an input pinMode(ForwardLED, OUTPUT); //Making this pin an output & not an input RightServo.attach(11); //attach servo to proper pins LeftServo.attach(10); //attach servo to proper pins HeadServo.attach(6); //attach servo to proper pins HeadServo.write(90); //centre head servo } void loop() { unsigned int FD = sonar.ping(); //pings & asigns the forward ping time in microseconds as FD forward distance //int distanceFwd = (FD / US_ROUNDTRIP_CM); //converts the ping time to centimetres & asigns it to distanceFwd, if I used this line instead of the one below I didn't have to use the int distanceFwd = 0 on line 36 so I left in for now so I'd remember it. distanceFwd = (FD / US_ROUNDTRIP_CM); //converts the ping time to centimetres & asigns it to distanceFwd delay(500); //Delay to ensure the sensor has enough time if (distanceFwd>dangerThresh || distanceFwd==0) //if path is clear, if the sensor is out of range then it will return a zero, so I added zero to the if statement { LeftServo.write(LForward); //move forward RightServo.write(RForward); //move forward //Comment out the 2 lines above & add the 2 lines below to help check your neutral point for both left & right servos //LeftServo.write(LNeutral); //Use this line to set the neutral point for your left servo //RightServo.write(RNeutral); //Use this line to set the neutral point for your right servo digitalWrite(LeftLED,LOW); // Turns this LED off digitalWrite(RightLED,LOW); // Turns this LED off digitalWrite(ForwardLED,HIGH); // Turns this LED on } else //if path is blocked { LeftServo.write(LNeutral); //Stop left motor RightServo.write(RNeutral); //Stop right motor HeadServo.write(0); //Head servo turn right delay(300); //Delay while the servo turns from centre to right unsigned int RD = sonar.ping(); //pings & asigns the right ping time in microseconds as RD right distance rightDistance = (RD / US_ROUNDTRIP_CM); //converts the ping time to centimetres & asigns it to rightDistance delay(300); //Delay to ensure the ping & calculations have finished HeadServo.write(180); //Head servo turn left delay(700); //Slightly longer delay while the servo turns from right 180 degrees to left unsigned int LD = sonar.ping(); //pings & asigns the left ping in microseconds time as LD left distance leftDistance = (LD / US_ROUNDTRIP_CM); //converts the ping time to centimetres & asigns it to leftDistance delay(300); //Delay to ensure the ping & calculations have finished HeadServo.write(90); //return head to center delay(300); //Delay while head returns to center compareDistance(); //This starts the compareDistance code below } //Add the 7 lines below for debugging with serial monitor //Serial.begin( 9600 ); //Serial.print(distanceFwd); //Serial.println("Forward cm"); //Serial.print(rightDistance); //Serial.println("Right cm"); //Serial.print(leftDistance); //Serial.println("Left cm"); } void compareDistance() { if (leftDistance==0 || leftDistance>=rightDistance) //if left is less obstructed or equal to left distance, a zero ping is out of range which also means it is clear, so with an equal option of left or right this robot will turn left { //LeftServo.writeMicroseconds(2000); // set servo to reverse, this is another way of controlling the motors, I kept it in to remind me how to do it LeftServo.write(LBackward); //turning left //RightServo.writeMicroseconds(2000); // set servo to forward, this is another way of controlling the motors, I kept it in to remind me how to do it RightServo.write(RForward); //turning left delay(450); //a delay of 450 makes my robot turn 90 degrees & run away from obstacles, depending on how fast or slow your servo is & how good your batteries are you may need to change this delay, you may want it to turn less than 90 degrees which would make the robot swerve around things rather than boxing around them digitalWrite(RightLED,LOW); // Turns this LED off digitalWrite(ForwardLED,LOW); // Turns this LED off digitalWrite(LeftLED,HIGH); // Turns this LED on } else if (rightDistance==0 || rightDistance>leftDistance) //if right is less obstructed or a zero ping which also means it is clear { //LeftServo.writeMicroseconds(2000); // set servo to forward, this is another way of controlling the motors, I kept it in to remind me how to do it LeftServo.write(LForward); //turning right //RightServo.writeMicroseconds(2000); // set servo to reverse, this is another way of controlling the motors, I kept it in to remind me how to do it RightServo.write(RBackward); //turning right delay(450); //a delay of 450 makes my robot turn 90 degrees & run away from obstacles, depending on how fast or slow your servo is & how good your batteries are you may need to change this delay, you may want it to turn less than 90 degrees which would make the robot swerve around things rather than boxing around them digitalWrite(LeftLED,LOW); // Turns this LED off digitalWrite(ForwardLED,LOW); // Turns this LED off digitalWrite(RightLED,HIGH); // Turns this LED on } //I've left the 8 lines below in, this is to show how you can add an else which captures anything not caught in the 2 ifs above. But this particular else is not needed now because I added an = sign in the first if that covers what the 7 lines below did. // It is also possible to add more else if statements like the one above, so you can come up with different scenarios & make your robot do different things. //else //if they are equally obstructed //{ //LeftServo.write(LForward); //turn 180 degrees //RightServo.write(RBackward); //turn 180 degrees //delay(900); //a delay of 900 makes my robot turn 180 degrees & run away from obstacles, depending on how fast or slow your servo is & how good your batteries are you may need to change this delay //} }