[RISOLTO]Robot 2 Sensore Ultrasuoni

Ciao a tutti. :slight_smile:
Sto lavorando su un robot evita ostacoli con 2 sensori ultrasuoni (HY-SRF05).
Vorrei che il robot, una volta incontrato un’ostacolo, scegliesse la direzione verso cui muoversi dove legge la distanza maggiore.
Sorge un problema, compie sempre la stessa scelta.
Vi posto qui il codice, magari mi potete dare una mano.

int vel_m1 = 5;  // PWM
int vel_m2 = 6;  // PWM
int dir_m1 = 4;
int dir_m2 = 7;
int trx_1 = 8;    // Pin Trasmettitore 1 - (DX - pin: 8-tx,9-rx)
int rcx_1 = 9;    // Pin Ricevitore 1
int trx_2 = 10;   // Pin Trasmettitore 2 - (SX - pin: 10-tx, 11-rx)
int rcx_2 = 11;   // Pin Ricevitore 2
long tempo_1;
long tempo_2;
long d_1;         // Distanza misurata Sensore 1(DX)
long d_2;         // Distanza misurata Sensore 2(SX)
int d_rcx_dx;
int d_rcx_sx;
int d_dx;         // Variabile somma robot -> DX d_dx=d_1+d_2 DX
int d_sx;         // Variabile somma robot -> SX d_sx=d_1+d_2 SX
int d_tot;        // Variabile somma -> d_tot=d_1+d_2
int d_diff;        //Variabile differenza -> d_diff= d_1-d_2
int d_max;
void setup()
{
  pinMode(vel_m1, OUTPUT);
  pinMode(vel_m2, OUTPUT);
  pinMode(dir_m1, OUTPUT);
  pinMode(dir_m2, OUTPUT);
  pinMode(trx_1, OUTPUT);
  digitalWrite(trx_1, LOW);
  pinMode(trx_2, OUTPUT);
  digitalWrite(trx_2, LOW);
  pinMode(rcx_1, INPUT);
  pinMode(rcx_2, INPUT);
  Serial.begin(9600);
  Serial.println("Cherokey 4WD + 2 Sens. Ultrasuoni");
}
void avanti(byte sx_vel, byte dx_vel)   // Utilizzo byte 0 <val.< 255
{
  analogWrite(vel_m1, dx_vel);
  digitalWrite(dir_m1, LOW);
  analogWrite(vel_m2, sx_vel);
  digitalWrite(dir_m2, LOW);
  Serial.println("Robot -> Avanti");
}
void indietro(byte sx_vel, byte dx_vel)   
{
  analogWrite(vel_m1, dx_vel);
  digitalWrite(dir_m1, HIGH);
  analogWrite(vel_m2, sx_vel);
  digitalWrite(dir_m2, HIGH);
  Serial.println("Robot -> Indietro");
}
void fermo(byte sx_vel, byte dx_vel)
{
  analogWrite(vel_m1, dx_vel);
  analogWrite(vel_m2, sx_vel);
  Serial.println("Robot -> Fermo");
}
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 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 sensore()
{ //Sensore 1(DX - pin: 8-tx,9-rx):
  digitalWrite(trx_1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trx_1, LOW);
  tempo_1 = pulseIn(rcx_1, HIGH);
  d_1 = tempo_1 / 58;
  Serial.print("T. Rit. Impulso 1:   ");
  Serial.print(tempo_1);
  Serial.print(" ; ");
  if(tempo_1 > 38000)
  {
    Serial.print("Sensore 1 -> Fuori Portata");
  }
  else
  {
    Serial.print(d_1);
    Serial.print("cm -");
  }
  delay(50);
  //Sensore 2(SX - pin: 10-tx, 11-rx):
  digitalWrite(trx_2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trx_2, LOW);
  tempo_2 = pulseIn(rcx_2, HIGH);
  d_2 = tempo_2 / 58;
  Serial.print("T. Rit. Impulso 2:  ");
  Serial.print(tempo_2);
  Serial.print(" ; ");
  if (tempo_2 > 38000)
  {
    Serial.print("Sensore 2 -> Fuori Portata");
  }
  else
  {
    Serial.print(d_2);
    Serial.println("cm");
  }
}
void loop()
{
  sensore();
  d_tot=d_1+d_2;  //Somma sensore 1 + Sensore 2(distanza in centimetri)
  if(d_tot>=50)
  {
    avanti(200,200);
  }
  else
  {
    destra(200,200);
    delay(2000);
    d_dx=d_1+d_2;   //Somma Sensore 1 + Sensore 2 quando robot guarda a destra
    delay(50);
    sinistra(200,200);
    delay(4000);
    d_sx=d_1+d_2;   //Somma Sensore 1 + Sensore 2 quando robot guarda a sinistra
    delay(50);
    d_max=max(d_dx,d_sx);   //Cerca il massimo fra le due somme
    delay(50);
    if(d_max==d_sx)        //Se il max è sinistra allora vai avanti(sono già a sx)
    {
      avanti(200,200);
    }
    else if(d_max==d_dx)       //Se il max è a destra, gira a destra e vai avanti
    {
      destra(200,200);
      delay(4000);
      avanti(200,200);
    }
  }
}

Ho fatto 2 prove:

  1. al posto dell‚Äô ‚Äúelse if‚ÄĚ vi era solo ‚Äúelse‚ÄĚ e il robot sceglieva come direzione sempre destra.
  2. Inserito ‚Äúelse if‚ÄĚ il robot ora sceglie sempre sinistra.
    Chiedo il vostro aiuto :grin:

Grazie mille in anticipo!

Metti una pausa maggiore tra la misura destra e sinistra.

Manda sulla seriale le 2 distanze misurate per capire dove sta il problema.
Ciao Uwe

Ho aumentato il delay a 500, ma non cambia nulla. Ma credo che la lettura della distanza sia corretta, perchè a monitor le distanze sembrano corrette.
Potrebbe essere solo un errore di programmazione nel impartire l'ordine di scelta direzione al robot? O √® meglio fare delle verifiche pi√Ļ approfondite anche sulla lettura?

Devi mettere sensore() prima di sommare i due tempi, lo hai messo solo all'inizio del loop e basta, ma devi richiamare quella funzione se vuoi aggiornare d_1 e d_2

Funziona!!! :slight_smile: Giustamente rimane il vecchio valore, non ci avevo pensato, perchè mi ero concentrato su qualche errore che avevo potuto commettere tra if ed else. Grazie mille!! :smiley: