Robot Car Algorith

Please some one help :frowning: i'd tryed everithing i know :frowning: i have a car toy with 4 ping sensors on it, and want to make it avoid the object. if is some one who can help plz. here is the code !

//_________________________________________________________________
// Bogdan T. Cristian                                              |
// Universitatea "1 Decembrie 1918" Alba Iulia                     |
// Facultatea de Stiinte, Specializare Matematica - Informatica    |
// Anul III - Gr. I                                                |
// R.A.I ( Robot Autonom Inteligent )                              |
// Software de conducere autonoma a mecanismului de deplasare.     |
//_________________________________________________________________|

//Declarare pini folositi!

//Senzorii
const int pingPin9 = 9; // Declararea pinului 9 ca si pin de semnal pentru senzor(dreapta)
const int pingPin8 = 8; // Declararea pinului 8 ca si pin de semnal pentru senzor(Stanga)
const int pingPin3 = 3; // Declararea pinului 3 ca si pin de semnal pentru senzor(Fata)
const int pingPin2 = 2; // Declararea pinului 2 ca si pin de semnal pentru senzor(Spate)

// Motoare
int E1 = 7; // Declararea pinului 7 ca si pin de semnal pentru Controlul Motorului electric 1
int M1 = 6; // Declararea pinului 6 ca si pin de semnal pentru Motorul electric 1
int E2 = 4; // Declararea pinului 4 ca si pin de semnal pentru Controlul Motorului electric 2
int M2 = 5; // Declararea pinului 5 ca si pin de semnal pentru Motorul electric 2

void setup() 
  { 
    Serial.begin(9600); // Citirea conexiuni seriale intre PC si Arduino
  pinMode(M1, OUTPUT); // Setarea pinului 6 ca si semnal de iesire
  pinMode(M2, OUTPUT); // Setarea pinului 5 ca si semnal de iesire
  }
  
  void loop()
{
  // pornire senzori !
  long dist2 = ping(pingPin2); // Stocarea datelor de la senzor in "dist2"
  delay(25); // Timpul de reactie a citiri senzorului 
  long dist8 = ping(pingPin8); // Stocarea datelor de la senzor in "dist8"
  delay(25); // Timpul de reactie a citiri senzorului
  long dist3 = ping(pingPin3); // Stocarea datelor de la senzor in "dist3"
  delay(25); // Timpul de reactie a citiri senzorului
  long dist9 = ping(pingPin9); // Stocarea datelor de la senzor in "dist9" 
  delay(25); // Timpul de reactie a citiri senzorului
  
//afisare pe Monitorul Serial valorile inregistrate de Senzori  
Serial.print("Back: "); Serial.println(dist2);    // Afisarea in monitorul serial a datelor receptionate de la senzorul aflat pe pinul 2
Serial.print("Left: "); Serial.println(dist8);   // Afisarea in monitorul serial a datelor receptionate de la senzorul aflat pe pinul 8
Serial.print("Front: "); Serial.println(dist3);     // Afisarea in monitorul serial a datelor receptionate de la senzorul aflat pe pinul 3
Serial.print("right: "); Serial.println(dist9);  // Afisarea in monitorul serial a datelor receptionate de la senzorul aflat pe pinul 9

//_____________________Conditii de deplasarea a dispozitivului mobil in functie de citirea senzorilor_______________________
Front();
if (dist3 < 70){
BackLeft();
}

if (dist9 < 50) { 
  FrontLeft();
} 

if (dist9 < 20) {
  backRight();
}

if (dist8 <50) {
  FrontRight();
}

if (dist8 < 20) {
  BackLeft();
}

if (dist2 < 40) {
  Front();
}
//__________________________________________________________________________________________________________________________________
}
// Functia de citire a senzorilor 
  long ping(int pinNumToReadFrom)
{
  long duration, cm; 
  pinMode(pinNumToReadFrom, OUTPUT); // Setarea de iesire pe canalul de semnal al senzorului
  digitalWrite(pinNumToReadFrom, LOW); // Valoarea trimisa pe canalul de semnal
  delayMicroseconds(2); // Durata de trimitere a valori pe canalul de semnal
  digitalWrite(pinNumToReadFrom, HIGH); // Valoarea trimisa pe canalul de semnal
  delayMicroseconds(5); // Durata de trimitere a valori pe canalul de semnal
  digitalWrite(pinNumToReadFrom, LOW); // Valoarea trimisa pe canalul de semnal
  pinMode(pinNumToReadFrom, INPUT); // Setarea ca si intrarea a canalului de semnal al senzorului
  duration = pulseIn(pinNumToReadFrom, HIGH); // Calculul pentru durata semnalului iesit pentru a reveni la senzor 
  cm = microsecondsToCentimeters(duration); // Calcularea distantei in centimetri in functie de durata de semnalului
  return cm; // Returneaza valoarea in centimetri
}

long microsecondsToCentimeters(long microseconds)
{
  return microseconds / 29 / 2; // Transformarea microsecundelor in cm 
}
 
 
 // Baza de comenzi prestabilite cu diferite moduri de conducere a dispozitivului mobil 
 
