Obstacle avoidance robot - advice

Using Arduino Project Guidance
#1

Hi all, I’m new to Arduino and the forums.

My first project is a basic obstacle avoidance robot. I use an Uno with a motor shield, driving 2 DC motors that power the wheels. I also use a US sensor mounted on a servo to get distances (left, right and centre).

My first iteration, the robot did things in sequence, i.e. move forward, if an obstacle is too close, then scan left and right, turn, then go again. It worked all fine.

I wanted to make it scan and move at the same time. so I modified my code based on this one. It all works nicely, the robot scans at the same time that it moves, so it knows where to turn as soon as it has to. Sweet!

Here is the code I’m currently using.

#include <Servo.h>

#define runEvery(t) for (static typeof(t) _lasttime;(typeof(t))((typeof(t))millis() - _lasttime) > (t);_lasttime += (t))

Servo RadarServo;

// from 9V to 6V
const float MotorReduction = 0.66;

const int
PWM_L = 3, // motor 1 pins
DIR_L = 12,
BRA_L = 9,
CUR_L = A0,

PWM_R = 11, // motor 2 pins
DIR_R = 13,
BRA_R = 8,
CUR_R = A1,

USPin = A2, // ultrasonic sensor pin



TurnInterval = 500; // time spent turning / moving backwards

int MotorSpeed;
int rawDist;
int USpos = 82; // servo position to centre the US sensor
int USdir = 0; // 0 = going left, 1 = going right

int Ldist = 200; //distance left of robot
int Cdist = 200; //distance centre of robot
int Rdist = 200; //distance right of robot

int RobotDir = 0; // forward = 0 back = 1 left = 2 right = 3
int oldRobotDir = 0;

long oldTimer = 0;

int SweepSpeed = 40; //sweep speed ms


void setup(){


  RadarServo.attach(4);
  RadarServo.write(USpos);

  pinMode(USPin,INPUT);

  pinMode(PWM_L,OUTPUT);
  pinMode(DIR_L,OUTPUT);  
  pinMode(BRA_L,OUTPUT);
  pinMode(CUR_L,INPUT);

  pinMode(PWM_R,OUTPUT);
  pinMode(DIR_R,OUTPUT);  
  pinMode(BRA_R,OUTPUT);
  pinMode(CUR_R,INPUT);


  digitalWrite(BRA_L,LOW);
  digitalWrite(DIR_L,LOW);
  analogWrite(PWM_L,0);

  digitalWrite(BRA_R,LOW);
  digitalWrite(DIR_R,HIGH);
  analogWrite(PWM_R,0);

  Serial.begin(9600);
}

void loop(){

  Scan();
  Ping();

  // Serial.println(rawDist);

  if (USpos >= 0 && USpos <= 30){ // if sensor is to the far left we get left dist
    Ldist = rawDist;
  }

  if (USpos >= 134 && USpos <= 164){ // if sensor is to the far right we get right dist
    Rdist = rawDist;
  }

  if (USpos >= 67 && USpos <= 97){  // ... centre dist
    Cdist = rawDist;
  }

  if (Cdist >= 20){ // front clear, go forward
    RobotDir = 0;
  }
  else             // need to turn
  {
    if (Ldist >= Rdist){
      RobotDir = 2;
    }
    if (Ldist < Rdist){
      RobotDir = 3;
    }
    if(Ldist <= 20 && Rdist <= 20 && Cdist <= 20){
      RobotDir = 1;
    }
  }

  // now move according to measures

  //Serial.println(RobotDir);


  unsigned long newTimer = millis();  //set a timer

  if (RobotDir == 0 && RobotDir == oldRobotDir){
    Serial.println(RobotDir);
    MoveFor();
    oldRobotDir = RobotDir;
  }    
  if (RobotDir == 0 && RobotDir != oldRobotDir && newTimer - oldTimer > TurnInterval){
    Serial.println(RobotDir);
    MoveFor();
    oldRobotDir = RobotDir;
    oldTimer = newTimer;
  }

  if (RobotDir == 1 && RobotDir == oldRobotDir){
    Serial.println(RobotDir);
    MoveBack();
    oldRobotDir = RobotDir;
  }    
  if (RobotDir == 1 && RobotDir != oldRobotDir && newTimer - oldTimer > TurnInterval){
    Serial.println(RobotDir);
    MoveBack();
    oldRobotDir = RobotDir;
    oldTimer = newTimer;
  }

  if (RobotDir == 2 && RobotDir == oldRobotDir){
    Serial.println(RobotDir);
    MoveLeft();
    oldRobotDir = RobotDir;
  }    
  if (RobotDir == 2 && RobotDir != oldRobotDir && newTimer - oldTimer > TurnInterval){
    Serial.println(RobotDir);
    MoveLeft();
    oldRobotDir = RobotDir;
    oldTimer = newTimer;
  }

  if (RobotDir == 3 && RobotDir == oldRobotDir){
    Serial.println(RobotDir);
    MoveRight();
    oldRobotDir = RobotDir;
  }    
  if (RobotDir == 3 && RobotDir != oldRobotDir && newTimer - oldTimer > TurnInterval){
    Serial.println(RobotDir);
    MoveRight();
    oldRobotDir = RobotDir;
    oldTimer = newTimer;
  }

}


