Managing IR detection&Beacon detection at high speed

amine244 · March 11, 2011, 3:16pm

Hello
I’m participating in a competition where the robot needs to go at a great speed from a corner to the opposite one on the table (Diagonal) Meanwhile there are some obstacles and other robots coming from the other corners that have to be avoided. I intend to put an IR beacon at the far end of the table.
I tried to make the robot avoid obstacles close to each other (35 to 40 cm), and i noticed that sometimes the robots blocks and starts going back and forth several seconds before deciding to go to the beacon, especially when he detects something very close with all four IRs. I hope I made myself clear.

Here is my main loop.

void loop() {

scan(); 
boolean obsDetected = foundObstacle();
readTransceiver();
boolean beaconDetected = foundBeacon();

if (obsDetected == true){
obstacleAvoidanceModule();
}
else if(beaconDetected == true){
moveTowardBeacon();
}
else{
go_forward();
}
}

It’s pretty basic stuff. Do you find something to be corrected?
Now this works for a speed value of 150, when I give it 250 it crashes against the obstacles as if it doesn’t get to read the IRs fast enough, how can I correct this? Since my competition is a speed contest (The winner is the fastest robot to cross the diagonal), I need it to be able to function well at at least 250.
I’m not home right now so i can’t post the rest of the code.
I’ll post it later
Thanks^^

system · March 11, 2011, 4:09pm

I'm not home right now so i can't post the rest of the code. I'll post it later

If you need help with it, you'll have to.

amine244 · March 11, 2011, 10:54pm

#include <math.h>

int i;
      
      // TRANSCEIVER VARIABLES //
      #define DETECTION_CONST 800
      #define BEACON_SPEED 100
      int west = 0;
      int south = 0;
      int east = 0;
      int north = 0;
      int dir = 0;
      int minValue = 800;
      boolean beaconDetected = false;
      // TRANSCEIVER DIRECTION MODE VARIABLES //
      const int NUM_READINGS = 10;
      int directionReadings[NUM_READINGS];
      int modeOfDirections = 1;
      int index = 0;

      // IR detection 
      #define NB_SENSORS 15
      #define IR_NB_READINGS 10
      #define DISTANCE_LIMIT 40
      int ir0, ir1, ir2, ir3;
      float ir_dist[4]={0};
      int irReadings0[IR_NB_READINGS]={0};
      int irReadings1[IR_NB_READINGS]={0};
      int irReadings2[IR_NB_READINGS]={0};
      int irReadings3[IR_NB_READINGS]={0};
      boolean obsDetected  = false;
      boolean accelerateOn = false;
          
      // Variables Moteurs
      #define TOPSPEED 220
      #define FORWARD_SPEED 150
      #define TURN_SPEED 60
      // motor A
      int dir1PinA = 13;
      int dir2PinA = 12;
      int speedPinA = 10;
      // motor B
      int dir1PinB = 11;
      int dir2PinB = 8;
      int speedPinB = 9;
      int speeds[2]={0};
      
      void setup() {

  Serial.begin(9600);
  pinMode(dir1PinA, OUTPUT);
  pinMode(dir2PinA, OUTPUT);
  pinMode(speedPinA, OUTPUT);
  pinMode(dir1PinB, OUTPUT);
  pinMode(dir2PinB, OUTPUT);
  pinMode(speedPinB, OUTPUT);}
 
    void loop(){
      
      scan(11,12,13,14);
      obsDetected    = obstacle();
      accelerateOn   = nothingInFront();
      readTransceiver(1,2,3,4);
      beaconDetected = foundBeacon();
      
      if (obsDetected==true){
        Serial.println("obstacle");
        obstacleAvoidanceModule();}
       
      else{
          if(beaconDetected==true){ 
              Serial.println("beacon");
              moveTowardBeacon();}
          else{
              go_forward(FORWARD_SPEED,FORWARD_SPEED);}}
               
}

