# Managing IR detection&Beacon detection at high speed

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();
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^^

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.

``````#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 //
int modeOfDirections = 1;
int index = 0;

// IR detection
#define NB_SENSORS 15
#define DISTANCE_LIMIT 40
int ir0, ir1, ir2, ir3;
float ir_dist[4]={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();
beaconDetected = foundBeacon();

if (obsDetected==true){
Serial.println("obstacle");
obstacleAvoidanceModule();}

else{
if(beaconDetected==true){
Serial.println("beacon");
moveTowardBeacon();}
else{
go_forward(FORWARD_SPEED,FORWARD_SPEED);}}

}
``````
`````` // Detection Functions

void scan(int a, int b, int c, int d){

for(int i=0 ; i< IR_NB_READINGS; i++){
valuesOptimisation();
}

void valuesOptimisation(){

//Serial.println("");
//Serial.println("");
//Serial.println("");
//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

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){

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;}
//print_beacon_direction();

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("|");

//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);}}
}
``````

This is the code^^ Waiting for replies^^

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.