void Front()
{ 
digitalWrite(M1,LOW); 
digitalWrite(M2,HIGH);
analogWrite(E1, 255);  
analogWrite(E2, 0); 
} 

void FrontLeft()
{ 
digitalWrite(M1,LOW); 
digitalWrite(M2,HIGH);
analogWrite(E1, 255);  
analogWrite(E2, 255); 
} 

void FrontRight()
{ 
digitalWrite(M1,LOW); 
digitalWrite(M2,LOW);
analogWrite(E1, 255);  
analogWrite(E2, 255); 
} 

void Back()
{ 
digitalWrite(M1,HIGH); 
digitalWrite(M2,HIGH);
analogWrite(E1, 255);  
analogWrite(E2, 0); 
} 

void BackLeft()
{ 
digitalWrite(M1,HIGH); 
digitalWrite(M2,HIGH);
analogWrite(E1, 255);  
analogWrite(E2, 255); 
} 

void BackRight()
{ 
digitalWrite(M1,HIGH); 
digitalWrite(M2,LOW);
analogWrite(E1, 255);  
analogWrite(E2, 255); 
} 
 
void Stop()
{
digitalWrite(M1,LOW);
digitalWrite(M2,LOW);
analogWrite(E1, 0);
analogWrite(E2, 0);
}

You've just posted a load of code. (I'm guessing you didn't write it)
You haven't said anything about the vehicle, how and where the sensors are mounted, or how the vehicle behaves and how that differs from what you expect or want it to behave.

Now, can we start again?

analogWrite(E1, 0);
analogWrite(E2, 0);
}

Which board are you using?
On a basic Arduino, neither pin 7 nor pin 4 support PWM.
Have you got M1 and M2 confused with E1 and E2?

I write the hole code !This is the car
it has a shield l298n wich has the pwm control on the pins 4 to 7

i only need some one to help me with the statement part ! wich is this :

Front();
if (dist3 < 70){
BackLeft();
}

if (dist9 < 50) { 
  FrontLeft();
} 

if (dist9 < 20) {
  backRight();
}

if (dist8 <50) {
  FrontRight();
}

if (dist8 < 20) {
  BackLeft();
}

if (dist2 < 40) {
  Front();
}

Conditions of driving

this way i'm using to much "if" ... is there a simpler or an elegant solution ?

this way i'm using to much "if" ... is there a simpler or an elegant solution ?

It looks to me like you are using just the right amount of if statements. Of course, many of them should be probably be else or else if statements. Presumably, you want the car to one thing based on each value.

if (dist9 < 50) { 
  FrontLeft();
} 

if (dist9 < 20) {
  backRight();
}

If dist9 is 15, both FrontLeft() and backRight() will be called. Is that what you want?

Why are some functions starting with upper case letters, while others use lower case letters? You should be consistent.

Sorry i writte the code in romanian language and translete it in englesh :slight_smile: all the code stare with upper case letters.
yes i want the care to do something based on each sensor value. But way i thing i use to much "if" is because the Void lood has to read each IF statemant and the care is moving fast and dosent have time to react. if you want i can put a video :slight_smile:

This is how it reacts :frowning: Please give me something an ideea or something !

Before you damage the car and/or the furniture, can I suggest you lift the car off the floor, and put in some debug prints to see what is going on?
You can use a book or similar to simulate an approaching wall/chair leg/cat.

As PaulS pointed out, you have potential situations in which you are commanding the car to go FrontLeft() and backRight() at the same time. It is physically impossible to command the car to go in two completely opposite directions at the same time.

if (dist9 < 50) { 
  FrontLeft();
} 

if (dist9 < 20) {
  backRight();
}

in this part "dist9" its the sensor in the right so if the distant from him to the wall is less than 50 to go FrontLeft the opposite direction and if is less than 20 to backwars steer right, to get away from the wall. At lest that is the ideea :(. Do you have a better ideea how can i make this work, plz tell me i'm willing to try everything.

Look at your bit of code there.
Imagine that "dist9" has the value 9.

So, is "dist9" less than 50?
Yes, so call "FrontLeft". This takes maybe a hundred microseconds.
Now, is "dist9" less than 20?
Yes, so call "backRight".

Test your smallest distance first and use an "else"

now i see it when the car is closer to the wall she is doing both conditions is less then 50 and by 20 and it jam's :D. i need to test it firs to the smallest distance that the car is responding and use that as a referencem, and to use some else ?

but how can i do something like this ? The car to move forward 1m , stop, compare all the senzor distance and move(1m) toward the sensor that has the bigest distance in front of him ? can i do that, and it's possible plz can you give me an ideea how can i do it ?

Tnx very much

Or can i use something the values like betwen 30 and 50 do FrontRight and if values betwen 10 and 20 do BackLeft ? it's this possible ? to do a map of values ? betwen the car should respons ?

no one know's ? can i use a syntax or something to make the car do somethig ( like go forward) on if the sensor has the values betwen 30 and 50 for example ?

somethig like this :

if (dist9 = [30; 50]
{ 
ForwardLeft 
}
 else 
{
Forward
}

something like that to read all the values betwen 30 and 50. Please help.
Tnx

if(dist9 > 30 && dist9 < 50)
{
ForwardLeft();
}

TNX very much :slight_smile: