[gl-como] RoboGrass

[Davide De Prisco]

Ecco il programma scritto in fiera, da sistemare ;-P



//---------------------------------------------------------//
//                                                         //
//Robograss V0.1                                           //
//                                                         //
//Programmed by albertux 2014                              //
//                                                         //
// Listato principale con due sensori SRF05                //
// posti frontalmente e Vnh5019 Motor shield               //
//                                                         //
//---------------------------------------------------------//


#include "DualVNH5019MotorShield.h" // Inizializzazione libreria VNH5019
#include <NewPing.h>
#define trigPinSINISTRO 3 //dove è collegato il pin trigger del sensore 1
#define echoPinSINISTRO 5 //dove è collegato il pin echo del sensore 1
#define trigPinDESTRO 11 //dove è collegato il pin trigger del sensore 2
#define echoPinDESTRO 13 //dove è collegato il pin echo del sensore 2


#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in
centimeters). Maximum sensor distance is rated at 400-500cm.

NewPing sonarDESTRO(trigPinDESTRO, echoPinDESTRO, MAX_DISTANCE); //
NewPing setup of pins and maximum distance.
NewPing sonarSINISTRO(trigPinSINISTRO, echoPinSINISTRO, MAX_DISTANCE);
// NewPing setup of pins and maximum distance.





DualVNH5019MotorShield md; // Setup motor shield with declaration md
void setup()
{

  Serial.begin(9600);
  md.init(); // Inizializzazione Motor Shield VNH5019
}


void svicola_ostacolo( int SINISTRA , int DESTRA )
{
 // md.setBrakes (0,0);
  if ( SINISTRA > DESTRA ) // gira a sinistra
     {
       md.setSpeeds (50, -50); // giro su asse ruote a sinistra
       delay (500); // attivo per 0.5 sec
     }
  else
      {
       md.setSpeeds (-50, 50); // giro su asse ruote a destra
       delay (500); // attivo per 0.5 sec
     }

}


void loop()
{

  //Controllo tensione batteria
  // if (tenisoneBAT < 10 ) //se minore di 10 V
  // {
  //    md.setBrakes (0,0); // fermo i motori
  //    delay (500000); aspetto per non consumare
  //    exit();
  // }


  delay(100);                      // Wait 50ms between pings (about
20 pings/sec). 29ms should be the shortest delay between pings.
  unsigned int DES = sonarDESTRO.ping(); // Send ping, get ping time
in microseconds (uS).
  unsigned DESTRA = DES / US_ROUNDTRIP_CM ;
  Serial.print("Ping: ");
  Serial.print( DESTRA ); // Convert ping time to distance in cm and
print result (0 = outside set distance range)
  Serial.println("cm");


  delay(100);                      // Wait 50ms between pings (about
20 pings/sec). 29ms should be the shortest delay between pings.
  unsigned int SIN = sonarSINISTRO.ping(); // Send ping, get ping time
in microseconds (uS).
  unsigned SINISTRA = SIN / US_ROUNDTRIP_CM ;
  Serial.print("Ping: ");
  Serial.print( SINISTRA ); // Convert ping time to distance in cm and
print result (0 = outside set distance range)
  Serial.println("cm");




  if (( SINISTRA > 50  || SINISTRA  == 0 ) && ( DESTRA > 50 || DESTRA
== 0 )) // non ci sono ostacoli avanti
  {
       md.setSpeeds (60, 40); //motori avanti velocità di taglio
       delay (250);
  }
     else
  {
    // Funzione SVICOLA_OSTACOLO
  svicola_ostacolo( SINISTRA , DESTRA  );
  delay (250);
  }


}



Davide