amine244 · March 11, 2011, 10:59pm

 // Detection Functions

void scan(int a, int b, int c, int d){
  
  for(int i=0 ; i< IR_NB_READINGS; i++){
  irReadings0[i] = analogRead(a);
  irReadings1[i] = analogRead(b);
  irReadings2[i] = analogRead(c);
  irReadings3[i] = analogRead(d);}
    valuesOptimisation();
}

void valuesOptimisation(){
  
ir_dist[0]=read_gp2d12_range(test_valeurs(irReadings0, IR_NB_READINGS));
//Serial.println("");
ir_dist[1]=read_gp2d12_range(test_valeurs(irReadings1, IR_NB_READINGS));
//Serial.println("");
ir_dist[2]=read_gp2d12_range(test_valeurs(irReadings2, IR_NB_READINGS));
//Serial.println("");
ir_dist[3]=read_gp2d12_range(test_valeurs(irReadings3, IR_NB_READINGS));}
//Serial.println("");

boolean nothingInFront(){
if ((ir_dist[0] < 75) || (ir_dist[1] < 75) || (ir_dist[2] < 75) || (ir_dist[3] < 75)){
return false;}
else {
return true;}}

boolean obstacle(){
if ((ir_dist[0] < DISTANCE_LIMIT) || (ir_dist[1] < DISTANCE_LIMIT) || (ir_dist[2] < DISTANCE_LIMIT) || (ir_dist[3] < DISTANCE_LIMIT)){
return true;}
else {
return false;}
}
  
void obstacleAvoidanceModule(){


if ((ir_dist[0] < DISTANCE_LIMIT) && ((ir_dist[1] > DISTANCE_LIMIT) && (ir_dist[2] > DISTANCE_LIMIT) && (ir_dist[3] > DISTANCE_LIMIT))){
    go_forward((int)(TURN_SPEED+1.2*ir_dist[0]),(int)(0.75*FORWARD_SPEED));}
  
else if (((ir_dist[0] > DISTANCE_LIMIT) && (ir_dist[1] > DISTANCE_LIMIT) && (ir_dist[2] > DISTANCE_LIMIT)) && (ir_dist[3] < DISTANCE_LIMIT)){
    go_forward((int)(0.75*FORWARD_SPEED),(int)((TURN_SPEED+1.2*ir_dist[3])));}

else if (((ir_dist[0] < DISTANCE_LIMIT) && (ir_dist[3] < DISTANCE_LIMIT)) && ((ir_dist[1] > DISTANCE_LIMIT) && (ir_dist[2] > DISTANCE_LIMIT))){
    go_forward(FORWARD_SPEED,FORWARD_SPEED);}
  
else if ((ir_dist[0] < DISTANCE_LIMIT)||(ir_dist[1] < DISTANCE_LIMIT)){
    turn_left(1.6*TURN_SPEED,FORWARD_SPEED);}
  
else if ((ir_dist[3] < DISTANCE_LIMIT)||(ir_dist[2] < DISTANCE_LIMIT)){
    turn_right(FORWARD_SPEED,1.6*TURN_SPEED);}
  
if ((ir_dist[1] < DISTANCE_LIMIT) && (ir_dist[2] < DISTANCE_LIMIT)){
    if((ir_dist[0] < DISTANCE_LIMIT)&&(ir_dist[3] > DISTANCE_LIMIT)){
      go_backwards((int)(0.75*FORWARD_SPEED),(int)((TURN_SPEED+1.2/3*(ir_dist[0]+ir_dist[1]+ir_dist[2]))));}
    else if ((ir_dist[3] < DISTANCE_LIMIT)&&(ir_dist[0] > DISTANCE_LIMIT)){
      go_backwards((TURN_SPEED+(int)(1.2/3*(ir_dist[1]+ir_dist[2]+ir_dist[3]))),(int)(0.75*FORWARD_SPEED));}
    if ((ir_dist[3] < DISTANCE_LIMIT)&&(ir_dist[0] < DISTANCE_LIMIT)){
        if(ir_dist[0] < ir_dist[3]){
            go_backwards(TOPSPEED,0);}
        else{
            go_backwards(0,TOPSPEED);}}
  }
}

