Pages: [1] 2   Go Down
Author Topic: Robot Car Algorith  (Read 2064 times)
0 Members and 1 Guest are viewing this topic.
0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 61
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Please some one help smiley-sad i'd tryed everithing i know smiley-sad 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 !
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);
}
Logged

Global Moderator
UK
Offline Offline
Brattain Member
*****
Karma: 299
Posts: 26196
I don't think you connected the grounds, Dave.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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?

Code:
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?
« Last Edit: March 24, 2011, 06:39:16 am by AWOL » Logged

"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 61
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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 :
Code:
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
Logged

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 61
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Seattle, WA USA
Offline Offline
Brattain Member
*****
Karma: 610
Posts: 49040
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
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.

Code:
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.
Logged

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 61
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Sorry i writte the code in romanian language and translete it in englesh  smiley 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 smiley
Logged

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 61
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset





This is how it reacts smiley-sad Please give me something an ideea or something !
Logged

Global Moderator
UK
Offline Offline
Brattain Member
*****
Karma: 299
Posts: 26196
I don't think you connected the grounds, Dave.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

New Hampshire
Offline Offline
God Member
*****
Karma: 17
Posts: 781
There are 10 kinds of people, those who know binary, and those who don't.
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged


0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 61
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Code:
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 smiley-sad. Do you have a better ideea how can i make this work, plz tell me i'm willing to try everything.
Logged

Global Moderator
UK
Offline Offline
Brattain Member
*****
Karma: 299
Posts: 26196
I don't think you connected the grounds, Dave.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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"
« Last Edit: March 25, 2011, 04:20:50 am by AWOL » Logged

"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 61
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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  smiley-grin. 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
Logged

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 61
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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 ?
Logged

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 61
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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 :
Code:
if (dist9 = [30; 50]
{
ForwardLeft
}
 else
{
Forward
}

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

Offline Offline
Full Member
***
Karma: 11
Posts: 147
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Pages: [1] 2   Go Up
Jump to: