Adding and Checking Ultrasonic Sensor and Servo Motor: Arduino RoboMaster Part 10

In last few days I was busy with planning, adding and testing of ultrasonic sensor and servo motor. Both of these part is for self driving obstacle avoidance robotic car. Since I had to relearn how to add motors and sensors and control them I choose to add these two sensors. This self driving robotic car uses the servo to move the ultrasonic sensor. When the robot car senses wall or obstructive object, it will stop around, look left and right and go to the direction where there is no obstruction. Using this build exercise will allow me to plan for the movement for the crane body and the arms. Also I have to plan for the bluetooth control vs the automatic control, that is the programming aspect.

After I screwed the tank part of the Arduino RoboMaster in my last blog post, I now had to plan how to fix the servo motor on the car chassis and how to fix the HC-05 ultrasonic sensor onto the servo motor. Below is video showing part of the work of planning and fixing of the ultrasonic sensor and servo motor

After that, I started by searching for an L shaped plastic material. I finally found something from a wall socket box. I used soldering iron and cut to an L shaped object.

I cut off a rectangle part of the chassis to fit the servo motor using a soldering iron(I know this is not professional. I have fan turned on to blow the toxic plastic cloud. After that I fixed the

As you can see, the motor is fixed with the longer side along the car but later I changed to the longer side of the servo motor along the front car wheels. The reason is that the motor arm movement is along its longer part from 0 to 180. I should have tested the arm movement first and then cut off the part of the chassis. The circuit diagram below shows the connection of Arduino L293D motor shield to the servo motor and to the HC-05 ultrasonic sensor module.

circuit diagram obstacle avoiding robot car

It should be noted that here I have not shown connection to the Bluetooth module because it is not required here in this work. For servo motor control see, servo motor control with arduino motor shield.

I fixed the L shaped part and the glued the ultrasonic sensor on the L shaped using super glue and bit using soldering iron.

Finally I completed installing the servo motor and the ultrasonic sensor onto the diy robotic car.

At this point I thought it would work perfectly fine. I used the following arduino program code for the obstacle avoiding arduino based robotic car.

#include <AFMotor.h>  
#include <NewPing.h>
#include <Servo.h> 

#define TRIG_PIN A1 
#define ECHO_PIN A2
#define MAX_DISTANCE 200 
#define MAX_SPEED 190 // sets speed of DC  motors
#define MAX_SPEED_OFFSET 20

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); 

AF_DCMotor motor1(1, MOTOR12_1KHZ); 
AF_DCMotor motor2(2, MOTOR12_1KHZ);
AF_DCMotor motor3(3, MOTOR34_1KHZ);
AF_DCMotor motor4(4, MOTOR34_1KHZ);
Servo myservo;   

boolean goesForward=false;
int distance = 100;
int speedSet = 0;

void setup() {
 Serial.begin(9600);
  myservo.attach(10);  
  myservo.write(50); //115
  delay(2000);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);

  myservo.write(78);
  delay(500);
  myservo.write(55);
  delay(500);
  myservo.write(130); // Move servo arm 180 degrees
  delay(500);
  Serial.print(distance);
   Serial.println(" cm");
}

void loop() {
 int distanceR = 0;
 int distanceL = 0;
 delay(40);
 Serial.print(distance);
   Serial.println(" cm");
 
 if(distance<=15)
 {
  moveStop();
  delay(100);
  moveBackward();
  delay(300);
  moveStop();
  delay(200);
  distanceR = lookRight();
  delay(200);
  distanceL = lookLeft();
  delay(200);

  if(distanceR>=distanceL)
  {
    turnRight();
    moveStop();
  }else
  {
    turnLeft();
    moveStop();
  }
 }else
 {
  moveForward();
 }
 distance = readPing();
}

int lookRight()
{
    myservo.write(146); //70
    delay(500);
    int distance = readPing();
    delay(100);
    myservo.write(100); //150
    return distance;
}

int lookLeft()
{
    myservo.write(100); //150
    delay(500);
    int distance = readPing();
    delay(100);
    myservo.write(55); 
    return distance;
    delay(100);
}

int readPing() { 
  delay(70);
  int cm = sonar.ping_cm();
  if(cm==0)
  {
    cm = 250;
  }
  return cm;
}

void moveStop() {
  motor1.run(RELEASE); 
  motor2.run(RELEASE);
  motor3.run(RELEASE);
  motor4.run(RELEASE);
  } 
  
