Rover 5 autonomous mode part 2

Dagu Rover 5 in autonomous mode from Bajdi on Vimeo.

Since my Rover 5 has 4 encoders I wanted to do something with these encoders. There is an encoder library for Arduino which makes it very easy to read the encoders. I first tried the example sketches that come with the library to check if my encoders where working. So I thought of making a sketch to let the Rover drive autonomous and when it detects an object it does a 90° turn. The 90° turn equals xx number of pulses from the encoders. Sounds more simple then it is, I got some help from the friendly people at letsmakerobots.com. I made 2 sketches, one where the Rover always does a 90° turn to the left after it has detected an object and one where I use a servo to turn the ultrasonic sensor left/right and then decide in which direction the Rover should turn. This last sketch does not work very well. I use the delay function after sending the command to the servo. During this delay the reading from the encoders stops. Another thing to take into account is that the tracks of the Rover slip depending on the surface. So it’s not really possible to determine how many pulses you exactly need for a 90° turn.

I’ve added some leds to my Rover 5, 6 white 10mm leds on the front and 4 3mm orange leds on the corners. The white leds get their power through a NPN transistor that is controlled by the Red back spider controller. The orange leds are connected to a NE555 chip, the supply of that chip is also controlled from a NPN transistor connected to the Red back spider controller.

This is the sketch where the Rover will turn 90° to the left when an object is detected:

// http://www.bajdi.com
// Dagu Rover 5 chassis with 4 motors
// Autonomous mode using an SRF06 ultrasonic sensor
// Using one encoder to do 90° turns when an object is detected (80 pulses = 90°)
// µ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 <Encoder.h>

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

const int  TRIG = 36;
const int  ECHO = 37;

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 orange = 38; // orange leds
const int headlight = 39; // white leds
const int leftled = 40; // red led

Ultrasonic SRF6A(TRIG, ECHO);

Encoder encl(2, 3);

const int ATTENTION_DISTANCE = 25;
const int NINETY_DEGREES = 80;

int currentDegrees = 0;
int lastPulse = 0;
int left;
int right;

void setup() {
  pinMode(DirM1, OUTPUT);  // direction motor 1, right front
  pinMode(DirM2, OUTPUT);  // direction motor 2, right rear
  pinMode(DirM3, OUTPUT);  // direction motor 3, left front
  pinMode(DirM4, OUTPUT);  // direction motor 4, left 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
  pinMode(orange, OUTPUT); // orange leds
  pinMode(headlight, OUTPUT); // white leds
  pinMode(leftled, OUTPUT);   // red led
  delay(200);
  forward(); 
}

void loop() {

  digitalWrite(headlight, HIGH);

  bat = analogRead(A0);

  if (bat > 765){          // 765 / 1023 * 5 * 2 = 7,4V from battery  

    if(SRF6A.Ranging(CM) < 10){
      digitalWrite(buzzer, HIGH);      // we're going to crash 
    }
    else {
      digitalWrite(buzzer, LOW); 
    } 

    if(isAttention()){
      turnleft();
    }
    if(isTurnFinished()){
      resetDegreeMeasurement();
      forward();
    }
    else{
      measureDegrees();
    }
  }
  else {
    stopped();
    digitalWrite(lowbat, HIGH);
  }
}

boolean isAttention(){
  int distance = SRF6A.Ranging(CM);

  if(distance > ATTENTION_DISTANCE){
    return false;
  }
  return true;
}

void measureDegrees(){

  int pulse = readPulse();
  if(pulse == lastPulse){

    return;
  }
  lastPulse = pulse;
  currentDegrees++;  
}

int readPulse(){
  return encl.read();
}

boolean isTurnFinished(){ 
  return (currentDegrees == NINETY_DEGREES);
}

void resetDegreeMeasurement(){
  currentDegrees = 0;
}

void forward()
{
  digitalWrite(DirM1, LOW);
  digitalWrite(DirM2, LOW);
  digitalWrite(DirM3, LOW);
  digitalWrite(DirM4, LOW);
  analogWrite(PWMM1, 65);
  analogWrite(PWMM2, 65);
  analogWrite(PWMM3, 65);
  analogWrite(PWMM4, 65);
  digitalWrite(orange, LOW);
  digitalWrite(leftled, LOW);
}

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

void stopped()
{
  analogWrite(PWMM1, 0);
  analogWrite(PWMM2, 0);
  analogWrite(PWMM3, 0);
  analogWrite(PWMM4, 0);
  digitalWrite(orange, LOW);
  digitalWrite(leftled, LOW);
}