Buonasera a tutti.
Vi scrivo perchè ho dei dubbi nella scrittura del codice di un robot 4wd che evita gli ostacoli tramite un sensore ad ultrasuoni.
Vorrei che il robot facendo delle rotazioni “analizzasse” lo spazio introno a se compiendo delle rotazioni a destra e sinistra nel momento in cui trova un ostacolo davanti a se, e scegliere la direzione in cui procedere.
Vi posto di seguito il codice.
int vel_m1 = 5; // PWM
int vel_m2 = 6; // PWM
int dir_m1 = 4;
int dir_m2 = 7;
int trx = 8; // Pin Trasmettitore
int rcx = 9; // Pin Ricevitore
long d; // Var. Distanza
int val_rcx_sx;
int val_rcx_dx;
void setup()
{
pinMode(vel_m1, OUTPUT);
pinMode(vel_m2, OUTPUT);
pinMode(dir_m1, OUTPUT);
pinMode(dir_m2, OUTPUT);
pinMode(trx, OUTPUT);
pinMode(rcx, INPUT);
Serial.begin(9600);
Serial.println("Cherokey 4WD + Sens. Ultrasuoni");
}
void indietro(byte sx_vel, byte dx_vel) // Utilizzo byte 0 <val.< 255
{
analogWrite(vel_m1, dx_vel);
digitalWrite(dir_m1, HIGH);
analogWrite(vel_m2, sx_vel);
digitalWrite(dir_m2, HIGH);
Serial.println("Robot -> Indietro");
}
void avanti(byte sx_vel, byte dx_vel)
{
analogWrite(vel_m1, dx_vel);
digitalWrite(dir_m1, LOW);
analogWrite(vel_m2, sx_vel);
digitalWrite(dir_m2, LOW);
Serial.println("Robot -> Avanti");
}
void sinistra(byte sx_vel, byte dx_vel)
{
analogWrite(vel_m1, dx_vel);
digitalWrite(dir_m1, LOW);
analogWrite(vel_m2, sx_vel);
digitalWrite(dir_m2, HIGH);
Serial.println("Robot -> Sinistra");
}
void destra(byte sx_vel, byte dx_vel)
{
analogWrite(vel_m1, dx_vel);
digitalWrite(dir_m1, HIGH);
analogWrite(vel_m2, sx_vel);
digitalWrite(dir_m2, LOW);
Serial.println("Robot -> Destra");
}
void fermo(byte sx_vel, byte dx_vel)
{
analogWrite(vel_m1, dx_vel);
analogWrite(vel_m2, sx_vel);
Serial.println("Robot -> Fermo");
}
void sensore()
{
digitalWrite(trx, LOW);
digitalWrite(trx, HIGH);
delay(10);
digitalWrite(trx, LOW);
long durata=pulseIn(rcx, HIGH);
d= 0.034 * durata / 2;
Serial.print("Durata Ritorno Impulso: ");
Serial.print(durata);
Serial.print(" -> ");
if(durata>38000)
{
Serial.print("Fuori Portata");
}
else
{
Serial.print(d);
Serial.println("cm");
}
}
void loop()
{
sensore();
if(d>20)
{
avanti(200,200);
}
if(d<=20) // Confronto dist. sx. - dist. dx.
{
sinistra(200,200);
delay(2000);
val_rcx_sx=d;
destra(200,200);
delay(4000);
val_rcx_dx=d;
if(val_rcx_sx<val_rcx_dx)
{
avanti(200,200);
}
else
{
sinistra(200,200);
delay(4000);
avanti(200,200);
}
}
}
Non credo che funzioni perfettamente questo software poichè compie sempre la stessa scelta, ovvero girare a sinistra.
Grazie mille in anticipo