Robot Car Part 3 Code Preview

Update: If you want to control the motor speeds, please swap pin 11 and pin 13 connections, as well as update the define section in the code. Pin 13 is not PWM pin but pin 11 is. sorry for the mistake.

This is the code preview for the smart robot car tutorial. Load it to your arduino board then it should work. Please let me know down below if you running to any problems.

#include <Servo.h>

// Pins
#define TRIG_PIN A0
#define ECHO_PIN A1

//Define all the connections maps to the L298N
#define enA 13
#define in1 12
#define in2 11
#define in3 7
#define in4 6
#define enB 5
#define servoPin 2

class Motor{

int enablePin;
int directionPin1;
int directionPin2;

public:

//Method to define the motor pins
Motor(int ENPin,int dPin1,int dPin2){
enablePin = ENPin;
directionPin1 = dPin1;
directionPin2 = dPin2;
};

//Method to drive the motor 0~255 driving forward. -1~-255 driving backward
void Drive(int speed){
if(speed>=0){
digitalWrite(directionPin1, LOW);
digitalWrite(directionPin2, HIGH);
}
else{
digitalWrite(directionPin1, HIGH);
digitalWrite(directionPin2, LOW);
speed = - speed;
}
analogWrite(enablePin, speed);
}
};

Motor leftMotor = Motor(enA, in1, in2);
Motor rightMotor = Motor(enB, in3, in4);
Servo myservo; // create servo object to control a servo

void motorInitiation(){
pinMode(enA, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
// Set initial direction and speed
digitalWrite(enA, LOW);
digitalWrite(enB, LOW);
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
}

//Variables--------------------------------------------------------------------------
// Anything over 400 cm (23200 us pulse) is "out of range"
const unsigned int MAX_DIST = 23200;
bool ObsticalAhead = false;
int servoPos = 90;

enum Directions { Forward, TurnLeft, TurnRight, TurnAround,Brake};

Directions nextStep = Forward;

unsigned long t1;
unsigned long t2;
unsigned long pulse_width;
float cm;
float inches;

//SETUP--------------------------------------------------------------------------
void setup() {

// The Trigger pin will tell the sensor to range find
pinMode(TRIG_PIN, OUTPUT);
digitalWrite(TRIG_PIN, LOW);

// We'll use the serial monitor to view the sensor output
Serial.begin(9600);
myservo.attach(servoPin);
motorInitiation();
Directions nextStep = Forward;
}

void loop() {

checkDistance();
checkDirection();
drive();
}

void checkDistance(){

// Hold the trigger pin high for at least 10 us
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);

// Wait for pulse on echo pin
while ( digitalRead(ECHO_PIN) == 0 );

// Measure how long the echo pin was held high (pulse width)
// Note: the micros() counter will overflow after ~70 min
t1 = micros();
while ( digitalRead(ECHO_PIN) == 1);
t2 = micros();
pulse_width = t2 - t1;

// Calculate distance in centimeters and inches. The constants
// are found in the datasheet, and calculated from the assumed speed
//of sound in air at sea level (~340 m/s).
cm = pulse_width / 58.0;
inches = pulse_width / 148.0;

// Print out results
if ( pulse_width > MAX_DIST ) {
//Serial.println("Out of range");
} else {
//Serial.print(cm);
//Serial.print(" cm \t");
//Serial.print(inches);
//Serial.println(" in");
}

// Wait at least 60ms before next measurement
delay(60);

if(cm<= 20){
ObsticalAhead = true;
Serial.println("Problem Ahead");

}
else{ ObsticalAhead = false;}

}
void checkDirection(){
Serial.println("checking direction");
if(ObsticalAhead ==true){
nextStep = Brake;
drive();
myservo.write(180); // tell servo to go to position in variable 'pos'
delay(400); // waits 15ms for the servo to reach the position
checkDistance();
if(ObsticalAhead ==false){//if left side is open
nextStep = TurnLeft;
Serial.println("Next step is TurnLeft");
myservo.write(90);//reset servo position
delay(400);
}
else{// left is blocked, now need to look at right
myservo.write(0); // tell servo to go to position in variable 'pos'
delay(800); // waits 15ms for the servo to reach the position
checkDistance();
if(ObsticalAhead ==false){//if right side is open
nextStep = TurnRight;
Serial.println("Next step is TurnRight");
myservo.write(90);//reset servo position
delay(400);
}
else{//right is blocked as well, need to turn around
nextStep = TurnAround;
myservo.write(90);//reset servo position
delay(300);
Serial.println("Next step is TurnAround");
}
}

}
else{nextStep = Forward;}//No obstical ahead
}

void drive(){
switch (nextStep){
case Forward:
leftMotor.Drive(255);
rightMotor.Drive(255);
Serial.println("Forward");

break;
case TurnLeft:
leftMotor.Drive(-255);
rightMotor.Drive(255);
Serial.println(" TurnLeft");
delay(400);

break;
case TurnRight:
leftMotor.Drive(255);
rightMotor.Drive(-255);
Serial.println(" TurnRight");
delay(400);
break;
case TurnAround:
leftMotor.Drive(255);
rightMotor.Drive(-255);
Serial.println(" TurnAround");
delay(600);
break;

case Brake:
leftMotor.Drive(0);
rightMotor.Drive(0);
Serial.println(" stopped");
}

}

5,061 total views, 0 views today

61 thoughts on “Robot Car Part 3 Code Preview”

  1. Hi Chen,

    Looks like you made some changes on the cables, here is the pin definitions to match your previous posts:

    // Pins
    #define TRIG_PIN A0
    #define ECHO_PIN A1

    //Define all the connections maps to the L298N
    #define enA 10
    #define in1 8
    #define in2 9
    #define in3 6
    #define in4 7
    #define enB 5
    #define servoPin 0

    So far i got the robot to go around and measure distance, but it seems like after turning on the wheels the servo doesn’t have enough power to move so the robot turn around every time it finds an obstacle, did you had any kind of power issues with your setup?

    I put the following at the beginning of the setup function, so i know the servo is working:

    myservo.attach(servoPin);
    myservo.write(90);
    delay(400);
    myservo.write(25);
    delay(400);
    myservo.write(90);
    delay(400);
    myservo.write(155);
    delay(400);
    myservo.write(90);
    delay(400);

    The servo “looks around” when i start the robot, but after it find an obstacle the servo never moves again, I had put some comments on the IFs that move the servos and the logic seems to be working fine.

    Hope you can provide some comments,

    1. Hey Claudio, yeah the pins have to be changed due to some conflicts.

      I suffered all sort of problem and I realized it’s most likely due to the power. Seems like in low current conditions the ultra sonic sensor returns weird signals, sometimes it runs into things, sometimes it turns around without stop. So make sure your aa batteries are fresh, or even try to connect a 9v battery to arduino Vin and Gnd (Not the sensor shield Vcc and Gnd).

      Let me know if this helps.

  2. Hello Chen,
    Great tutorial, it was so helpful.
    I had to replace “&gt” with > to make it work.
    The robot is working but I still get a minor error :
    warning: ISO C++ forbids declaration of ‘Drive’ with no type [-fpermissive]
    Drive(int speed){
    and I am not able to change the motor speed other than 255.
    Anyway, I m learning along my daughter. Thank you!

    1. Hi Cedric, glad I can help! 😀

      Yeah I noticed that “>” was copied wrong when writing the post, I thought I fixed it by retyping. Thanks for pointing that out.

      The warning is new to me. In my test, when I changed the speed parameter, there seems to have no significant speed change other than anything lower than 128 that will stop the motor. I haven’t look into it but it might be this motor or the L298N. I’ll share whatever I found in the future!

      Let me know if you have any other questions and happy making!

  3. I am a real Arduino novice and tried compiling your sketch but it keeps failing, saying GT not declared. When I change the &gt to > it still fails giving me another error. Can you offer any help please?

    1. Hi Steve,

      Sorry about the inconvenience. I just checked the code and updated it. Could you run it again and let me know if it works for you?

      Thanks!

      Chen

      1. Hi Chen,

        Thanks for your reply, I managed to figure out the typos and got the sketch to compile but I had speed=0 and not speed >= zero, now amended, Thanks. I have not uploaded it to the car yet still to do a bit of soldering.

        I do have another question though if you don’t mind answering! You have the battery connected to the L298N 5V and GND but the 4 x AA cells gives 6V! Will the L298N be OK with 6V on the 5V pin and what about 6V going to the Arduino, the Vcc on the shield goes to the 5V pins? will that not damage the Arduino?

        Thanks Again

        Steve

        1. Hi Steve, the L298N 5v pin supports input from 5 to 7v. So you are good. In the case the input is more than 7. You could use the 12v pin for input and enable the onboard regulator. Here is my video about L298N pins in detail: https://youtu.be/TMwBQ4sb3XY

          As for the arduino, I have no problem running this way with 4 AAs. But your concern is right that it might not be the best practise. My quick reserch showing vcc can support up to 5.5v input but I need investigate more. To be safe, you could connect the vcc wire from the L298N 5V to the Vin on Arduino. Just like the way I connect the 9V battery clip in the part 3 video. The Vin supports 5 to 12v input since there is a voltage regulator inside.

          Hope this help and let me know how it goes!

          Chen

          1. Hi Chen,

            Car well on now, I had severe problems getting my iMac to work with the Arduino board ( no port to attach to) so brought out an old laptop. In your main code you changed from Pins 8, 9 & 10 6 to Pins 11, 12 & 13. with those pins chosen and the motor test sketch altered to suit, one motor would not run! when I changed things back to Pins 7, 8 & 9 the motor test program works fine. The only thing I could see that needed changing was the define statements for 8, 9 & 10. Any ideas why Pins 11, 12 & 13 don’t work?

            Thanks

            Steve

  4. Hi Chen

    Having fun putting this together. Nice work on the videos.

    I am having trouble with the code, I am rather new to this.
    I have wrote it up on the arduino upload page, But when I tried
    To verify it, I get a (field ‘left Motor’ has incomplete type motor)
    This is on line 46. I am pretty sure I copied it right.

    Any help will be appreciated

      1. I believe it’s the stand alone version.

        When I tried to check the code it says that the motor
        Info is not complete.

        Looking to recheck the code to see if I miss anything.

        1. oK, I just rechecked all the code and fixed all my mistakes, now when I verify the code I get an (expected initializer before ‘int’ on line 42.
          Is there an issue with using the web base version or the stand alone version?

          1. Hey Ray, I actually updated the full code couple days ago. Can you try it and let me know? It should be error free. I never used the web version so really not sure.

  5. Hi Chen,
    very good tutorial and easy to follow along!
    I encountered a strange issue, with enA and enB. No matter if i choose pin 13 and 5 or 8 and 5 (changing the define accordingly and of course the wiring), the motors do not move. When i connect the jumpers to enA and enB the motors drive. Do you know what the reason might be for this issue?

    1. Hi mc_plectrum, do you mind to post your code so I can take a look? The motors drive so the L298 should be fine. I think it might be a connection/software problem.

      1. I Found the issue. It was just wrong wiring of the power supply. Thanks a lot for your great support to all the questions on this page!

  6. Hi Chen,

    There is no reply link below your last reply to me. The code I used was the motor test code and I changed 8, 9 & 10 in the define section to 11, 12 & 13. I did not do anything else. When I upload only one motor runs, yet when I change it back to 8, 9 & 10 all works fine. Obviously, I am changing the connections on the shield from 8, 9 & 10 to 11, 12 & 13 and back. Any ideas?

    Regards

    Steve

      1. Hi Chen,

        Changed to Pins 1, 2 & 3 and it worked fine. Tried the basic code but nothing worked at all! Thought I’d try Pins 11, 12 & 13 separately on a blinking LED sketch, Pins 11 & 12 worked fine but not Pin 13. It appears there may be a fault in my Shield as Pin 13 works without the shield plugged in but with it plugged in the on board LED on the Arduino does not blink. Unless you can think of something else that may be causing it?

        Regards

        Steve

        1. Yeah I think your trouble shooting is on the mark. I think you can use the pin123 for the motor and just don’t use the serial monitor functions. When the car is running untethered, you couldn’t use the monitor anyway. Just comment out or delete all the Serial.begin and Serial.print functions. Then you can use pin 0 for your servo with ultrasonic snesor.

          1. Tried as you said, see code below. Nothing happens apart form servo twitching. Any pointers would be welcome. Thanks

            #include

            // Pins
            #define TRIG_PIN A0
            #define ECHO_PIN A1

            //Define all the connections maps to the L298N
            #define enA 3
            #define in1 2
            #define in2 1
            #define in3 7
            #define in4 6
            #define enB 5
            #define servoPin 0

            class Motor{

            int enablePin;
            int directionPin1;
            int directionPin2;

            public:

            //Method to define the motor pins
            Motor(int ENPin,int dPin1,int dPin2){
            enablePin = ENPin;
            directionPin1 = dPin1;
            directionPin2 = dPin2;
            };

            //Method to drive the motor 0~255 driving forward. -1~-255 driving backward
            void Drive(int speed){
            if(speed>=0){
            digitalWrite(directionPin1, LOW);
            digitalWrite(directionPin2, HIGH);
            }
            else{
            digitalWrite(directionPin1, HIGH);
            digitalWrite(directionPin2, LOW);
            speed = – speed;
            }
            analogWrite(enablePin, speed);
            }
            };

            Motor leftMotor = Motor(enA, in1, in2);
            Motor rightMotor = Motor(enB, in3, in4);
            Servo myservo; // create servo object to control a servo

            void motorInitiation(){
            pinMode(enA, OUTPUT);
            pinMode(in1, OUTPUT);
            pinMode(in2, OUTPUT);
            pinMode(enB, OUTPUT);
            pinMode(in3, OUTPUT);
            pinMode(in4, OUTPUT);
            // Set initial direction and speed
            digitalWrite(enA, LOW);
            digitalWrite(enB, LOW);
            digitalWrite(in1, LOW);
            digitalWrite(in2, HIGH);
            digitalWrite(in3, LOW);
            digitalWrite(in4, HIGH);
            }

            //Variables————————————————————————–
            // Anything over 400 cm (23200 us pulse) is “out of range”
            const unsigned int MAX_DIST = 23200;
            bool ObsticalAhead = false;
            int servoPos = 90;

            enum Directions { Forward, TurnLeft, TurnRight, TurnAround,Brake};

            Directions nextStep = Forward;

            unsigned long t1;
            unsigned long t2;
            unsigned long pulse_width;
            float cm;
            float inches;

            //SETUP————————————————————————–
            void setup() {

            // The Trigger pin will tell the sensor to range find
            pinMode(TRIG_PIN, OUTPUT);
            digitalWrite(TRIG_PIN, LOW);

            // We’ll use the serial monitor to view the sensor output
            // Serial.begin(9600);
            myservo.attach(servoPin);
            motorInitiation();
            Directions nextStep = Forward;
            }

            void loop() {

            checkDistance();
            checkDirection();
            drive();
            }

            void checkDistance(){

            // Hold the trigger pin high for at least 10 us
            digitalWrite(TRIG_PIN, HIGH);
            delayMicroseconds(10);
            digitalWrite(TRIG_PIN, LOW);

            // Wait for pulse on echo pin
            while ( digitalRead(ECHO_PIN) == 0 );

            // Measure how long the echo pin was held high (pulse width)
            // Note: the micros() counter will overflow after ~70 min
            t1 = micros();
            while ( digitalRead(ECHO_PIN) == 1);
            t2 = micros();
            pulse_width = t2 – t1;

            // Calculate distance in centimeters and inches. The constants
            // are found in the datasheet, and calculated from the assumed speed
            //of sound in air at sea level (~340 m/s).
            cm = pulse_width / 58.0;
            inches = pulse_width / 148.0;

            // Print out results
            if ( pulse_width > MAX_DIST ) {
            //Serial.println(“Out of range”);
            } else {
            //Serial.print(cm);
            //Serial.print(” cm \t”);
            //Serial.print(inches);
            //Serial.println(” in”);
            }

            // Wait at least 60ms before next measurement
            delay(60);

            if(cm<= 20){
            ObsticalAhead = true;
            // Serial.println("Problem Ahead");

            }
            else{ ObsticalAhead = false;}

            }
            void checkDirection(){
            //Serial.println("checking direction");
            if(ObsticalAhead ==true){
            nextStep = Brake;
            drive();
            myservo.write(180); // tell servo to go to position in variable 'pos'
            delay(400); // waits 15ms for the servo to reach the position
            checkDistance();
            if(ObsticalAhead ==false){//if left side is open
            nextStep = TurnLeft;
            //Serial.println("Next step is TurnLeft");
            myservo.write(90);//reset servo position
            delay(400);
            }
            else{// left is blocked, now need to look at right
            myservo.write(0); // tell servo to go to position in variable 'pos'
            delay(800); // waits 15ms for the servo to reach the position
            checkDistance();
            if(ObsticalAhead ==false){//if right side is open
            nextStep = TurnRight;
            // Serial.println("Next step is TurnRight");
            myservo.write(90);//reset servo position
            delay(400);
            }
            else{//right is blocked as well, need to turn around
            nextStep = TurnAround;
            myservo.write(90);//reset servo position
            delay(300);
            // Serial.println("Next step is TurnAround");
            }
            }

            }
            else{nextStep = Forward;}//No obstical ahead
            }

            void drive(){
            switch (nextStep){
            case Forward:
            leftMotor.Drive(255);
            rightMotor.Drive(255);
            // Serial.println("Forward");

            break;
            case TurnLeft:
            leftMotor.Drive(-255);
            rightMotor.Drive(255);
            // Serial.println(" TurnLeft");
            delay(400);

            break;
            case TurnRight:
            leftMotor.Drive(255);
            rightMotor.Drive(-255);
            // Serial.println(" TurnRight");
            delay(400);
            break;
            case TurnAround:
            leftMotor.Drive(255);
            rightMotor.Drive(-255);
            // Serial.println(" TurnAround");
            delay(600);
            break;

            case Brake:
            leftMotor.Drive(0);
            rightMotor.Drive(0);
            // Serial.println(" stopped");
            }
            }

          2. Hi Chen,

            Did as you suggested but the only thing that happens is the servo turns one way then the other and centres. No motors run at all. I will post the code tomorrow, need to get it off my laptop.

          3. Steve, what you described is the sequence when the car see obsticals in all directions. Are you powering it with usb? Or 4AAs? I suggestyou isolate the code in the void loop codes. Comment out everything other than the drive function and see what happens?

  7. Chen,

    I am using a 4 cell NiMH battery connected to the Arduino external socket.
    I will try isolating the code but not 100% sure all what to comment out and what to keep. I am wondering if the sensor shield might be causing other problems knowing the the Pin 13 issue is there. I have ordered a new one to try. I’ll see how I get on but will probably be in touch soon!

    Thanks again for all your assistance.

    Best Regards

    Steve

    1. Chen,

      I am getting some success, the NiMH battery is possibly flat. Tried new AA batteries and in is moving forward in millimetre steps, very very slowly. It seems to see obstacles and turns just as slowly! How can I speed it up? Or is there something else wrong?

      Thanks

      Steve

  8. Chen,

    It would appear that the 4 x AA cells is too low to be plugged into Arduino external power socket. I have reverted to your original design and have connected the 4 x AA cells to the sensor board and all appears to be working. A few tweaks on the turning angles and the motor speeds, other than that all is good. Once again thanks for your help.

    Cheers

    Steve

    1. That’s great! Hope you are having fun with it now! What would be your next project? Do you see any kits on the market you are thinking buying?

      I’d love to get something for new tutorials.

      Thanks!

      Chen

      1. Chen,

        I want to play about with the car for a while and try a few different things with it. I need to learn more about Arduino coding before I go for a more sophisticated kit. Going through your code there are things I can’t get my head round, so still on this steep learning curve.

        Regards

        Steve

  9. Hi,
    Thank for the video. can you give us the new drawing. My car is only moving forward. the servo and the sensor are not working.

    1. Hi Nii, have you tried the part 2 codes piece by piece yet to verify every part works? The code in this post should work. Make sure you ultra sonic sensor is reading correctly. Also try to use fresh batteries.

  10. Hi Chen,

    Thank you for your well explained tutorial. How can you:
    1. Make the Robot to go a specific distance forward, and a specific distance right or left? 2. And how can you make the Robot to apply the breaks if the Range sensor detects an object about 7cm away?

    Thanks in anticipation of your reply.

  11. I’m trying to make it go from the start Forward about 4 feet and stop. Then turn Right and go Forward about 1 Foot and stop to Drop a load with the Servo. Proceed Forward about 2 Feet and finally stop when the Range Sensor detects an object 6in or 15cm away.

    Can you help me here?
    Thanks

    1. Hi Jajay, I think the last question is the easiest, right now the program can run any code when detecting any distance in the available range(about 3 cm to 40 cm?) The first 2 questions, depends on how accurate you want it to be. Right now the kit does not include anything to tell the system the exact distance. We can somewhat achieve your goal here by tweaking the delay time, but of course it depends on the motors and batteries. The right way to do it is to apply an encoder like this one: https://www.banggood.com/HC-020K-Double-Speed-Measuring-Module-With-Speed-Encoder-Kit-p-970327.html?p=JH2514288496201309O3

      that measures exactly how many circles it ran, so you can achieve your goal precisely. I might make a video about it in the future. Let me know if you have any other questions.

      1. Thank you Chen for being so kind to reply.
        You tutorial and code worked very well for me. Only the battery power was an issue. I just want to modify the code for an entirely different project that I am working on. In my current project the Range Sensor is stationary facing forward. The Servo will carry a ball to be dropped off at a specified location by rotating and tilting.
        I just want to know how to make it to only go Forward, then turn Right and Forward again and make a full stop when it reaches 15 cm before a wall. How can I modify the code to stop at approximately 15 cm before an object? I will appreciate your help with this.
        I will definitely try using the encoders from the link you forwarded to see if they will work for me and I will let you know.

        1. Hi Jajay,

          Here’s a high level of what you are looking for, I’ll ignore the syntax for a moment so don’t copy past.

          in your void loop{
          Goforward();
          Delay(amount of the time for your distance);

          TurnRight();
          Delay(amount of the time to turn 90 degrees right)

          Goforward();
          while(rangeSensorDistance >15cm){
          //do nothing here. it will keep going
          }
          //the while() will stop when the sensor range = 15cm, so we can stop now
          Break();
          Delay(you can put a really long time to make sure it stops, to restart the program, use the reset button on arduino)
          }

          Hopefully this makes sense, I don’t have the IDE at the moment so couldn’t verify everything, give it some tinkering and let me know if you running into problems.

          1. Many thanks again Chen for taking your time to help.
            This is what I have now and I am still struggling with it:

            #include // Add Servo Motor Library
            #include // Add Ultrasonic Sensor Library
            // Pins
            //Define all the connections maps to the L298N
            #define buttonPin 2
            #define servoPin 3
            #define in1 4
            #define in2 5
            #define in3 6
            #define in4 7
            #define enA 10
            #define enB 11
            #define TRIG_PIN 8
            #define ECHO_PIN 9
            #define buzzPin 12
            #define ledPin 13

            // Define the button.
            const int buttonPin = 2; // Pin connected to pushbutton
            int buttonState = 0; // Give pushbutton a value
            boolean run;

            class Motor{

            int enablePin;
            int directionPin1;
            int directionPin2;

            public:

            //Method to define the motor pins
            Motor(int ENPin,int dPin1,int dPin2){
            enablePin = ENPin;
            directionPin1 = dPin1;
            directionPin2 = dPin2;
            };

            //Method to drive the motor 0~255 driving forward. -1~-255 driving backward
            void Drive(int speed){
            if(speed>=0){
            digitalWrite(directionPin1, HIGH);
            digitalWrite(directionPin2, LOW);
            }
            else{
            digitalWrite(directionPin1, LOW);
            digitalWrite(directionPin2, HIGH);
            speed = – speed;
            }
            analogWrite(enablePin, speed);
            }
            };

            Motor leftMotor = Motor(enA, in1, in2);
            Motor rightMotor = Motor(enB, in3, in4);
            Servo myservo; // create servo object to control a servo

            void motorInitiation(){
            pinMode(enA, OUTPUT);
            pinMode(in1, OUTPUT);
            pinMode(in2, OUTPUT);
            pinMode(enB, OUTPUT);
            pinMode(in3, OUTPUT);
            pinMode(in4, OUTPUT);
            // Set initial direction and speed
            digitalWrite(enA, LOW);
            digitalWrite(enB, LOW);
            digitalWrite(in1, HIGH);
            digitalWrite(in2, LOW);
            digitalWrite(in3, HIGH);
            digitalWrite(in4, LOW );
            }

            //Variables————————————————————————–
            // Anything over 400 cm (23200 us pulse) is “out of range”
            const unsigned int MAX_DIST = 23200;
            bool ObsticalAhead = false;
            int servoPos = 90;

            enum Directions { Forward, TurnLeft, TurnRight, TurnAround,Brake};

            Directions nextStep = Forward;

            unsigned long t1;
            unsigned long t2;
            unsigned long pulse_width;
            float cm;
            float inches;

            //SETUP————————————————————————–
            void setup() {

            // The Trigger pin will tell the sensor to range find
            pinMode(TRIG_PIN, OUTPUT);
            digitalWrite(TRIG_PIN, LOW);

            // We’ll use the serial monitor to view the sensor output
            Serial.begin(9600);
            pinMode(buttonPin, INPUT); //Set pushbutton pin as input
            pinMode(buzzPin, OUTPUT);
            pinMode(ledPin, OUTPUT);
            pinMode(servoPin, OUTPUT);

            myservo.attach(servoPin);
            motorInitiation();
            Directions nextStep = Forward;

            run = true;
            }

            void loop() {

            checkDistance();
            checkDirection();
            drive();

            Goforward();
            Delay(amount of the time for your distance);

            TurnRight();
            Delay(amount of the time to turn 90 degrees right)

            Goforward();
            while(rangeSensorDistance >15cm){
            //do nothing here. it will keep going
            }
            //the while() will stop when the sensor range = 15cm, so we can stop now
            Break();
            Delay(you can put a really long time to make sure it stops, to restart the program, use the reset button on arduino)
            }
            }

            void checkDistance(){

            // Hold the trigger pin high for at least 10 us
            digitalWrite(TRIG_PIN, HIGH);
            delayMicroseconds(10);
            digitalWrite(TRIG_PIN, LOW);

            // Wait for pulse on echo pin
            while ( digitalRead(ECHO_PIN) == 0 );

            // Measure how long the echo pin was held high (pulse width)
            // Note: the micros() counter will overflow after ~70 min
            t1 = micros();
            while ( digitalRead(ECHO_PIN) == 1);
            t2 = micros();
            pulse_width = t2 – t1;

            // Calculate distance in centimeters and inches. The constants
            // are found in the datasheet, and calculated from the assumed speed
            //of sound in air at sea level (~340 m/s).
            cm = pulse_width / 58.0;
            inches = pulse_width / 148.0;

            // Print out results
            if ( pulse_width > MAX_DIST ) {
            //Serial.println(“Out of range”);
            } else {
            //Serial.print(cm);
            //Serial.print(” cm \t”);
            //Serial.print(inches);
            //Serial.println(” in”);
            }

            // Wait at least 60ms before next measurement
            delay(60);

            if(cm<= 15){
            ObsticalAhead = true;
            Serial.println("Problem Ahead");

            }
            else{ ObsticalAhead = false;}

            }
            void checkDirection(){
            Serial.println("checking direction");
            if(ObsticalAhead ==true){
            nextStep = Brake;
            drive();
            myservo.write(180); // tell servo to go to position in variable 'pos'
            delay(400); // waits 15ms for the servo to reach the position
            checkDistance();
            if(ObsticalAhead ==false){//if left side is open
            nextStep = TurnLeft;
            Serial.println("Next step is TurnLeft");
            myservo.write(90);//reset servo position
            delay(400);
            }
            else{// left is blocked, now need to look at right
            myservo.write(0); // tell servo to go to position in variable 'pos'
            delay(800); // waits 15ms for the servo to reach the position
            checkDistance();
            if(ObsticalAhead ==false){//if right side is open
            nextStep = TurnRight;
            Serial.println("Next step is TurnRight");
            myservo.write(90);//reset servo position
            delay(400);
            }
            else{//right is blocked as well, need to turn around
            nextStep = TurnAround;
            myservo.write(90);//reset servo position
            delay(300);
            Serial.println("Next step is TurnAround");
            }
            }

            }
            else{nextStep = Forward;}//No obstical ahead
            }

            void drive(){
            switch (nextStep){
            case Forward:
            leftMotor.Drive(255);
            rightMotor.Drive(255);
            Serial.println("Forward");

            break;
            case TurnLeft:
            leftMotor.Drive(-255);
            rightMotor.Drive(255);
            Serial.println(" TurnLeft");
            delay(400);

            break;
            case TurnRight:
            leftMotor.Drive(255);
            rightMotor.Drive(-255);
            Serial.println(" TurnRight");
            delay(400);
            break;
            case TurnAround:
            leftMotor.Drive(255);
            rightMotor.Drive(-255);
            Serial.println(" TurnAround");
            delay(600);
            break;

            case Brake:
            leftMotor.Drive(0);
            rightMotor.Drive(0);
            Serial.println(" stopped");
            }

            }

            * I don't know what to add and what to eliminate. I am still working on the Servo.

  12. Thank you, Chen. Excellent tutorial. I’m struggling for a few days to correctly set up the connections and your explanations have been of great help to me. (I’m trying to teach a team of kids about making a Robot Car, with 2wd, servomotor and ultrasonic sensor)
    I still have one question. I use a source ( for both L298N and Arduino) of two 18650 rechargeables, so a total of 7.5V. It is still OK to use that “bridge” connection between +12V and +5V pins?

    1. Hi RodicaA,

      You shouldn’t use the bridge when using 7.5v. Just connect the 7.5v to 12v pin and ground, then you are good to go. For detailed explanation, please watch it here.

  13. Thanks. I checked that, it’s working, but I think I can not control the speed of the motors. If I tried from program to send different values of speed (for example 250 for motor A and 135 for B), but it seems both of them are moving at the same speed). Also any speed below 130 is considered as 0 (not moving). Could you help me, what could be the explanation for this?

    I am using the same pins as you, and the code is not exactly, but similar. I have one connection from the source (7.5V) to L298 (12v pin), one connection from the source Gnd to L298 Gnd and the same to the Arduino Gnd, and one connection from L298D pin 5v to the Arduino Vcc in pin.
    (Maybe here is the problem, but in other combination, with the power source directly to Arduino Vcc and the same L289 12v pin, the motors didn’t move) If i put the jumpers on the EnA and EnB pins it’s working, but this is, as I understood, without the possibility of speed adjustments.

    1. Hi Rodicaa, you reminded me that I have the same problem. Indeed the speed is not changed by different PWMs. I’ll try to do some test this week and get back to you.

    2. Hi Rodicaa,

      I figured out after some tests, turns out the pin 13 is not a PWM pin, thus it couldn’t change the speed. You can just swap the pin 11 and pin 13 connections, then update the code in the define part, it should work. Sorry I made this mistake in my tutorial…

      Let me know if this works for you.

      Thanks!

  14. Hi Chen, I am using the following code, I am having trouble, but if you see ” disabled ” and then move back to the left and then left and then left and then left, ” how can I write this code? Can you send mail to me?

    Shipping list:
    1 X Car chassis
    2 X Car Wheels
    DC Gear Motor 1 48: a deceleration DC motor (otherwise 120: 1 Optional)
    2 X 20 line gun code disk
    4 X Fasteners (high intensity black acrylic)
    1 X Caster
    1 X Four battery box
    1 X Quality Rocker Switch
    1 X Assembly drawing
    Several screw nut
    1 X V5 shield
    1 X UNO R3
    1 X SG90
    1 X FPV
    1 X L298Nf

    #include

    // Pins
    #define TRIG_PIN A0
    #define ECHO_PIN A1

    //Define all the connections maps to the L298N
    #define enA 13
    #define in1 12
    #define in2 11
    #define in3 7
    #define in4 6
    #define enB 5
    #define servoPin 2

    class Motor{

    int enablePin;
    int directionPin1;
    int directionPin2;

    public:

    //Method to define the motor pins
    Motor(int ENPin,int dPin1,int dPin2){
    enablePin = ENPin;
    directionPin1 = dPin1;
    directionPin2 = dPin2;
    };

    //Method to drive the motor 0~255 driving forward. -1~-255 driving backward
    void Drive(int speed){
    if(speed>=0){
    digitalWrite(directionPin1, LOW);
    digitalWrite(directionPin2, HIGH);
    }
    else{
    digitalWrite(directionPin1, HIGH);
    digitalWrite(directionPin2, LOW);
    speed = – speed;
    }
    analogWrite(enablePin, speed);
    }
    };

    Motor leftMotor = Motor(enA, in1, in2);
    Motor rightMotor = Motor(enB, in3, in4);
    Servo myservo; // create servo object to control a servo

    void motorInitiation(){
    pinMode(enA, OUTPUT);
    pinMode(in1, OUTPUT);
    pinMode(in2, OUTPUT);
    pinMode(enB, OUTPUT);
    pinMode(in3, OUTPUT);
    pinMode(in4, OUTPUT);
    // Set initial direction and speed
    digitalWrite(enA, LOW);
    digitalWrite(enB, LOW);
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, LOW);
    digitalWrite(in4, HIGH);
    }

    //Variables————————————————————————–
    // Anything over 400 cm (23200 us pulse) is “out of range”
    const unsigned int MAX_DIST = 23200;
    bool ObsticalAhead = false;
    int servoPos = 90;

    enum Directions { Forward, TurnLeft, TurnRight, TurnAround, Brake};

    Directions nextStep = Forward;

    unsigned long t1;
    unsigned long t2;
    unsigned long pulse_width;
    float cm;
    float inches;

    //SETUP————————————————————————–
    void setup() {

    // The Trigger pin will tell the sensor to range find
    pinMode(TRIG_PIN, OUTPUT);
    digitalWrite(TRIG_PIN, LOW);

    // We’ll use the serial monitor to view the sensor output
    Serial.begin(9600);
    myservo.attach(servoPin);
    motorInitiation();
    Directions nextStep = Forward;
    }

    void loop() {

    checkDistance();
    checkDirection();
    drive();
    }

    void checkDistance(){

    // Hold the trigger pin high for at least 10 us
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);

    // Wait for pulse on echo pin
    while ( digitalRead(ECHO_PIN) == 0 );

    // Measure how long the echo pin was held high (pulse width)
    // Note: the micros() counter will overflow after ~70 min
    t1 = micros();
    while ( digitalRead(ECHO_PIN) == 1);
    t2 = micros();
    pulse_width = t2 – t1;

    // Calculate distance in centimeters and inches. The constants
    // are found in the datasheet, and calculated from the assumed speed
    //of sound in air at sea level (~340 m/s).
    cm = pulse_width / 100.0;
    inches = pulse_width / 39.0;

    // Print out results
    if ( pulse_width > MAX_DIST ) {
    //Serial.println(“Out of range”);
    } else {
    //Serial.print(cm);
    //Serial.print(” cm \t”);
    //Serial.print(inches);
    //Serial.println(” in”);
    }

    // Wait at least 60ms before next measurement
    delay(60);

    if(cm<= 20){
    ObsticalAhead = true;
    Serial.println("Problem Ahead");

    }
    else{ ObsticalAhead = false;}

    }
    void checkDirection(){
    Serial.println("checking direction");
    if(ObsticalAhead ==true){
    nextStep = Brake;
    drive();
    myservo.write(180); // tell servo to go to position in variable 'pos'
    delay(400); // waits 15ms for the servo to reach the position
    checkDistance();
    if(ObsticalAhead ==false){//if left side is open
    nextStep = TurnLeft;
    Serial.println("Next step is TurnLeft");
    myservo.write(90);//reset servo position
    delay(400);
    }
    else{// left is blocked, now need to look at right
    myservo.write(0); // tell servo to go to position in variable 'pos'
    delay(400); // waits 15ms for the servo to reach the position
    checkDistance();
    if(ObsticalAhead ==false){//if right side is open
    nextStep = TurnRight;
    Serial.println("Next step is TurnRight");
    myservo.write(90);//reset servo position
    delay(400);
    }
    else{//right is blocked as well, need to turn around
    nextStep = TurnAround;
    myservo.write(90);//reset servo position
    delay(300);
    Serial.println("Next step is TurnAround");
    }
    }

    }
    else{nextStep = Forward;}//No obstical ahead
    }

    void drive(){
    switch (nextStep){
    case Forward:
    leftMotor.Drive(255);
    rightMotor.Drive(255);
    Serial.println("Forward");

    break;
    case TurnLeft:
    leftMotor.Drive(-255);
    rightMotor.Drive(255);
    Serial.println(" TurnLeft");
    delay(250);

    break;
    case TurnRight:
    leftMotor.Drive(255);
    rightMotor.Drive(-255);
    Serial.println(" TurnRight");
    delay(250);

    break;
    case TurnAround:
    leftMotor.Drive(255);
    rightMotor.Drive(-255);
    Serial.println(" TurnAround");
    delay(400);
    break;

    case Brake:
    leftMotor.Drive(-1);
    rightMotor.Drive(-1);
    Serial.println(" stopped");

    }

    }

  15. Hi Murat,

    Do you mean “Disabled” is when the robot see some obstacles ahead? If so, and if the “move back to the left and then left and then left and then left” is the only action you want it to trigger, then I can help.

    Since there is a “Move Back”, we’ll have to define that action first by adding “Back” in to
    enum Directions { Forward, TurnLeft, TurnRight, TurnAround, Brake, Back};

    Then define the what the “Back” does by adding it to the switch:

    switch (nextStep){

    case Back:
    leftMotor.Drive(-255);
    rightMotor.Drive(-255);
    Serial.println(” Back”);
    delay(400);
    break;

    }

    And you can update the loop code as below:
    void loop() {

    checkDistance();
    if(ObsticalAhead ==true){
    nextStep = Back;
    drive();
    nextStep = TurnLeft;
    drive();
    nextStep = TurnLeft;
    drive();
    nextStep = TurnLeft;
    drive();
    nextStep = TurnLeft;
    drive();
    nextStep = Forward;
    drive();
    }
    }

    Let me know if you it works or not.

    Chen

  16. Hi, first of all thank you for the robot car! Mine works a little different then yours, for some reason my robot car doesn’t stop when it sees an object, it scans the area but doesn’t stop driving do you have na fix for this?

    1. Hi Coen, sounds like the motor connections might be wrong, is it matching the code like this?
      enA to 13
      in1 to 12
      in2 to 11
      in3 to 7
      in4 to 6
      enB to 5

  17. Hello

    I write you for problems
    I have only a right wheel which turns
    The servo does not move (we hear it to initialize in the starting up).
    2 engines work and the servo also.
    I have well to verify the electric contacts.
    For the red printed circuit, I have well to kidnap 2 riders.

    The return of the serial.
    I left my hand in front of the detector ultrasound, the right wheel on the photo is always underway back as long as I have the hand in front of then forward when I remove her(it)
    I have the return of information of rotation of the servo but it does not move

    1. Hi Patrick,

      Have you checked all the components piece by piece as in the part 2 video? Let me know if everything works just by themselves, if so, there might be other connection problems.

  18. helo
    the 2 tests are ok
    ultrason write onserial
    motor: the 2 Wheel turn
    servo : move
    Can you send me reply on my adress and so i send you photo

    1. It is good. Your code works.

      But you can show the good plan.

      Pine of the servo on the pine 2
      Pine 8 9 1 0 on pine 11 1 2 and 13.Realy good job
      Thank you

Leave a Reply

Your email address will not be published. Required fields are marked *