void moveForward() {

 if(!goesForward)
  {
    goesForward=true;
    motor1.run(FORWARD);      
    motor2.run(FORWARD);
    motor3.run(FORWARD); 
    motor4.run(FORWARD);     
   for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
   {
    motor1.setSpeed(speedSet);
    motor2.setSpeed(speedSet);
    motor3.setSpeed(speedSet);
    motor4.setSpeed(speedSet);
    delay(5);
   }
  }
}

void moveBackward() {
    goesForward=false;
    motor1.run(BACKWARD);      
    motor2.run(BACKWARD);
    motor3.run(BACKWARD);
    motor4.run(BACKWARD);  
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
  {
    motor1.setSpeed(speedSet);
    motor2.setSpeed(speedSet);
    motor3.setSpeed(speedSet);
    motor4.setSpeed(speedSet);
    delay(5);
  }
}  

void turnRight() {
  motor1.run(FORWARD);
  motor2.run(FORWARD);
  motor3.run(BACKWARD);
  motor4.run(BACKWARD);     
  delay(500);
  motor1.run(FORWARD);      
  motor2.run(FORWARD);
  motor3.run(FORWARD);
  motor4.run(FORWARD);      
} 
 
void turnLeft() {
  motor1.run(BACKWARD);     
  motor2.run(BACKWARD);  
  motor3.run(FORWARD);
  motor4.run(FORWARD);   
  delay(500);
  motor1.run(FORWARD);     
  motor2.run(FORWARD);
  motor3.run(FORWARD);
  motor4.run(FORWARD);
}

Now when I tested it for the first time there were couples of problems. First the servo arm was moving in range that I did not expect. I had to cut out part of the chassis to fit the servo motor body in order to move it in the right direction. I also had to create space so that the arm with the ultrasonic sensor does not collide with the part of the chassis and the DC motor of the front wheels. The following shows the new adjusted and fixed servo motor and ultrasonic sensor.

As you can see I made a big hole to allow the movement of the servo arm and the ultrasonic sensor module.

After that I checked the servo motor arm movement and realized that I had to change the servo arm movement angle. In the above program code I have adjusted the angle movement code so that it did not hit or touch the parts of the chassis. The servo problem was easy fix but the ultrasonic sensor was not working as expected.

It took me 2 full days and still could not find out the problem with the ultrasonic sensor. I had to do couple of new soldering between the motor driver and the ultrasonic sensor. I had to add serial print out to look whether I am getting correct distance reading. Since I did not get correct distance reading the motors wheels were not moving as it should be. So to trace the problem, I used a simple ultrasonic sensor object detection arduino code which you can find below to check the whether the ultrasonic sensor module. I had to take out the ultrasonic sensor and use a breadboard, Arduino Nano and LEDs to check the ultrasonic sensor.

The program used is below and explained in the blog post measure distance with HC-SR04 and Arduino.


//simple program to calculate distance with HC-SR04

#define echoPin 11 // Echo pin connected to Arduino 11
#define trigPin 12 // Trig pin connected to Arduino 12

void setup() {
   Serial.begin (115200);
   pinMode(echoPin, INPUT);
   pinMode(trigPin, OUTPUT);
}

void loop() {
   //define some constants
   int duration, distance;
   
   //send trigger signal
   digitalWrite(trigPin, LOW);
   delayMicroseconds(2);
   digitalWrite(trigPin, HIGH);
   delayMicroseconds(10);
   digitalWrite(trigPin, LOW);

   //calculate distance
   duration = pulseIn(echoPin, HIGH);
   distance = (duration / 2) / 29.1;

   //print it
   Serial.print("distance:");
   Serial.print(distance);
   Serial.println(" cm");

   //add some delay
   delay(500);
}

Unsurprisingly, I was not getting correct distance reading again. I realized that all my work on the ultrasonic sensor could have been avoided if I had checked the ultrasonic sensor first with simple program.

Anyway, since I don't have a spare ultrasonic part, I will have to buy and then try again. So next time I think I will be able to complete the self driving robot or obstacle avoiding robot. After that I have plan to add a stepper motor for the movement of the upper part of the diy Arduino robomaster. Then I think I need to add three more servo for the crane arm movement. Lets see how it goes.

For other part of this arduino robotic car follow the reference links.

References

# Building RoboMaster with Arduino - Part 2

# Quick HC-05 AT startup guide - Arduino RoboMaster Part 3

# Testing RC Bluetooth Car App and Motor Code -Arduino RoboMaster Part 4

# Lithium Ion Battery and Charger - Arduino RoboMaster Part 5

# My First Arduino RC Car - Arduino RoboMaster part 6

# Android Studio for Wireless Robotic controller -Arduino RoboMaster Part 8

# Arduino RoboMaster Modifications with Crane Part - Arduino RoboMaster Part 9