void MoveFor(){

  digitalWrite(DIR_L,LOW);
  digitalWrite(DIR_R,HIGH);

  analogWrite(PWM_L,150);
  analogWrite(PWM_R,150);
}

void MoveBack(){

  digitalWrite(DIR_L,HIGH);
  digitalWrite(DIR_R,LOW);

  analogWrite(PWM_L,150);
  analogWrite(PWM_R,150);

}

void MoveLeft(){

  digitalWrite(DIR_L,HIGH);
  digitalWrite(DIR_R,HIGH);

  analogWrite(PWM_L,90);
  analogWrite(PWM_R,90);

}

void MoveRight(){

  digitalWrite(DIR_L,LOW);
  digitalWrite(DIR_R,LOW);

  analogWrite(PWM_L,90);
  analogWrite(PWM_R,90);

}



void Stop(){

  analogWrite(PWM_L,0);
  analogWrite(PWM_R,0);

}

void Scan(){

  runEvery(4){

    if(USpos > 3 && USdir == 0){
      USpos = USpos - 1; // change direction to right
    }

    if(USpos <163 && USdir == 1){
      USpos = USpos + 1; // change direction to right
    }
  }

  if (USpos <= 3){
    USdir = 1;
  }
  if(USpos >= 163){
    USdir = 0;
  }
  RadarServo.write(USpos);
}




void Ping(){
  runEvery(40){

    rawDist = analogRead(USPin);
  }
}

No my problem. The ultrasonic sensor seems quite sensitive, the readings fluctuate a LOT. So I’d like to average its readings. I managed to do that in the 1st iteration of the code, but now that the sweep/scan functions do not use delay(), I’m a bit lost.

It was easy to do before with a simple “for” loop inside the Scan function, but know I think it would have to be a loop in the loop()… I’m not really sure.

I would greatly appreciate any suggestions. Thanks!

Edit: added the code directly in the post, thanks wildbill.

#2

You'll probably get a bit more response if you post (or attach) your code. Folks are less likely to follow links to it on some other site.

#3

You might want to move the newTimer initialization out of the loop into either setup or as a global. I can’t say for certain that it is bad, but it probably isn’t good.
So the easiest way to generate a running average is to use a looping array.

//Initialize: Note these need to persist between function calls, so make them global, at least to begin with
int averageArray[4] = {0,0,0,0};
short int averagePointer = 0;

//In Loop or function:
averageArray[averagePointer] = getMyADCorOtherValueHere(); //Get the value
averagePointer++; //increment the pointer
averagePointer&= 3; //Keeps the pointer within range.  This only works with powers of 2.  Use 1 for 2 points, 3 for 4 points, 7 for 8 points.  Otherwise just use an if statement to reset it to 0.
int sum  = 0;
for(int i = 0; i < 3; i++){
  sum += averageArray[i];
}
sum /= 4; //Sum is your output, and is the average of the last 4 values.

Note, the longer you make the array, the longer it takes the value to settle, but the more you’ll filter out random noise.

#4

Thanks, that's the perfect explanation.

If I understand correctly, you keep writing over previous values in the array, replacing the oldest first.

So for me let's say the sensor scans in front, wall is 50cm ahead, the array fills with [50 50 50 50].

Robot keeps going forward, sensor goes right, then back to front, rescans front.

