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