IMG_20150821_123844939

I had first used this Turtle 2WD robot platform (DFRobot) with a remote control (nRF24L01) and ITEADstudio’s MBoard, an inexpensive Arduino style motor board. My new plan was to make this robot autonomous, with 2 ultrasonic range sensors to enable it to avoid obstacles. Watch the video!

It was rather easy to get it working and the sketch is quite simple, i will describe it below but first elaborate on the hardware.

HARDWARE

The MBoard is a Leonardo style Arduino board, and next to the 2 motor drivers includes headers for the nRF24L01, SDcard, XBee, and Electronic Bricks. Those functions take up all the available I/O pins so you need to figure out from the datasheet how to access these pins in the above headers. I powered the board from a 6V3Ah sealed lead acid battery, which gives plenty of power for the motors.

Each ultrasonic range finder (type HC-SR04) needs 2 pins (trigger, echo); i used D0 and D1 availabe on the UART header, and D2 and D3 available on the IIC header. These sensors need 5V; unfortunately the MBoard has only 3V3 available on the headers, so i had to solder a wire to the bottom of the board to a 5V point. I connected this 5V to a breadboard to share it.

I added 2 red LEDs on the breadboard to indicate when each ultrasonic sensor observed an obstacles closer than 10cm, and a buzzer to beep before moving backwards. For these digital outputs i used the EB headers, which are connected the the analog input pins A0-A6 but can also be configured for digital output.

I left the nRF24L01 module plugged in but did not use it for this project.

Later on i moved the sensors to a lower position, in between the top and baseboard of the robot.

IMG_20150821_123903118

SOFTWARE

The sketch is quite straight forward: every loop starts with 2 ultrasonic measurements (implemented as a function with the trigger and echo pins as parameters) returning distance1 and distance2. I set a timeout for the pulseIn at 6000 microseconds, corresponding to about 100cm; we don’t need to measure beyond that.

If both distances are larger than 50cm, go straight; if any obstacle closer than 50cm, turn away to the other side.

If any of the distances is smaller than 10cm, switch on the respective LED (just for a visual warning).

Continue this movement for 100 milliseconds, then go back to the start of the loop.

If both are smaller than 10cm, stop the motors and reverse for a short time. To avoid getting into an infinite loop, i added a random element into the reverse movement: either reverse to left, right, or straight.

This worked quite well even in a narrow maze set up by my children as in above and below video (double speed):

Below is my Arduino sketch, the same as used in the video:

/* itead mboard and turtle 2WD robot platform
   Arduino Leonardo
   Tom Tobback August 2015
   pins:
   EB0    A0
   EB1    A1
   EB2    A2
   EB3    A3
   EB4    A4
   EB5    A5
   D0     RX uart 
   D1     TX uart
   D2     SDA iic
   D3     SCL iic
   D4     SDcard (no pin)
   D5     nRF
   D6     nRF
   D7     motor IN1
   D8     motor IN2
   D9     nRF
   D10    motor ENA
   D11    motor ENB
   D12    motor IN3
   D13    motor IN4
   D14    nRF
   D15    nRF
   D16    nRF

*/
int in1 = 7;
int in2 = 8;
int in3 = 12;
int in4 = 13;
int ena = 10;
int enb = 11;

int trig1 = 3;
int echo1 = 2;
int trig2 = 0;
int echo2 = 1;
int buzzer = A0;
int led1 = A1;
int led2 = A2;

int left_pwm, right_pwm;
int max_speed = 200;          // out of 0-255
int distance1, distance2;

void setup() {
  
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(ena, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(enb, OUTPUT);
  pinMode(trig1, OUTPUT);
  pinMode(echo1, INPUT);
  pinMode(trig2, OUTPUT);
  pinMode(echo2, INPUT);
  pinMode(buzzer, OUTPUT);
  pinMode(led1, OUTPUT);
  pinMode(led2, OUTPUT);

  left_pwm = max_speed;
  right_pwm = max_speed;
  motorstop();
  
  Serial.begin(9600);
  Serial.println("autonomous robot v2 with 2 eyes");
  for (int i = 0; i<3; i++) {
    digitalWrite(buzzer, HIGH);
    digitalWrite(led1, HIGH);
    digitalWrite(led2, HIGH);
    delay(100);
    digitalWrite(buzzer, LOW);
    digitalWrite(led1, LOW);
    digitalWrite(led2, LOW);
    delay(100);
  }
}

void loop()  {
  
  distance1 = ultrasonic(trig1, echo1);
  Serial.print("distance1,2 (cm): \t ");
  Serial.print(distance1);
  Serial.print("\t");
  distance2 = ultrasonic(trig2, echo2);
  Serial.print(distance2);
  Serial.print("\t");
  
  if (distance1 < 10) {
    digitalWrite(led1, HIGH);
  } else {
    digitalWrite(led1, LOW);
  }
  if (distance2 < 10) {
    digitalWrite(led2, HIGH);
  } else {
    digitalWrite(led2, LOW);
  }
  
  if (distance1 < 10 && distance2 < 10) {      // if both sides closer than 10cm, stop and go back
    motorstop();
    Serial.println("Stop");
    goback(500);                               // random left, right, straight
  }
  else
  { 
    if (distance1 > 50 && distance2 > 50) {    // nothing to see within 50cm, full speed forward
      left_pwm = max_speed;
      right_pwm = max_speed;
    Serial.println("Forward");
    }
    else 
    {
      if (distance1 > distance2) {              // if anything closer than 50cm, turn away from the closest
        left_pwm = 0;
        right_pwm = max_speed;
        Serial.println("Left");
      }
      else
      {
        left_pwm = max_speed;
        right_pwm = 0;
        Serial.println("Right");
      }        
    }
 
  move(left_pwm, right_pwm);
     
  }
    
  delay(100);
}

void motorstop() {     // free running motor stop
  
  analogWrite(ena, 0);
  analogWrite(enb, 0);
  digitalWrite(buzzer, HIGH);
  delay(300);
  digitalWrite(buzzer, LOW);

}

void move(int left, int right) {
  
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  analogWrite(ena, left);
  
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  analogWrite(enb, right);
  
}

void goback(int t) {            // move back for time t, random direction
  
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  analogWrite(ena, max_speed * random(0,2));  // random 0 or 1
  
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  analogWrite(enb, max_speed * random(0,2));  // random 0 or 1        
  
  delay(t);
  
}


int ultrasonic(int trig, int echo) {
 long duration;
 int distance;
 digitalWrite(trig, LOW);                // trigger OFF
 delayMicroseconds(2);                   
 digitalWrite(trig, HIGH);               // trigger ON
 delayMicroseconds(10); 
 digitalWrite(trig, LOW);                // trigger OFF
 duration = pulseIn(echo, HIGH, 6000);   // timeout of 6000us means we do not measure beyond 1 meter, will return 0
 
 //Calculate the distance (in cm) based on the speed of sound (340.29 m/s)
 distance = (int) (duration * 340.29L / 10000 /2);
 if (distance == 0) distance = 100;      // timeout, distance beyond 1 meter
 return distance;
}

Leave a Reply

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