// Motor Functions

void set_speed(int left, int right){ // set speed
  speeds[0]=left;
  speeds[1]=right;
  analogWrite(speedPinA, speeds[0]);
  analogWrite(speedPinB, speeds[1]);}
  
void set_direction (byte j){ // set direction   ==>>  0 = right ; 1 = left ; 2 = go backward ; 3 = forward  <<==
  switch(j){
  case 0:
  digitalWrite(dir1PinA, LOW);
  digitalWrite(dir2PinA, HIGH);
  digitalWrite(dir1PinB, HIGH);
  digitalWrite(dir2PinB, LOW);
  break;
  case 1:
  digitalWrite(dir1PinA, HIGH);
  digitalWrite(dir2PinA, LOW);
  digitalWrite(dir1PinB, LOW);
  digitalWrite(dir2PinB, HIGH);
  break;
  case 2:
  digitalWrite(dir1PinA, LOW);
  digitalWrite(dir2PinA, HIGH);
  digitalWrite(dir1PinB, LOW);
  digitalWrite(dir2PinB, HIGH);
  break;
  case 3:
  digitalWrite(dir1PinA, HIGH);
  digitalWrite(dir2PinA, LOW);
  digitalWrite(dir1PinB, HIGH);
  digitalWrite(dir2PinB, LOW);
  break;}}

void go_forward(int left, int right){
  set_speed(left, right);
  set_direction(3);}
  
void go_backwards(int left, int right){
  set_speed(left, right);
  set_direction(2);}
  
void turn_right(int left, int right){
  set_speed(left,right);
  set_direction(1);}

void turn_left(int left, int right){
  set_speed(left,right);
  set_direction(0);}

void accelerate(int init, int fin){
  for (i=init; i<=fin; i+=1){
    go_forward(i,i);}}
 
void slowToStop(){
  for (i=TOPSPEED; i>=0 ; i-=5) {
    go_forward(i,i);}}

void stoppe(){
  set_speed(0,0);}

// Distance Conversion
  
float read_gp2d12_range(float ir_value){
    if (ir_value < 3){
    return -1; }// invalid value
    return (6787.0 /(ir_value - 3.0)) - 4.0;}
        
// Test Values
  
 float test_valeurs(int *tab, int sizeTab){
        // Local Variables 
        int i,j;
        int copy[sizeTab];
        float var[sizeTab];
        float mean=0;
        float varmean=0;
        int nb_best = sizeTab*3/5;
        int newCopy[nb_best];
        float newmean=0;
        
        // Copy tab, Calculate mean
        for (i=0; i<sizeTab;i++){
          copy[i]=tab[i];
          mean+=tab[i];}
        mean/=sizeTab;
    
        // Calculate variances and meand of variances  : Var = (value - mean)²
        for(i=0 ; i<sizeTab ; i++){
          var[i]=pow((tab[i] - mean), 2);
          varmean+=var[i];}
        varmean/=sizeTab;
      
       // Selection and Copy best values according to the variance criterion then calculate new mean
        for (i=0; i<nb_best; i++){
          int it=-1;
          int max=0;
                  for (j=0; j<sizeTab; j++){
                    if (copy[j]>max && var[j]<= varmean){
                      max=copy[j];
                      it=j;}
                  }
          newCopy[i]=copy[it];
          if((newCopy[i]>600)||(newCopy[i]<3)){
          newCopy[i]=mean;}  
          newmean+=newCopy[i];
          copy[it]=0;
        }
      newmean/=nb_best;

return newmean;
}  
  
// Beacon Functions

void readTransceiver(int a, int b, int c, int d){
  north  = analogRead(a);
  east   = analogRead(b);
  south  = analogRead(c);
  west   = analogRead(d);}
  
boolean foundBeacon(){
  if((west  > DETECTION_CONST) && (east  > DETECTION_CONST ) && (south  > DETECTION_CONST ) && (north > DETECTION_CONST)){
  return false;   }
else{     
return true;   }}
  
void getDirAndSetMode(){
  getDirection();
  setModeOfDirections();}
  
void getDirection(){
     
  if(minValue > west){
    //minValue = west;
    dir = 4;}
  else if(minValue >  south){
    //minValue = south;
    dir = 3;}
  else if(minValue > east){
   //minValue = east;
    dir = 2;}
  else if(minValue > north){
   //minValue = north;
    dir = 1;}
   addDirectionToReadings();}
  //print_beacon_direction();

void addDirectionToReadings(){
  directionReadings[index] = dir;
  index = (index + 1);
  if (index == NUM_READINGS)             // if we're at the end of the array...
    index = 0;}
  
void setModeOfDirections(){
  
  int directionCounts[4] = {0,0,0,0}; //{North(1), East(2), South(3), West(4)}
 
  for (int i = 0; i < NUM_READINGS; i++){
    //Serial.print(directionReadings[i]);
    //Serial.print("|");
     directionCounts[directionReadings[i]-1]++;}
 
  //Determine mode of directions from count array
  //Serial.println("");
  
  int maxi=0;
  int it=-1;
  for(int i = 0; i < 4; i++){
    //Serial.print(directionCounts[i]);
    //Serial.print("|");
    
    if(maxi < directionCounts[i]){
      maxi = directionCounts[i]; //set count
      it = i+1;}} // set direction} 
 
  modeOfDirections = it;
  //Serial.println("");
  //Serial.print("Direction Mode: ");
  //Serial.println(modeOfDirections);
}

void moveTowardBeacon(){
   //g_lcd.clear();
  
    getDirAndSetMode();
    if( modeOfDirections == 4 || modeOfDirections==3){ // West
  
    turn_left(BEACON_SPEED, TURN_SPEED);}
    
    else if( modeOfDirections == 2 ){ // East
    
    turn_right(TURN_SPEED,BEACON_SPEED);}
    //delay (100);
   
    else if( modeOfDirections == 1 ){ //North
  
      if (accelerateOn==true){
        go_forward(TOPSPEED,TOPSPEED);}
      else{
    go_forward(FORWARD_SPEED,FORWARD_SPEED);}}
 }

amine244 · March 11, 2011, 11:00pm

This is the code^^ Waiting for replies^^

system · March 12, 2011, 6:54pm

Remember that you ask someone who does not know what the code drives to to give advice on how to fix the robot?

First advice - describe the robot wiring and components... and how it connects to the arduino.

However your code is also poorly documented.

As code gets longer, the task of understanding the intent grows, perhaps much faster than the actual length.. unless the writer is careful.

This code is moderate in length, but very difficult to understand because you have not documented it well.

In general you should document the reason you are writing the code...

In the case of a micro-controller controlling a motor using sensors, you should provide in the code the following information..

the expected range of values from the sensors.
the range of the motor controls, and their meaning.
for each define, the is a reason for it, as well as a name that reminds you of the reason
for each formula, the intention of it, and the reason the values are chosen.

Then there are the variable names, they should be sensible, and relate to the problem. For example ir0, .. ir4 presumably are geometrically laid out on the robot. Something like if_Left, ir_Right, ir_Front, ir_Back would help.

Then there are the magic pin numbers scan(11, 12, 13, 14)

These should be defined and referred to by a name.

After these sorts of issues are fixed the code will be easier to read, understand and comment on.

Sorry that I can not help you with your problem :(.

amine244 · March 12, 2011, 7:32pm

Sorry! :blush: I'll work on it, and come back to you^^

system · May 5, 2021, 7:29pm