Rover 5 autonomous mode

Rover 5 in autonomous mode from Bajdi on Vimeo.

After writing a sketch to remote control the Rover 5 it was time to try and write a sketch to let it drive around the house autonomously. To keep things simple I used only one ultrasonic sensor for obstacle detection. My SRF06 ultrasonic module is mounted on a small micro 9G servo. I used the varspeedservo library to sweep the servo from left to right. My biggest problem was reading the distance from the ultrasonic sensor at a certain servo position (Servo.read() == xxx). When I sweeped the servo to fast I wouldn’t get any reading because the servo overshoots some positions. That took me some time to figure out. I tried lots of different things but in the end I just read the distance between two servo positions. This gave me the best results. My next step is to make use of the encoders that are fitted to the wheels. I want to make a sketch that when an obstacle is detected the Rover turns 90°. The 90° would be X number of pulses from the encoder.

I did have some trouble with my Rover 5 during the many test sessions. Suddenly one motor stopped spinning. I was already thinking the worst, maybe I had blown something up? After taking everything apart I found that one of the wires going from the noise suppression coil in the bottom of the chassis to the motor connector had come loose. I re soldered the wire and all was fine again. Then a wheel and axle came of the rover, the axle is only fixed by a plastic gear. I refitted the wheel but the axle has a lot of play. That seems to be the reason that the track keeps coming of.
Here is my first Rover 5 autonomous sketch, keep in mind that I’m new to programming and robots so the code is probably a very bad example. Comments on how to improve it are very welcome 🙂

// http://www.bajdi.com
// Dagu Rover 5 chassis with 4 motors
// Autonomous mode using an SRF06 ultrasonic sensor mounted on a 9G servo on the front
// µcontroller = Dagu Red back spider, Arduino Mega 1280 compatible
// Motor controller = Dagu 4 channel motor controller
// Motor 1 and 2 on the left
// Motor 3 and 4 on the right

#include "Ultrasonic.h"
#include <VarSpeedServo.h>

int DirM1 = 30; // direction motor 1
int DirM2 = 31; // direction motor 2
int DirM3 = 32; // direction motor 3
int DirM4 = 33; // direction motor 4
int PWMM1 = 3;  // pwm motor 1
int PWMM2 = 4;  // pwm motor 2
int PWMM3 = 5;  // pwm motor 3
int PWMM4 = 6;  // pwm motor 4

int bat; // analog reading of battery voltage through voltage divider
const int lowbat = 34; // low battery led (red)
const int buzzer = 35; // buzzer

const int  TRIG = 36;
const int  ECHO = 37;
Ultrasonic SRF6A(TRIG, ECHO);

int val1;
int val2;
int val3;
int val4;
int val5;
int valavg;

VarSpeedServo SRF6AServo;        // servo objects

int servoSpeeds = 25; // sweep speed, 1 is slowest, 255 fastest)
int servoMinPosition = 25; // the minumum servo angle
int servoMaxPosition = 105; // the maximum servo angle

void setup() {

  // Serial.begin(9600);

  pinMode(DirM1, OUTPUT);  // direction motor 1, left front
  pinMode(DirM2, OUTPUT);  // direction motor 2, left rear
  pinMode(DirM3, OUTPUT);  // direction motor 3, right front
  pinMode(DirM4, OUTPUT);  // direction motor 4, right rear
  pinMode(PWMM1, OUTPUT);  // PWM motor 1
  pinMode(PWMM2, OUTPUT);  // PWM motor 2
  pinMode(PWMM3, OUTPUT);  // PWM motor 3
  pinMode(PWMM4, OUTPUT);  // PWM motor 4
  pinMode(lowbat, OUTPUT); // low battery led (red)
  pinMode(buzzer, OUTPUT); // buzzer

  SRF6AServo.attach(7);       
  SRF6AServo.slowmove(servoMinPosition,servoSpeeds) ; // start sweeping from min position

}

void loop() {

  bat = analogRead(A1);

  if (bat <= 730)                 // 730 equals 7V (2 cell Lipo)
  {
    digitalWrite(lowbat, HIGH);   // battery low
  }


  if ( SRF6AServo.read() == servoMinPosition) 
  {
    SRF6AServo.slowmove(servoMaxPosition,servoSpeeds); 
  }
  if ( SRF6AServo.read() >= 25 && SRF6AServo.read() < 30)
  {
    val1 = SRF6A.Ranging(CM);
  }

  if ( SRF6AServo.read() > 52 && SRF6AServo.read() < 57) 
  {
    val2 = SRF6A.Ranging(CM);
  }

  if ( SRF6AServo.read() > 62 && SRF6AServo.read() < 67) 
  {
    val3 = SRF6A.Ranging(CM);
  }

  if ( SRF6AServo.read() > 72 && SRF6AServo.read() < 77) 
  {
    val4 = SRF6A.Ranging(CM);
  }

  if ( SRF6AServo.read() > 100 && SRF6AServo.read() < 105)
  {
    val5 = SRF6A.Ranging(CM);
  }
  if( SRF6AServo.read() == servoMaxPosition)
  {
    SRF6AServo.slowmove(servoMinPosition,servoSpeeds); 
  }

  valavg = ((val2 + val3 + val4) /3);

  /*
  Serial.print("val1 = ");
   Serial.println(val1);
   Serial.print("val2 = ");
   Serial.println(val2);
   Serial.print("val3 = ");
   Serial.println(val3);
   Serial.print("val4 = ");
   Serial.println(val4);
   Serial.print("val5 = ");
   Serial.println(val5);
   */

  if (val2 < 25 || val3 < 25 || val4 < 25 || valavg < 25)
  {

    if (val1 < val5)
    {
      turnright();
    }
    if (val5 < val1)
    {
      turnleft();
    }
  }
  else
  {
    forward();
  }

  if (valavg < 10)
  {
    digitalWrite(buzzer, HIGH);     // we're going to crash :)
  }
  else
  {
    digitalWrite(buzzer, LOW);
  }
}

void turnleft()
{
  digitalWrite(DirM1, LOW);
  digitalWrite(DirM2, LOW);
  digitalWrite(DirM3, HIGH);
  digitalWrite(DirM4, HIGH);
  analogWrite(PWMM1, 150);
  analogWrite(PWMM2, 150);
  analogWrite(PWMM3, 150);
  analogWrite(PWMM4, 150);
}

void turnright()
{
  digitalWrite(DirM1, HIGH);
  digitalWrite(DirM2, HIGH);
  digitalWrite(DirM3, LOW);
  digitalWrite(DirM4, LOW);
  analogWrite(PWMM1, 150);
  analogWrite(PWMM2, 150);
  analogWrite(PWMM3, 150);
  analogWrite(PWMM4, 150); 
}

void forward()
{
  digitalWrite(DirM1, HIGH);
  digitalWrite(DirM2, HIGH);
  digitalWrite(DirM3, HIGH);
  digitalWrite(DirM4, HIGH);
  analogWrite(PWMM1, 70);
  analogWrite(PWMM2, 70);
  analogWrite(PWMM3, 70);
  analogWrite(PWMM4, 70);
}