Dodging objects

Hi, I have a Line Follower Arduino robot and i´m having some trouble to program it. It already follows the line and overcomes gaps, but it doesn´t seem to dodge objects properly.

It has an ultrasonic sensor (HC-SR04) in the front of the robot, I know how to calculate the distance but the values vary too much, and i wanted the sensor to make more readings to be sure there’s an obstacle ahead but I do not know how to do it.

Could you guys help me please?

Here’s the code i’m using without the dodge function.

 //Código do Robô 0FKSGV - OBR - Equipe COTIL

//Libraries
#include <QTRSensors.h>
#include <LiquidCrystal.h>

//Variaveis
#define EMITTER_PIN 2
#define Kp .2
#define Kd 5
#define rightMaxSpeed 220
#define leftMaxSpeed 220
#define rightBaseSpeed 200
#define leftBaseSpeed 200
#define NUM_SENSORS 6
#define TIMEOUT 2500
#define VALOR_GAP 12.5

//Config Sensores
QTRSensorsRC qtr ((unsigned char[]) {A0, A1, A2, A3, A4, A5}, NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensors [NUM_SENSORS];

//Pinos
int inA1 = 4;
int inA2 = 5;
int inB1 = 6;
int inB2 = 7;
int trigPin = 9;
int echoPin = 8;
int ledFunc = 3;

//Setup
void setup () {
  calibrarSensores();
}

//Loop do programa
void loop () {
  long duration;
  long distance;
  digitalWrite (trigPin, LOW);
  delayMicroseconds (2);
  digitalWrite (trigPin, HIGH);
  delayMicroseconds (10);
  digitalWrite (trigPin, LOW);
  duration = pulseIn (echoPin, HIGH);
  distance = (duration / 2) / 29.3867;
  int position = qtr.readLine(sensors);
  if (position < VALOR_GAP) {
    analogWrite(inA1, rightMaxSpeed);
    analogWrite(inA2, 0);
    analogWrite(inB1, leftMaxSpeed);
    analogWrite(inB2, 0);
  }
  else if (position >= VALOR_GAP && position <=1000) {
    analogWrite(inA1, rightMaxSpeed);
    analogWrite(inA2, 0);
    analogWrite(inB1, 0);
    analogWrite(inB2, 0);
  }
  else if (position <= 5000 && position >= 4000) {
    analogWrite(inA1, 0);
    analogWrite(inA2, 0);
    analogWrite(inB1, leftMaxSpeed);
    analogWrite(inB2, 0);
  }
  else {
    int lastError = 0;
    int error = position - 2500;
    int motorSpeed = Kp * error + Kd * (error - lastError);
    lastError = error;
    int rightMotorSpeed = rightBaseSpeed - motorSpeed;
    int leftMotorSpeed = leftBaseSpeed + motorSpeed;
    if (rightMotorSpeed > rightMaxSpeed) rightMotorSpeed = rightMaxSpeed;
    if (leftMotorSpeed > leftMaxSpeed) leftMotorSpeed = leftMaxSpeed;
    if (rightMotorSpeed < 0) rightMotorSpeed = 0;
    if (leftMotorSpeed < 0) leftMotorSpeed = 0;
    if(rightMotorSpeed >= 0 && leftMotorSpeed >= 0){
      analogWrite(inA1, rightMotorSpeed);
      analogWrite(inA2, 0);
      analogWrite(inB1, leftMotorSpeed);
      analogWrite(inB2, 0);
    }
    if(rightMotorSpeed >= 0 && leftMotorSpeed < 0){
      leftMotorSpeed = -leftMotorSpeed;
      analogWrite(inA1, rightMotorSpeed);
      analogWrite(inA2, 0);
      analogWrite(inB1, 0);
      analogWrite(inB2, leftMotorSpeed);
    }
    if(rightMotorSpeed < 0 && leftMotorSpeed >= 0){
      rightMotorSpeed = -rightMotorSpeed;
      analogWrite(inA1, 0);
      analogWrite(inA2, rightMotorSpeed);
      analogWrite(inB1, leftMotorSpeed);
      analogWrite(inB2, 0);
    }
  }
}

//Função de calibrar os sensores
void calibrarSensores () {
  int i;
  for (int i = 0; i < 200; i++) {
    if (i < 50 || i >= 150) {
      analogWrite(inA1, 100);
      analogWrite(inA2, 0);
      analogWrite(inB1, 0);
      analogWrite(inB2, 100);
    }
    else {
      analogWrite(inA1, 0);
      analogWrite(inA2, 100);
      analogWrite(inB1, 100);
      analogWrite(inB2, 0);
    }
    qtr.calibrate();
    delay(10);
  }
}

You should post code in a code block not a quote block as its easier for everyone to read :wink:

//Código do Robô 0FKSGV - OBR - Equipe COTIL

//Libraries
#include <QTRSensors.h>
#include <LiquidCrystal.h>

//Variaveis
#define EMITTER_PIN 2
#define Kp .2
#define Kd 5
#define rightMaxSpeed 220
#define leftMaxSpeed 220
#define rightBaseSpeed 200
#define leftBaseSpeed 200
#define NUM_SENSORS 6
#define TIMEOUT 2500
#define VALOR_GAP 12.5

//Config Sensores
QTRSensorsRC qtr ((unsigned char[]) {A0, A1, A2, A3, A4, A5}, NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensors [NUM_SENSORS];

//Pinos
int inA1 = 4;
int inA2 = 5;
int inB1 = 6;
int inB2 = 7;
int trigPin = 9;
int echoPin = 8;
int ledFunc = 3;

//Setup
void setup () {
  calibrarSensores();
}

//Loop do programa
void loop () {
  long duration;
  long distance;
  digitalWrite (trigPin, LOW);
  delayMicroseconds (2);
  digitalWrite (trigPin, HIGH);
  delayMicroseconds (10);
  digitalWrite (trigPin, LOW);
  duration = pulseIn (echoPin, HIGH);
  distance = (duration / 2) / 29.3867;
  int position = qtr.readLine(sensors);
  if (position < VALOR_GAP) {
    analogWrite(inA1, rightMaxSpeed);
    analogWrite(inA2, 0);
    analogWrite(inB1, leftMaxSpeed);
    analogWrite(inB2, 0);
  }
  else if (position >= VALOR_GAP && position <=1000) {
    analogWrite(inA1, rightMaxSpeed);
    analogWrite(inA2, 0);
    analogWrite(inB1, 0);
    analogWrite(inB2, 0);
  }
  else if (position <= 5000 && position >= 4000) {
    analogWrite(inA1, 0);
    analogWrite(inA2, 0);
    analogWrite(inB1, leftMaxSpeed);
    analogWrite(inB2, 0);
  }
  else {
    int lastError = 0;
    int error = position - 2500;
    int motorSpeed = Kp * error + Kd * (error - lastError);
    lastError = error;
    int rightMotorSpeed = rightBaseSpeed - motorSpeed;
    int leftMotorSpeed = leftBaseSpeed + motorSpeed;
    if (rightMotorSpeed > rightMaxSpeed) rightMotorSpeed = rightMaxSpeed;
    if (leftMotorSpeed > leftMaxSpeed) leftMotorSpeed = leftMaxSpeed;
    if (rightMotorSpeed < 0) rightMotorSpeed = 0;
    if (leftMotorSpeed < 0) leftMotorSpeed = 0;
    if(rightMotorSpeed >= 0 && leftMotorSpeed >= 0){
      analogWrite(inA1, rightMotorSpeed);
      analogWrite(inA2, 0);
      analogWrite(inB1, leftMotorSpeed);
      analogWrite(inB2, 0);
    }
    if(rightMotorSpeed >= 0 && leftMotorSpeed < 0){
      leftMotorSpeed = -leftMotorSpeed;
      analogWrite(inA1, rightMotorSpeed);
      analogWrite(inA2, 0);
      analogWrite(inB1, 0);
      analogWrite(inB2, leftMotorSpeed);
    }
    if(rightMotorSpeed < 0 && leftMotorSpeed >= 0){
      rightMotorSpeed = -rightMotorSpeed;
      analogWrite(inA1, 0);
      analogWrite(inA2, rightMotorSpeed);
      analogWrite(inB1, leftMotorSpeed);
      analogWrite(inB2, 0);
    }
  }
}

//Função de calibrar os sensores
void calibrarSensores () {
  int i;
  for (int i = 0; i < 200; i++) {
    if (i < 50 || i >= 150) {
      analogWrite(inA1, 100);
      analogWrite(inA2, 0);
      analogWrite(inB1, 0);
      analogWrite(inB2, 100);
    }
    else {
      analogWrite(inA1, 0);
      analogWrite(inA2, 100);
      analogWrite(inB1, 100);
      analogWrite(inB2, 0);
    }
    qtr.calibrate();
    delay(10);
  }
}

BTW. In regard to your original question.

I think you are asking how to determine whether input values are in a specific range, and then take appropriate actions?

It may be an over complicated way to do it, but I normally use an array of range values and iterate though the list comparing the input value with range values from the array.

If you need to do completely different things for each range, you could use pointers to functions i.e have a function per distinct action. Or use enums to indicate an action number that you then process using a switch statement

I can see that you get the distance BUT You don’t use it any where?

But the real question is are you following the line or avoiding objects?

Take a close look at the way the sensor work what’s its angle of vision from side to side and up and down ( hint it spreads out in a cone) are you seeing the ground?

Mark

I didn’t include the code to calculate the distance using the ultrassonic sensor.
Here is a code I use to calculate the distance and show it on an LCD display:

#include <LiquidCrystal.h>

//Variaveis
LiquidCrystal lcd (11,10,9,8,7,6);
int ledObs = 2;
int ledClr = 1;
int trigPin = 13;
int echoPin = 12;

//Setup
void setup () {
  Serial.begin(9600);
  lcd.clear();
  pinMode (trigPin, OUTPUT);
  pinMode (echoPin, INPUT);
  pinMode (ledObs, OUTPUT);
  pinMode (ledClr, OUTPUT);
}

//Loop
void loop () {
  long duration;
  long distance;
  lcd.clear();
  digitalWrite (trigPin, LOW);
  delayMicroseconds (2);
  digitalWrite (trigPin, HIGH);
  delayMicroseconds (10);
  digitalWrite (trigPin, LOW);
  duration = pulseIn (echoPin, HIGH);
  distance = (duration / 2) / 29.3867;
  if (distance < 30) {
    digitalWrite (ledObs, HIGH);
    digitalWrite (ledClr, LOW);
  }
  else {
    digitalWrite (ledObs, LOW);
    digitalWrite (ledClr, HIGH);
  }
  lcdprint () ;
}

void lcdprint () {
  long duration;
  long distance;
  digitalWrite (trigPin, LOW);
  delayMicroseconds (2);
  digitalWrite (trigPin, HIGH);
  delayMicroseconds (10);
  digitalWrite (trigPin, LOW);
  duration = pulseIn (echoPin, HIGH);
  distance = (duration / 2) / 29.3867;
  lcd.setCursor (1,1);
  lcd.print ("Distancia");
  lcd.setCursor (12,4);
  lcd.print(distance);
  delay (100);
}

The issue is that I need to get many readings of this value and take an “average” to check if the sensor is really “seeing” an object. And I need my robot to follow the line AND avoid objects. The code I’ve put up there is just the line follower code.

Do you mean as described here

http://en.wikipedia.org/wiki/Moving_average

or here

http://www.bennadel.com/blog/1627-create-a-running-average-without-storing-individual-values.htm

The second one.

http://playground.arduino.cc/Main/RunningAverage

Sorry man, I'm very newbie to this. Could you help me using this? For example, I've got this code and the 'distance' value returned to me. My objective is that, while he's following a line, if there's an object in front of him, he'd dodge it. But, as I said, the values vary too much, so I'd have to use this "Running Average". Could you help me with this? Sorry about my english, I'm from Brazil :/

I took a look at the running average code in the Arduino page, but it doesn’t compile

So I’ve modified it, and this uses a rolling window (size you can specify)
The demo has a window of 10 values, therefore the value returned is the average over the last 10 times you have called the function.
It may be possible to improve the speed a bit.

There are other methods to average, but I think what you want is short term windowing average rather than a method that takes into consideration all values you have ever received from the sensor.

You may need to change the window size to suit your application. If you need to use a really long window, you may overflow the sum var so you may need to make this a float

#define WINDOW_SIZE 10


int windowedAverage(int v)
{
static int LM[WINDOW_SIZE];      // LastMeasurements
static int index = 0;
  long sum=0;
  int windowSize;

  LM[index%WINDOW_SIZE]=v;
  
  index++;
  windowSize=index;
  if (index>WINDOW_SIZE)
  {
    windowSize=WINDOW_SIZE;
  }  
  for(int i=0;i<windowSize;i++)
  {
    sum += LM[i];
  }

  return sum / windowSize;
}

void setup(void) 
{
  int rn;
  Serial.begin(115200);
  Serial.println("Demo Windowed Average ");
  
  for(int i=0;i<100;i++)
  {
    rn = random(0, 1000);
    Serial.print("Value,");
    Serial.print(rn);
    Serial.print(",Average,");
    Serial.println(windowedAverage(rn));  
  }

}

void loop(void) 
{

}