Now the wall is 30cm in front. The array will go [30 50 50 50], then [30 30 50 50] etc. So it will take some time for the average to reflect the actual distance. Is that correct?

I need to find the optimal array length then. My robot can easily move 20cm between two front scans (that may actually be another problem).

Thanks a lot!

#5

You could set up a timer interrupt and do scans every few ms. Have it update the array every time, and also calculate a running average. Then, in your main code, just read that averaged variable.

Don't forget to define the variables you use in the interrupt as volatile.

#6

looks fine averaging is simple by using 0.1 of actual sample and 0.9 of old distance for example, you can vary the numbers.
another thought, make it analog. example move in the direction with most distance, if left and fore are long and right is short move left motor 50% and right at 100%.
when making this analog the robot will move fluent instead of move stop scan.
scan while moving and only stop when direction changes too much.
have a look at fuzzy control, bit heavy but maybe.

#7

It looks like you understand the explanation.

SirNickity:
You could set up a timer interrupt and do scans every few ms. Have it update the array every time, and also calculate a running average. Then, in your main code, just read that averaged variable.

Don’t forget to define the variables you use in the interrupt as volatile.

This will work if you don’t run into issues with positioning (IE, you only want to measure when the sensor is in position and not in transit). However, look out for data race conditions, where you are accessing a variable that is being used by another process. If you can, just access the average of the array only once before you start your calculations, or the value may change if the interrupt triggers mid calculation.

I’ve tried to use the .1New+.9Old before with another application and was thoroughly disappointed by it, as it can retain old and irrelevant data indefinitely, where as the moving window discards it.

You may need to speed up the frequency of scans, possibly sweeping while you run like shooter was mentioning. Additionally, you can use the array method I mentioned (or just take a series of measurements quickly in a row) to get better data for one particular direction. However, it would be best if the device wasn’t moving.

Example:

//Run this every time you want to take a measurement
int scan(short int counts){
int sum = 0;
for(int i = 0; i < counts; i++){ //take counts number of measurements and add them together
  sum += InsertScanValueHere
  delay(5); //This is optional, depending on your ultrasonic sensor, it might help to delay some amount inbetween each measurements to let the previous measurement settle down first.  It will directly slow down the time it takes
}
sum /= counts;
return sum;
}

I am ignoring a significant amount of control system theory in all of this. Trying to explain that over the internet won’t work.

#8

I missed the part where mirith’s code snippet calculated a sum of the array. So I guess my only contribution here is to suggest doing the scan in a timer interrupt (as he pointed out: assuming this is OK to do continuously, while moving for example.)

#9

Thanks heaps all! You are awesome.

I didn't know about timer interrupts, I had a look and it seems powerful. But for this application I don't know if they are needed.

My robot at the moment does move and scan at the same time. The sensor servo sweeps left to right continuously; and the distance is read from the sensor every 40ms. Then in the main loop(), it checks the position of the servo driving the sensor. If it's at the leftmost 30° or so, it will store the distance as "distance to the left". If it's in the central 30° it stores distance as "front distance" etc. Then takes action depending on each distance.

In the loop(), I'll try to modify the "if" loops that check for sensor position and update distances. At the moment I have "if sensor is in the central 30°, distance to centre = output from sensor (only 1 value, refreshed every 40ms).

I'll try to change that to keep 8 or so rolling values in an array as Mirith suggested, and see how that goes. It should at least smooth out the sensor readings.

I also read a bit more on US sensors; it seems that unsing analogRead() is not the best; perhaps I'll try to change that to a proper "ping" method, or use PWM. That might give less noisy readings.

Anyways thanks a lot. This is so much fun, I feel I have SO much to learn, in programming, electronics and all!

#10

Alright, small update.

I changed the code to do a rolling average of the distance as Mirith suggested. It does improve the behaviour, less turns at random with nothing close and less running into walls. Still not quite right though.

I tried changing sensor reading from analog to PWM but it didn’t really improve, and more importantly it messed up the timers (I had a PulseIn in a runEvery loop).

I think the main problem though is that the sensor is constantly moving (constant sweep by servo + robot movement). I tested the sensor quickly and it’s very sensitive to the angle the wall is at.

I’ll try to change the sweep to be more “sequential” to limit sensor movement.

#11