Bluetooth Controlled Car with Arduino Mega

You can remotely control your car with Bluetooth. Four settings are available: manual, automatic, steering and vocal control.

Feb 23, 2019

•

18766 views

•

19 respects

Components and supplies

4WD-Robot-Smart-Car-Chassis-Kits-car-with-Speed-Encoder

Convertitore DC-DC step-down LM317

12 volt battery 1,3 ah

Motor Encoder RPM Speed Counter Interrupter Sensor Module FC-03

HC-05 Bluetooth Module

Ultrasonic Sensor - HC-SR04 (Generic)

NEO-6M

Jumper wires (generic)

QMC5883L

SG90 Micro-servo motor

Arduino Mega 2560

motor shield L293D

Apps and platforms

MIT App Inventor 2

Project description

Code

Arduino IDE

arduino

1#include <NMEAGPS.h>
2#include <SoftwareWire.h>
3#include <Servo.h>
4#include   <AFMotor.h>
5#include <NewPing.h>
6#define GPSport_h
7#define gpsPort Serial3
8#define   GPS_PORT_NAME "Serial3"
9
10SoftwareWire MyWire( 20, 21);
11#define TRIG_PIN_A   41 
12#define ECHO_PIN_A 40 
13#define ECHO_PIN_R 39
14#define TRIG_PIN_R 38   
15#define MAX_DISTANCE 300
16#define SENSORE_AVANTI 0
17#define SENSORE_INDIETRO   1 
18#define RIDUZIONE_POTENZA_LOW 2
19#define DIR_INDIETRO 0
20#define DIR_AVANTI   1
21#define DIR_STOP 2
22#define DIR_DESTRA 3
23#define DIR_SINISTRA 4
24
25/*   Definizione Comandi ricevuti dall'App */
26#define ARRESTA           48
27#define   AVANTI            49
28#define INDIETRO          50
29#define DESTRA            51
30#define   SINISTRA          52
31#define DESTRA_AVANTI     53
32#define SINISTRA_AVANTI    54
33#define DESTRA_INDIETRO   55
34#define SINISTRA_INDIETRO 56
35#define   ACCELLERA         57
36#define DECELERA          58
37#define AUTOMATICO        59
38#define   STOP              79
39#define POTENZA           80
40#define SETTING           90
41#define   OFFSET_GPS        96
42#define OFFSET_BUSSOLA    97
43#define CALIBRA_GPS       98
44#define   ATTIVA_GPS        99
45#define GPS              100
46
47#define PIN_PRINT_APP      18
48
49NewPing sonar[2] = {
50  NewPing(TRIG_PIN_A, ECHO_PIN_A, MAX_DISTANCE),
51   NewPing(TRIG_PIN_R, ECHO_PIN_R, MAX_DISTANCE)
52  };
53AF_DCMotor motore_anteriore_dx(2);
54AF_DCMotor   motore_anteriore_sx(3);
55AF_DCMotor motore_posteriore_sx(4);
56AF_DCMotor motore_posteriore_dx(1);
57Servo   MyServo;
58Servo MyServo_Info;
59static NMEAGPS  gps; // This parses the GPS characters
60static   gps_fix  fix;
61int direzione; 
62int potenza;
63int bytericevuto = 0;
64String   cPotenza;
65String cDistanza;
66String cVelocita;
67int pin_velocita = 19;
68unsigned   int cm_al_secondo; 
69volatile unsigned int pulses;
70unsigned long timeold;  //   controllo ogni 100mS per print info
71unsigned long timeold1; // controllo ogni   Secondo per l'avanzamento automatico GPS
72unsigned long timeold2; // controllo   ogni 500mS per il calcolo della velocit
73unsigned int pulsesperturn = 20;
74int   Dist_Sinistra      = 0;
75int Dist_Destra        = 0;
76int Dist_Avanti        =   0;
77int Dist_Dietro        = 0;
78int Dist_Sinistra_Diag = 0;
79int Dist_Destra_Diag    = 0;
80boolean DestraSinistra = false;
81int Gradi              = 0;
82int   nX;
83int Distanza_Frontale  = 30;
84int Distanza_Laterale  = 20;
85int Distanza_Minima     = 50;
86int Potenza_Automatico = 65;
87int Potenza_Minima     = 50;
88int   Accelera_Decelera  = 5;
89int offset             = 5;
90int Tempo_Rotazione    =   200;
91int variabile          = 0;
92String dataingps       = "";
93long latitudine_car   =  0;
94long longitudine_car = 0;
95long latitudine_tablet = 0;
96long longitudine_tablet   = 0;
97long distanza_tc = 0;
98double angolo_tc = 0;
99String cLatitudine = "";
100String   cLongitudine = "";
101String cAngolo = "";
102String cDistanza_tc = "";
103int   Angolo;
104boolean Attivo_GPS = false;
105int offset_bussola = 0;
106long offset_lat   = 0;
107long offset_long = 0;
108static double xlow  = 0;
109static double ylow   = 0;
110static double xhigh = 0;
111static double yhigh = 0;
112
113/* ----------   Interupt1----------------- */
114void counter()  {
115  pulses++;
116}
117
118/*   ---------- Setup -------------------- */
119void setup() {
120  MyWire.begin();
121   QMC5883L_Configura();
122  Arresta();
123  int Distanza = 0;
124  Serial.begin(115200);      // seriale utilizzata per il debug      
125  Serial2.begin(115200);    //   seriale utilizzata per la trasmissione all'applicazione android
126  gpsPort.begin(115200);       // seriale utilizzata per il colloquio con GPS
127  potenza = Potenza_Minima;
128   motore_posteriore_dx.setSpeed(potenza);
129  motore_posteriore_sx.setSpeed(potenza);
130   motore_anteriore_sx. setSpeed(potenza);
131  motore_anteriore_dx. setSpeed(potenza);
132
133   motore_posteriore_dx.run(RELEASE);
134  motore_posteriore_sx.run(RELEASE);
135   motore_anteriore_sx. run(RELEASE);
136  motore_anteriore_dx. run(RELEASE);
137   
138  // interupt utilizzato per calcolare la velocit
139  pinMode(pin_velocita,   INPUT);
140  digitalWrite(pin_velocita, HIGH);
141  attachInterrupt(digitalPinToInterrupt(pin_velocita),   counter, FALLING);
142  pulses = 0;
143  
144  // interupt utilizzato per inviare   le informazioni all'APP sul tablet
145  pinMode(PIN_PRINT_APP, INPUT);
146  digitalWrite(PIN_PRINT_APP,   HIGH);
147  attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP), Print_Info, FALLING);
148
149   cm_al_secondo = 0;
150  timeold = 0;
151  timeold2 = 0;
152  
153  MyServo_Info.attach(9);
154   MyServo_Info.write(90);
155  
156  MyServo.attach(10);
157  MyServoWriteNew(20);
158   MyServoWriteNew(160);
159  MyServoWriteNew(90);
160}
161
162/* ---------- Loop   Principale ---------- */
163void loop() {
164  bytericevuto = 0;
165  int appoggio;
166   if (Serial2.available() > 0) { bytericevuto = Serial2.read();   }
167  else                         {   delay(10);                       }
168  
169  Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
170   if (Dist_Avanti < Distanza_Minima && (direzione == DIR_AVANTI)) {
171   if (potenza   > Potenza_Minima) {
172    if (potenza > 200)      { potenza *= 0.50; }
173    else   if (potenza > 150) { potenza *= 0.65; }
174    else                    { potenza   *= 0.80; }
175    Potenza(potenza);
176    }
177    if (Dist_Avanti <= 20) {
178       Indietro();
179      delay(20);
180      Arresta(); 
181      
182    }      
183   }
184    
185  if ( direzione == DIR_INDIETRO ) {
186    Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
187     if (Dist_Dietro < Distanza_Minima) {
188      if (potenza > 200)      { potenza   *= 0.50; }
189      else if (potenza > 150) { potenza *= 0.65; }
190      else                    {   potenza *= 0.80; }
191      Potenza(potenza);
192      }
193    if (Dist_Dietro   <= 20) {
194      Avanti();
195      delay(20);
196      Arresta();
197      Potenza(Potenza_Minima);   
198    } 
199  }
200    
201  switch (bytericevuto) {
202    case ARRESTA:
203       Arresta();
204      break;
205    case AVANTI:
206      Avanti();
207      break;
208     case INDIETRO:
209      Indietro(); 
210      break;
211    case DESTRA:
212       Destra();
213      break;
214    case SINISTRA:
215      Sinistra();
216      break;
217     case DESTRA_AVANTI:
218      Destra_Avanti();
219      break;
220    case SINISTRA_AVANTI:
221       Sinistra_Avanti();
222      break;
223    case DESTRA_INDIETRO:
224      Destra_Indietro();
225       break;
226    case SINISTRA_INDIETRO:
227      Sinistra_Indietro();
228      break;
229     case ACCELLERA:
230      Accelera();
231      break;
232    case DECELERA:
233       Decelera();
234      break;
235    case AUTOMATICO:
236      Automatico();
237       break;    
238    case STOP:
239      Arresta();
240      Potenza(0);
241       break;
242    case POTENZA:
243      while (Serial2.available() < 1 ){ delay(10);   }
244      potenza = Serial2.read();
245      Potenza(potenza);
246      break;
247     case SETTING:
248      Impostazioni();
249      break;
250    case OFFSET_GPS:
251       offset_lat  = (latitudine_car-latitudine_tablet);
252      offset_long = (longitudine_car-longitudine_tablet);
253       break;
254    case OFFSET_BUSSOLA:
255      offset_bussola = 0;
256      offset_bussola   = QMC5883L_Angolo();
257      break;
258    case CALIBRA_GPS:
259      QMC5883L_Calibrazione();
260       break;
261    case ATTIVA_GPS:
262      while (Serial2.available() < 1 ){   delay(10); }
263      if (Serial2.peek() == 1 || Serial2.peek() == 2) {
264         appoggio   = Serial2.read();
265         if      ( appoggio == 1) { Attivo_GPS = true ;  }   
266         else if ( appoggio == 2) { Attivo_GPS = false ; } 
267      }     
268       break;    
269    case GPS:
270       Gps();
271       break;
272    default:
273       break;
274   }        
275}
276
277/* ---------- Avanti -------------------   */
278void Avanti(){
279  if ( potenza < Potenza_Minima) {potenza = Potenza_Minima;   }
280  
281  Potenza(potenza);
282  motore_posteriore_dx.run(FORWARD);
283  motore_posteriore_sx.run(FORWARD);
284   motore_anteriore_sx. run(FORWARD);
285  motore_anteriore_dx. run(FORWARD);
286   direzione = DIR_AVANTI;
287}
288
289/* ---------- Indietro ----------------- */
290void   Indietro() {
291  if ( potenza < Potenza_Minima) {potenza = Potenza_Minima;  }
292   
293  Potenza(potenza);
294  motore_posteriore_dx.run(BACKWARD);
295  motore_posteriore_sx.run(BACKWARD);
296   motore_anteriore_sx. run(BACKWARD);
297  motore_anteriore_dx. run(BACKWARD);
298   direzione = DIR_INDIETRO;
299}
300
301/* ---------- Arresta ------------------   */
302void Arresta() {
303  motore_posteriore_dx.run(RELEASE);
304  motore_posteriore_sx.run(RELEASE);
305   motore_anteriore_sx. run(RELEASE);
306  motore_anteriore_dx. run(RELEASE);
307   Potenza(potenza);
308  direzione = DIR_STOP;
309}
310
311/* ---------- Potenza   ------------------ */
312void Potenza(int Speed) {  
313  motore_posteriore_dx.setSpeed(Speed);
314   motore_posteriore_sx.setSpeed(Speed);
315  motore_anteriore_sx. setSpeed(Speed);
316   motore_anteriore_dx. setSpeed(Speed); 
317  potenza = Speed;
318}
319
320/* ----------   Destra ------------------- */
321void Destra() {
322  motore_posteriore_dx.run(BACKWARD);
323   motore_posteriore_sx.run(FORWARD);
324  motore_anteriore_sx. run(FORWARD);
325   motore_anteriore_dx. run(BACKWARD); 
326  direzione = DIR_DESTRA;
327}
328
329/*   ---------- Sinistra ----------------- */
330void Sinistra() {
331  motore_posteriore_dx.run(FORWARD   );
332  motore_posteriore_sx.run(BACKWARD);
333  motore_anteriore_sx. run(BACKWARD);
334   motore_anteriore_dx. run(FORWARD );
335  direzione = DIR_SINISTRA;  
336}
337
338/*   ---------- Destra Avanti ------------ */
339void Destra_Avanti() {
340  motore_posteriore_dx.run(FORWARD);
341   motore_posteriore_sx.run(FORWARD);
342  motore_anteriore_sx. run(FORWARD);
343   motore_anteriore_dx. run(FORWARD);
344  motore_anteriore_dx. setSpeed(potenza   / RIDUZIONE_POTENZA_LOW);
345  motore_posteriore_dx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
346   motore_anteriore_sx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
347  motore_posteriore_sx.setSpeed(potenza   * RIDUZIONE_POTENZA_LOW);
348  direzione = DIR_AVANTI;
349  }
350
351/* ----------   Sinistra Avanti ---------- */
352void Sinistra_Avanti() {
353  motore_posteriore_dx.run(FORWARD);
354   motore_posteriore_sx.run(FORWARD);
355  motore_anteriore_sx. run(FORWARD);
356   motore_anteriore_dx. run(FORWARD);
357  motore_anteriore_sx. setSpeed(potenza   / RIDUZIONE_POTENZA_LOW);
358  motore_posteriore_sx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
359   motore_anteriore_dx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
360  motore_posteriore_dx.setSpeed(potenza   * RIDUZIONE_POTENZA_LOW);
361  direzione = DIR_AVANTI;
362}
363
364/* ----------   Destra Indietro ---------- */
365void Destra_Indietro() {
366  motore_posteriore_dx.run(BACKWARD);
367   motore_posteriore_sx.run(BACKWARD);
368  motore_anteriore_sx. run(BACKWARD);
369   motore_anteriore_dx. run(BACKWARD);
370  motore_anteriore_dx. setSpeed(potenza   / RIDUZIONE_POTENZA_LOW);
371  motore_posteriore_dx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
372   motore_anteriore_sx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
373  motore_posteriore_sx.setSpeed(potenza   * RIDUZIONE_POTENZA_LOW);
374  direzione = DIR_INDIETRO;
375}
376
377/* ----------   Sinistra Indietro -------- */
378void Sinistra_Indietro() {
379  motore_posteriore_dx.run(BACKWARD);
380   motore_posteriore_sx.run(BACKWARD);
381  motore_anteriore_sx. run(BACKWARD);
382   motore_anteriore_dx. run(BACKWARD);
383  motore_anteriore_sx. setSpeed(potenza   / RIDUZIONE_POTENZA_LOW);
384  motore_posteriore_sx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
385   motore_anteriore_dx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
386  motore_posteriore_dx.setSpeed(potenza   * RIDUZIONE_POTENZA_LOW);
387  direzione = DIR_INDIETRO;
388}
389 
390
391/* ----------   Accellera ---------------- */
392void Accelera() { 
393  potenza += Accelera_Decelera;
394   if ( potenza > 250) { potenza = 250;  }
395  
396  Potenza(potenza);
397}
398   
399/* ---------- Decelera ----------------- */
400void Decelera() { 
401  potenza   -= Accelera_Decelera;
402  if ( potenza < 0) { potenza = 0;  }
403  
404  Potenza(potenza);
405}
406
407/*   ---------- Misura distanza ---------- */
408int Misura_Distanza(int a_r) {
409  int   distanza_cm;
410  distanza_cm = sonar[a_r].ping_cm();
411  if ( distanza_cm <= 0   ) { distanza_cm = MAX_DISTANCE;  }
412  return distanza_cm;
413}
414
415/* ----------   Impostazioni ---------- */
416void Impostazioni() {
417      while (Serial2.available()   < 1 ){ delay(10); }
418      Distanza_Frontale  = Serial2.read();
419      while   (Serial2.available() < 1 ){ delay(10); }
420      Distanza_Laterale  = Serial2.read();
421       while (Serial2.available() < 1 ){ delay(10); }
422      Distanza_Minima    =   Serial2.read();
423      while (Serial2.available() < 1 ){ delay(10); }
424      Potenza_Automatico   = Serial2.read();
425      while (Serial2.available() < 1 ){ delay(10); }
426      Potenza_Minima      = Serial2.read();
427      while (Serial2.available() < 1 ){ delay(10); }
428       Accelera_Decelera  = Serial2.read();
429      while (Serial2.available() <   1 ){ delay(10); }
430      offset             = (Serial2.read() - 90);
431      while   (Serial2.available() < 1 ){ delay(10); }
432      Tempo_Rotazione    = (Serial2.read()*10);
433       MyServoWriteNew(90);
434  }
435
436
437/* ---------- Avanzamento Automatico   --- */
438int Automatico() {
439int returnvalue = 0;
440
441  while (returnvalue   != AUTOMATICO ) {
442    Gradi = 0;
443    if (Serial2.available() > 0){ returnvalue   = Serial2.read();}
444    Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
445    while   ( Dist_Avanti > Distanza_Frontale  && returnvalue != AUTOMATICO ){  
446      if   (Serial2.available() > 0){ returnvalue = Serial2.read();}
447      // guarda a destra   e sinistra
448      if   (DestraSinistra) { Gradi++; }
449      // guarda a sinistra
450       else                  { Gradi--; }
451      
452      MyServo.write(90+Gradi+offset);
453       if (abs(Gradi) > 30) { DestraSinistra = !DestraSinistra;}
454      Avanti();
455       Potenza(Potenza_Automatico);
456      delay(10);        
457      Dist_Avanti   = Misura_Distanza(SENSORE_AVANTI);
458    }
459    Arresta();
460    if (returnvalue   == AUTOMATICO) {break;}
461    Distanza_Ostacolo();
462    // nel caso viene chiuso   su tre lati va indietro
463    if  ( Dist_Destra   < Distanza_Laterale && Dist_Destra_Diag    < Distanza_Frontale &&
464          Dist_Sinistra < Distanza_Laterale && Dist_Sinistra_Diag   < Distanza_Frontale &&
465          Dist_Avanti   < Distanza_Frontale && (Dist_Dietro   > 25)) { 
466      DestraSinistra = false;
467      Gradi = 0;
468      while (   Dist_Avanti < Distanza_Frontale*2 ){  
469        if (DestraSinistra){  Gradi +=   2;  }
470        else               {  Gradi -= 2;  }
471        MyServoWriteNew(90+Gradi);
472         if (abs(Gradi) > 70) {
473          Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
474           if (Dist_Avanti > Distanza_Laterale*2)  {
475            Potenza(Potenza_Automatico*3);
476             if ( Gradi > 0 ) { Sinistra();        }
477            else             {   Destra();          }  
478            delay(Tempo_Rotazione); 
479            break;
480           }
481          DestraSinistra = !DestraSinistra;
482        }
483        Potenza(Potenza_Minima);
484         Indietro();
485        delay(10);        
486        Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
487         if (Dist_Dietro < 10) {  break;}
488      }
489      MyServoWriteNew(90);
490       }
491          
492    // decide se girare a sinistra o destra
493    else   if  (Dist_Sinistra < Dist_Destra   && Dist_Sinistra < Dist_Avanti) {
494        Potenza(Potenza_Automatico*3);
495         Destra(); 
496        delay(Tempo_Rotazione*0.75);
497     }
498    else   if (Dist_Destra < Dist_Sinistra && Dist_Destra   < Dist_Avanti) {
499      Potenza(Potenza_Automatico*3);
500       Sinistra();
501      delay(Tempo_Rotazione*0.75);
502      }
503    else if   (Dist_Destra        < Distanza_Laterale)                     {
504      Potenza(Potenza_Automatico*3);
505       Sinistra();
506      delay(Tempo_Rotazione);
507      }
508    else if (Dist_Sinistra       < Distanza_Laterale)                     {
509      Potenza(Potenza_Automatico*3);
510       Destra();
511      delay(Tempo_Rotazione);
512      } 
513    else if (Dist_Sinistra_Diag   < Distanza_Frontale)                     {
514      Potenza(Potenza_Automatico*3);
515       Destra();
516      delay(Tempo_Rotazione);
517      } 
518    else if (Dist_Destra_Diag    < Distanza_Frontale)                     {
519      Potenza(Potenza_Automatico*3);
520       Sinistra();
521      delay(Tempo_Rotazione);
522      }
523    else if (Dist_Destra>Dist_Sinistra   && Dist_Destra>50)                {
524      Potenza(Potenza_Automatico*3);
525       Destra();
526      delay(Tempo_Rotazione); }
527    else if (Dist_Sinistra>Dist_Destra   && Dist_Sinistra>50)              {
528      Potenza(Potenza_Automatico*3);
529       Sinistra();
530      delay(Tempo_Rotazione); }
531      
532    else if (Dist_Avanti        <= Distanza_Frontale)                     {  
533      DestraSinistra = false;
534       Gradi = 0;
535      while ( Dist_Avanti < Distanza_Frontale*2 ){  
536        if   (DestraSinistra){  Gradi += 2;  }
537        else               {  Gradi -= 2;  }
538         MyServoWriteNew(90+Gradi);
539        if (abs(Gradi) > 70) {
540          Dist_Avanti   = Misura_Distanza(SENSORE_AVANTI);
541          if (Dist_Avanti > Distanza_Laterale*2)   {
542            Potenza(Potenza_Automatico*3);
543            if ( Gradi > 0   ) { Sinistra();        }
544            else             { Destra();          }   
545            delay(Tempo_Rotazione); 
546            break;
547          }
548           DestraSinistra = !DestraSinistra;
549        }
550        Potenza(Potenza_Minima);
551         Indietro();
552        delay(10);        
553        Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
554         if (Dist_Dietro < 10) {  break;}
555      }
556      MyServoWriteNew(90);
557     }
558  }
559  MyServoWriteNew(90);
560  Arresta();
561}
562
563/* ----------   Distanza Ostacolo -------- */
564void Distanza_Ostacolo()
565{ 
566  //Misura la   distanza degli ostacoli, destra, sinistra, destra diagonale, sinistra diagonale,   Avanti e dietro. 
567  // Dist_Sinistra, Dist_Destra, Dist_Avanti; Dist_Dietro,   Dist_Sinistra_Diag, Dist_Destra_Diag
568  detachInterrupt(digitalPinToInterrupt(pin_velocita));
569   detachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP));
570  Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
571   
572  MyServoWriteNew(130);
573  Dist_Sinistra_Diag = Misura_Distanza(SENSORE_AVANTI);
574   
575  MyServoWriteNew(170);
576  Dist_Sinistra = Misura_Distanza(SENSORE_AVANTI);
577
578   MyServoWriteNew(50);
579  Dist_Destra_Diag = Misura_Distanza(SENSORE_AVANTI);
580   
581  MyServoWriteNew(10);
582  Dist_Destra = Misura_Distanza(SENSORE_AVANTI);
583   
584  MyServoWriteNew(90);
585  Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
586   attachInterrupt(digitalPinToInterrupt(pin_velocita), counter, FALLING);
587  attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP),   Print_Info, FALLING);
588}
589
590/* ---------- Stampa informazioni in APP --------   */
591 void Print_Info() {
592    if (millis() - timeold2 >= 500) {
593      detachInterrupt(digitalPinToInterrupt(pin_velocita));
594       detachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP));
595      cm_al_secondo   = pulses*2/((millis() - timeold2)/500);
596      pulses = 0; 
597      timeold2   = millis();
598      
599      if (direzione != DIR_STOP && cm_al_secondo <= 5)   {
600        variabile += 1;
601        if(variabile >= 10){
602          variabile   = 0;
603          if (bytericevuto == 58) {Distanza_Ostacolo();}
604          else   {Arresta();}
605       }
606      }
607      else {variabile = 0;}
608      attachInterrupt(digitalPinToInterrupt(pin_velocita),   counter, FALLING);
609      attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP),   Print_Info, FALLING);
610    }
611    if (millis() - timeold >= 100) {
612       detachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP));
613        detachInterrupt(digitalPinToInterrupt(pin_velocita));
614      if (direzione   == DIR_INDIETRO) { cDistanza = String(Dist_Dietro);  }     
615      else                           {   cDistanza = String(Dist_Avanti);  }
616      
617      while (cDistanza.length()   < 3) {  cDistanza = " " + cDistanza;     }
618      Serial2.print("D"+cDistanza);
619       
620      cPotenza = String( map(potenza, 0, 250, 0, 100));
621      while   (cPotenza.length() < 3 ) {  cPotenza = " " + cPotenza;       }     
622      Serial2.print("P"   + cPotenza);
623      
624      cVelocita = String(cm_al_secondo);
625      while   (cVelocita.length() < 3) {  cVelocita = " " + cVelocita;     }      
626      Serial2.print("V"   + cVelocita);
627
628      if ( Attivo_GPS ) {
629         while (gps.available(gpsPort)   ) {
630           fix = gps.read(); // save the latest
631           if (fix.valid.location)   {
632             latitudine_car = fix.latitudeL();
633             longitudine_car   = fix.longitudeL();   
634           }      
635         }
636         String cAngolo_tc   = String(angolo_tc,0);
637         while (cAngolo_tc.length() < 3 ) {  cAngolo_tc   = " " + cAngolo_tc; } 
638         Serial2.print("C" + cAngolo_tc);
639         
640          //Angolo = QMC5883L_Angolo();
641         cAngolo = String(Angolo);
642          while (cAngolo.length() < 3 ) {  cAngolo = " " + cAngolo; } 
643         Serial2.print("A"   + cAngolo);
644         
645         cLatitudine = String(latitudine_car);
646         while   (cLatitudine.length() < 10) {  cLatitudine = " " + cLatitudine; } 
647         Serial2.print("L"   + cLatitudine);
648      
649         cLongitudine = String(longitudine_car);
650          while (cLongitudine.length() < 10) {  cLongitudine = " " + cLongitudine;   } 
651         Serial2.print("G" + cLongitudine);
652
653         cDistanza_tc   = String(distanza_tc);
654         while (cDistanza_tc.length() < 10 ) {  cDistanza_tc   = " " + cDistanza_tc; } 
655         Serial2.print("M" + cDistanza_tc);
656       }
657      timeold = millis();
658      attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP),   Print_Info, FALLING);
659      attachInterrupt(digitalPinToInterrupt(pin_velocita),   counter, FALLING);
660    }
661 }
662
663 /* ---------- Avanzamento Graduale Servo   ------------------ */
664void MyServoWriteNew(int Gradi) {  
665  int oldposition   = MyServo.read();
666  if (oldposition > Gradi) {
667    for (int i = oldposition;   i > Gradi; i -=1) {
668      MyServo.write(i+offset);
669      delay(3);
670    }
671   } 
672  else if (oldposition < Gradi) {
673    for (int i = oldposition; i < Gradi;   i +=1) {
674      MyServo.write(i+offset);
675      delay(3);
676    }
677  }
678   MyServo.write(Gradi+offset);
679}
680
681
682 /* ---------- Configura sensore   QMC5883L  ------------------ */ 
683 void  QMC5883L_Configura(){
684   MyWire.beginTransmission(0x0D);
685    MyWire.write(0x0B);
686   MyWire.write(0x01);
687   MyWire.endTransmission();   
688   MyWire.beginTransmission(0x0D);
689   MyWire.write(0x09);
690   MyWire.write(0b11000000|0b00000000|0b00000100|0b00000001);
691    MyWire.endTransmission();
692 }
693
694 /* ---------- Calibrazione sensore QMC5883L   ------------------ */ 
695void QMC5883L_Calibrazione () {
696  int returnvalue   = 0;
697  xlow = ylow = xhigh = yhigh = 0;
698  unsigned long timeold = millis()+20000;
699   Potenza(Potenza_Automatico*2);
700    
701  while ( (millis() < timeold ) && (returnvalue   != ARRESTA ))  {    
702       QMC5883L_Angolo(); 
703       Destra();
704       delay(40);   
705       if (Serial2.available() > 0){ returnvalue = Serial2.read();} 
706    }
707     Arresta();
708}
709
710 /* ---------- Lettura sensore QMC5883L  ------------------   */ 
711int QMC5883L_Angolo () {
712  int nX,nY,nZ;
713  float fx,fy;
714  
715  Arresta();
716   delay(10);
717  MyWire.beginTransmission(0x0D);
718  MyWire.write(0x00);
719  MyWire.endTransmission();
720   MyWire.requestFrom(0x0D, 6);
721  nX = MyWire.read() | (MyWire.read()<<8);
722   nY = MyWire.read() | (MyWire.read()<<8);
723  nZ = MyWire.read() | (MyWire.read()<<8);
724   MyWire.endTransmission();
725  
726  if(nX<xlow)  xlow  += (nX - xlow )*0.2;
727   if(nX>xhigh) xhigh += (nX - xhigh)*0.2;
728  if(nY<ylow)  ylow  += (nY - ylow   )*0.2;
729  if(nY>yhigh) yhigh += (nY - yhigh)*0.2; 
730 
731  nX -= (xhigh+xlow)/2;
732   nY -= (yhigh+ylow)/2;
733  fx = (float)nX/(xhigh-xlow);
734  fy = (float)nY/(yhigh-ylow);
735   
736  Angolo = (3.81667f + 180.0*atan2(fy,fx)/3.14159265358979323846264338327950288)-offset_bussola;
737   if(Angolo<=0) Angolo += 360;
738  if(Angolo>=360) Angolo -= 360;
739  return Angolo;
740}
741
742/*   ----------  GPS  ------------------ */ 
743
744void Gps () {
745  int returnvalue   = 0;
746  while (Serial2.available() < 1 ){ delay(10); }
747  dataingps   = ""   ;
748  dataingps   = Serial2.readString();  
749  latitudine_tablet  = (dataingps.substring(dataingps.indexOf("LAT")+3,   dataingps.indexOf("LONG")).toInt())+offset_lat;
750  longitudine_tablet = (dataingps.substring(dataingps.indexOf("LONG")+4,   dataingps.length()).toInt())+offset_long;
751  NeoGPS::Location_t tablet( latitudine_tablet,   longitudine_tablet );
752  NeoGPS::Location_t car   ( latitudine_car,    longitudine_car     );
753  distanza_tc =  fix.location.DistanceKm      ( car, tablet );    
754   angolo_tc   =  fix.location.BearingToDegrees( car, tablet );
755  
756  while   (returnvalue != ARRESTA ) {
757    if (Serial2.available() > 0){ returnvalue = Serial2.read();}
758     Potenza(Potenza_Automatico*2);
759    while (!(((Angolo+10)>=angolo_tc) && ((Angolo-10)<=angolo_tc))   && (returnvalue != ARRESTA) ) { 
760      if (Serial2.available() > 0){ returnvalue   = Serial2.read();}
761      QMC5883L_Angolo();
762      if(((Angolo-angolo_tc)<=   180) && ((Angolo-angolo_tc) >= 0)){Sinistra();}
763      else {Destra();}
764      delay(40);
765     }
766    if (distanza_tc<=1){
767      Arresta();
768      returnvalue = ARRESTA;
769     }
770    else {
771      returnvalue=Automatico_GPS();
772      if (distanza_tc<=1){
773         Arresta();
774        returnvalue = ARRESTA;
775      }
776    }
777  }
778}
779
780   /* ---------- Avanzamento Automatico --- */
781int Automatico_GPS() {
782int returnvalue   = 0;
783
784    Gradi = 0;
785    Potenza(Potenza_Automatico);
786    Dist_Avanti   = Misura_Distanza(SENSORE_AVANTI);
787    while ( (Dist_Avanti > Distanza_Frontale)   && (returnvalue != ARRESTA) && (distanza_tc>1) ){  
788      if (Serial2.available()   > 0){ returnvalue = Serial2.read();}
789      NeoGPS::Location_t tablet( latitudine_tablet,   longitudine_tablet );
790      NeoGPS::Location_t car   ( latitudine_car,    longitudine_car     );
791      distanza_tc =  fix.location.DistanceKm      ( car, tablet );    
792       angolo_tc   =  fix.location.BearingToDegrees( car, tablet );
793      // guarda   a destra e sinistra
794      if   (DestraSinistra) { Gradi++; }
795      // guarda   a sinistra
796      else                  { Gradi--; }
797      
798      MyServo.write(90+Gradi+offset);
799       if (abs(Gradi) > 30) { DestraSinistra = !DestraSinistra;}
800      Avanti();
801       Potenza(Potenza_Automatico);
802      delay(10);        
803      Dist_Avanti   = Misura_Distanza(SENSORE_AVANTI);
804    }
805    Arresta();
806    if ((returnvalue   != ARRESTA) && (distanza_tc>1)) {
807      Distanza_Ostacolo();
808      
809      //   nel caso viene chiuso su tre lati va indietro
810      if  ( Dist_Destra   < Distanza_Laterale   && Dist_Destra_Diag   < Distanza_Frontale &&
811            Dist_Sinistra < Distanza_Laterale   && Dist_Sinistra_Diag < Distanza_Frontale &&
812            Dist_Avanti   < Distanza_Frontale   && (Dist_Dietro > 25)) { 
813        DestraSinistra = false;
814        Gradi =   0;
815        while ( Dist_Avanti < Distanza_Frontale*2 ){  
816          if (DestraSinistra){   Gradi += 2;  }
817          else               {  Gradi -= 2;  }
818          MyServoWriteNew(90+Gradi);
819           if (abs(Gradi) > 70) {
820            Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
821             if (Dist_Avanti > Distanza_Laterale*2)  {
822              Potenza(Potenza_Automatico*3);
823               if ( Gradi > 0 ) { Sinistra();        }
824              else             {   Destra();          }  
825              delay(Tempo_Rotazione); 
826              break;
827             }
828            DestraSinistra = !DestraSinistra;
829          }
830           Potenza(Potenza_Minima);
831          Indietro();
832          delay(10);         
833          Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
834          if   (Dist_Dietro < 10) {  break;}
835        }
836        MyServoWriteNew(90);
837      }
838             
839      // decide se girare a sinistra o destra
840      else if  (Dist_Sinistra   < Dist_Destra   && Dist_Sinistra < Dist_Avanti) {
841          Potenza(Potenza_Automatico*3);
842           Destra(); 
843          delay(Tempo_Rotazione*0.75);
844       }
845      else   if (Dist_Destra < Dist_Sinistra && Dist_Destra   < Dist_Avanti) {
846        Potenza(Potenza_Automatico*3);
847         Sinistra();
848        delay(Tempo_Rotazione*0.75);
849        }
850      else   if (Dist_Destra        < Distanza_Laterale)                     {
851        Potenza(Potenza_Automatico*3);
852         Sinistra();
853        delay(Tempo_Rotazione);
854        }
855      else   if (Dist_Sinistra      < Distanza_Laterale)                     {
856        Potenza(Potenza_Automatico*3);
857         Destra();
858        delay(Tempo_Rotazione);
859        } 
860      else   if (Dist_Sinistra_Diag < Distanza_Frontale)                     {
861        Potenza(Potenza_Automatico*3);
862         Destra();
863        delay(Tempo_Rotazione);
864        } 
865      else   if (Dist_Destra_Diag   < Distanza_Frontale)                     {
866        Potenza(Potenza_Automatico*3);
867         Sinistra();
868        delay(Tempo_Rotazione);
869        }
870      else   if (Dist_Destra>Dist_Sinistra && Dist_Destra>50)                {
871        Potenza(Potenza_Automatico*3);
872         Destra();
873        delay(Tempo_Rotazione); }
874      else if (Dist_Sinistra>Dist_Destra   && Dist_Sinistra>50)              {
875        Potenza(Potenza_Automatico*3);
876         Sinistra();
877        delay(Tempo_Rotazione); }
878        
879      else   if (Dist_Avanti       <= Distanza_Frontale)                     {  
880        DestraSinistra   = false;
881        Gradi = 0;
882        while ( Dist_Avanti < Distanza_Frontale*2   ){  
883          if (DestraSinistra){  Gradi += 2;  }
884          else               {   Gradi -= 2;  }
885          MyServoWriteNew(90+Gradi);
886          if (abs(Gradi)   > 70) {
887            Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
888            if   (Dist_Avanti > Distanza_Laterale*2)  {
889              Potenza(Potenza_Automatico*3);
890               if ( Gradi > 0 ) { Sinistra();        }
891              else             {   Destra();          }  
892              delay(Tempo_Rotazione); 
893              break;
894             }
895            DestraSinistra = !DestraSinistra;
896          }
897           Potenza(Potenza_Minima);
898          Indietro();
899          delay(10);         
900          Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
901          if   (Dist_Dietro < 10) {  break;}
902        }
903     }
904  motore_posteriore_dx.run(RELEASE);
905   motore_posteriore_sx.run(RELEASE);
906  motore_anteriore_sx. run(RELEASE);
907   motore_anteriore_dx. run(RELEASE);
908  Potenza(potenza);
909  Distanza_Ostacolo();
910   if (direzione == DIR_DESTRA){
911    while(Dist_Sinistra < Distanza_Laterale*3){
912       Potenza(Potenza_Automatico);
913      Avanti();
914      MyServoWriteNew(170);
915       Dist_Sinistra = Misura_Distanza(SENSORE_AVANTI);
916      if( Dist_Sinistra   < Distanza_Laterale) {
917        Potenza(Potenza_Automatico*3);
918        Destra();
919         delay(50);
920        Arresta();
921        }
922      MyServoWriteNew(90);
923       Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
924      if (Dist_Avanti <   Distanza_Frontale){
925        Potenza(Potenza_Automatico*3);
926        Destra();   
927        delay(Tempo_Rotazione*0.75);
928        }
929    }
930    Potenza(Potenza_Automatico*3);
931     Sinistra();
932    delay(Tempo_Rotazione);
933    Potenza(Potenza_Automatico);
934     Avanti();
935    delay(Tempo_Rotazione*2);
936  }
937  else if(direzione ==   DIR_SINISTRA){
938    while(Dist_Destra < Distanza_Laterale*3){
939      Potenza(Potenza_Automatico);
940       Avanti();
941      MyServoWriteNew(10);
942      Dist_Destra = Misura_Distanza(SENSORE_AVANTI);
943       if( Dist_Destra < Distanza_Laterale) {
944        Potenza(Potenza_Automatico*3);
945         Sinistra();
946        delay(50);
947        Arresta();
948        }
949       MyServoWriteNew(90);
950      Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
951       if (Dist_Avanti < Distanza_Frontale){
952        Potenza(Potenza_Automatico*3);
953         Sinistra(); 
954        delay(Tempo_Rotazione*0.75);
955        }
956    }   
957    Potenza(Potenza_Automatico*3);
958    Destra();
959    delay(Tempo_Rotazione);
960     Potenza(Potenza_Automatico);
961    Avanti();
962    delay(Tempo_Rotazione*2);
963     }
964  
965  }
966  MyServoWriteNew(90);
967  QMC5883L_Angolo();
968  return   returnvalue ; 
969}
970

#include <NMEAGPS.h>
#include <SoftwareWire.h>
#include <Servo.h>
#include   <AFMotor.h>
#include <NewPing.h>
#define GPSport_h
#define gpsPort Serial3
#define   GPS_PORT_NAME "Serial3"

SoftwareWire MyWire( 20, 21);
#define TRIG_PIN_A   41 
#define ECHO_PIN_A 40 
#define ECHO_PIN_R 39
#define TRIG_PIN_R 38   
#define MAX_DISTANCE 300
#define SENSORE_AVANTI 0
#define SENSORE_INDIETRO   1 
#define RIDUZIONE_POTENZA_LOW 2
#define DIR_INDIETRO 0
#define DIR_AVANTI   1
#define DIR_STOP 2
#define DIR_DESTRA 3
#define DIR_SINISTRA 4

/*   Definizione Comandi ricevuti dall'App */
#define ARRESTA           48
#define   AVANTI            49
#define INDIETRO          50
#define DESTRA            51
#define   SINISTRA          52
#define DESTRA_AVANTI     53
#define SINISTRA_AVANTI    54
#define DESTRA_INDIETRO   55
#define SINISTRA_INDIETRO 56
#define   ACCELLERA         57
#define DECELERA          58
#define AUTOMATICO        59
#define   STOP              79
#define POTENZA           80
#define SETTING           90
#define   OFFSET_GPS        96
#define OFFSET_BUSSOLA    97
#define CALIBRA_GPS       98
#define   ATTIVA_GPS        99
#define GPS              100

#define PIN_PRINT_APP      18

NewPing sonar[2] = {
  NewPing(TRIG_PIN_A, ECHO_PIN_A, MAX_DISTANCE),
   NewPing(TRIG_PIN_R, ECHO_PIN_R, MAX_DISTANCE)
  };
AF_DCMotor motore_anteriore_dx(2);
AF_DCMotor   motore_anteriore_sx(3);
AF_DCMotor motore_posteriore_sx(4);
AF_DCMotor motore_posteriore_dx(1);
Servo   MyServo;
Servo MyServo_Info;
static NMEAGPS  gps; // This parses the GPS characters
static   gps_fix  fix;
int direzione; 
int potenza;
int bytericevuto = 0;
String   cPotenza;
String cDistanza;
String cVelocita;
int pin_velocita = 19;
unsigned   int cm_al_secondo; 
volatile unsigned int pulses;
unsigned long timeold;  //   controllo ogni 100mS per print info
unsigned long timeold1; // controllo ogni   Secondo per l'avanzamento automatico GPS
unsigned long timeold2; // controllo   ogni 500mS per il calcolo della velocit
unsigned int pulsesperturn = 20;
int   Dist_Sinistra      = 0;
int Dist_Destra        = 0;
int Dist_Avanti        =   0;
int Dist_Dietro        = 0;
int Dist_Sinistra_Diag = 0;
int Dist_Destra_Diag    = 0;
boolean DestraSinistra = false;
int Gradi              = 0;
int   nX;
int Distanza_Frontale  = 30;
int Distanza_Laterale  = 20;
int Distanza_Minima     = 50;
int Potenza_Automatico = 65;
int Potenza_Minima     = 50;
int   Accelera_Decelera  = 5;
int offset             = 5;
int Tempo_Rotazione    =   200;
int variabile          = 0;
String dataingps       = "";
long latitudine_car   =  0;
long longitudine_car = 0;
long latitudine_tablet = 0;
long longitudine_tablet   = 0;
long distanza_tc = 0;
double angolo_tc = 0;
String cLatitudine = "";
String   cLongitudine = "";
String cAngolo = "";
String cDistanza_tc = "";
int   Angolo;
boolean Attivo_GPS = false;
int offset_bussola = 0;
long offset_lat   = 0;
long offset_long = 0;
static double xlow  = 0;
static double ylow   = 0;
static double xhigh = 0;
static double yhigh = 0;

/* ----------   Interupt1----------------- */
void counter()  {
  pulses++;
}

/*   ---------- Setup -------------------- */
void setup() {
  MyWire.begin();
   QMC5883L_Configura();
  Arresta();
  int Distanza = 0;
  Serial.begin(115200);      // seriale utilizzata per il debug      
  Serial2.begin(115200);    //   seriale utilizzata per la trasmissione all'applicazione android
  gpsPort.begin(115200);       // seriale utilizzata per il colloquio con GPS
  potenza = Potenza_Minima;
   motore_posteriore_dx.setSpeed(potenza);
  motore_posteriore_sx.setSpeed(potenza);
   motore_anteriore_sx. setSpeed(potenza);
  motore_anteriore_dx. setSpeed(potenza);

   motore_posteriore_dx.run(RELEASE);
  motore_posteriore_sx.run(RELEASE);
   motore_anteriore_sx. run(RELEASE);
  motore_anteriore_dx. run(RELEASE);
   
  // interupt utilizzato per calcolare la velocit
  pinMode(pin_velocita,   INPUT);
  digitalWrite(pin_velocita, HIGH);
  attachInterrupt(digitalPinToInterrupt(pin_velocita),   counter, FALLING);
  pulses = 0;
  
  // interupt utilizzato per inviare   le informazioni all'APP sul tablet
  pinMode(PIN_PRINT_APP, INPUT);
  digitalWrite(PIN_PRINT_APP,   HIGH);
  attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP), Print_Info, FALLING);

   cm_al_secondo = 0;
  timeold = 0;
  timeold2 = 0;
  
  MyServo_Info.attach(9);
   MyServo_Info.write(90);
  
  MyServo.attach(10);
  MyServoWriteNew(20);
   MyServoWriteNew(160);
  MyServoWriteNew(90);
}

/* ---------- Loop   Principale ---------- */
void loop() {
  bytericevuto = 0;
  int appoggio;
   if (Serial2.available() > 0) { bytericevuto = Serial2.read();   }
  else                         {   delay(10);                       }
  
  Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
   if (Dist_Avanti < Distanza_Minima && (direzione == DIR_AVANTI)) {
   if (potenza   > Potenza_Minima) {
    if (potenza > 200)      { potenza *= 0.50; }
    else   if (potenza > 150) { potenza *= 0.65; }
    else                    { potenza   *= 0.80; }
    Potenza(potenza);
    }
    if (Dist_Avanti <= 20) {
       Indietro();
      delay(20);
      Arresta(); 
      
    }      
   }
    
  if ( direzione == DIR_INDIETRO ) {
    Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
     if (Dist_Dietro < Distanza_Minima) {
      if (potenza > 200)      { potenza   *= 0.50; }
      else if (potenza > 150) { potenza *= 0.65; }
      else                    {   potenza *= 0.80; }
      Potenza(potenza);
      }
    if (Dist_Dietro   <= 20) {
      Avanti();
      delay(20);
      Arresta();
      Potenza(Potenza_Minima);   
    } 
  }
    
  switch (bytericevuto) {
    case ARRESTA:
       Arresta();
      break;
    case AVANTI:
      Avanti();
      break;
     case INDIETRO:
      Indietro(); 
      break;
    case DESTRA:
       Destra();
      break;
    case SINISTRA:
      Sinistra();
      break;
     case DESTRA_AVANTI:
      Destra_Avanti();
      break;
    case SINISTRA_AVANTI:
       Sinistra_Avanti();
      break;
    case DESTRA_INDIETRO:
      Destra_Indietro();
       break;
    case SINISTRA_INDIETRO:
      Sinistra_Indietro();
      break;
     case ACCELLERA:
      Accelera();
      break;
    case DECELERA:
       Decelera();
      break;
    case AUTOMATICO:
      Automatico();
       break;    
    case STOP:
      Arresta();
      Potenza(0);
       break;
    case POTENZA:
      while (Serial2.available() < 1 ){ delay(10);   }
      potenza = Serial2.read();
      Potenza(potenza);
      break;
     case SETTING:
      Impostazioni();
      break;
    case OFFSET_GPS:
       offset_lat  = (latitudine_car-latitudine_tablet);
      offset_long = (longitudine_car-longitudine_tablet);
       break;
    case OFFSET_BUSSOLA:
      offset_bussola = 0;
      offset_bussola   = QMC5883L_Angolo();
      break;
    case CALIBRA_GPS:
      QMC5883L_Calibrazione();
       break;
    case ATTIVA_GPS:
      while (Serial2.available() < 1 ){   delay(10); }
      if (Serial2.peek() == 1 || Serial2.peek() == 2) {
         appoggio   = Serial2.read();
         if      ( appoggio == 1) { Attivo_GPS = true ;  }   
         else if ( appoggio == 2) { Attivo_GPS = false ; } 
      }     
       break;    
    case GPS:
       Gps();
       break;
    default:
       break;
   }        
}

/* ---------- Avanti -------------------   */
void Avanti(){
  if ( potenza < Potenza_Minima) {potenza = Potenza_Minima;   }
  
  Potenza(potenza);
  motore_posteriore_dx.run(FORWARD);
  motore_posteriore_sx.run(FORWARD);
   motore_anteriore_sx. run(FORWARD);
  motore_anteriore_dx. run(FORWARD);
   direzione = DIR_AVANTI;
}

/* ---------- Indietro ----------------- */
void   Indietro() {
  if ( potenza < Potenza_Minima) {potenza = Potenza_Minima;  }
   
  Potenza(potenza);
  motore_posteriore_dx.run(BACKWARD);
  motore_posteriore_sx.run(BACKWARD);
   motore_anteriore_sx. run(BACKWARD);
  motore_anteriore_dx. run(BACKWARD);
   direzione = DIR_INDIETRO;
}

/* ---------- Arresta ------------------   */
void Arresta() {
  motore_posteriore_dx.run(RELEASE);
  motore_posteriore_sx.run(RELEASE);
   motore_anteriore_sx. run(RELEASE);
  motore_anteriore_dx. run(RELEASE);
   Potenza(potenza);
  direzione = DIR_STOP;
}

/* ---------- Potenza   ------------------ */
void Potenza(int Speed) {  
  motore_posteriore_dx.setSpeed(Speed);
   motore_posteriore_sx.setSpeed(Speed);
  motore_anteriore_sx. setSpeed(Speed);
   motore_anteriore_dx. setSpeed(Speed); 
  potenza = Speed;
}

/* ----------   Destra ------------------- */
void Destra() {
  motore_posteriore_dx.run(BACKWARD);
   motore_posteriore_sx.run(FORWARD);
  motore_anteriore_sx. run(FORWARD);
   motore_anteriore_dx. run(BACKWARD); 
  direzione = DIR_DESTRA;
}

/*   ---------- Sinistra ----------------- */
void Sinistra() {
  motore_posteriore_dx.run(FORWARD   );
  motore_posteriore_sx.run(BACKWARD);
  motore_anteriore_sx. run(BACKWARD);
   motore_anteriore_dx. run(FORWARD );
  direzione = DIR_SINISTRA;  
}

/*   ---------- Destra Avanti ------------ */
void Destra_Avanti() {
  motore_posteriore_dx.run(FORWARD);
   motore_posteriore_sx.run(FORWARD);
  motore_anteriore_sx. run(FORWARD);
   motore_anteriore_dx. run(FORWARD);
  motore_anteriore_dx. setSpeed(potenza   / RIDUZIONE_POTENZA_LOW);
  motore_posteriore_dx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
   motore_anteriore_sx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
  motore_posteriore_sx.setSpeed(potenza   * RIDUZIONE_POTENZA_LOW);
  direzione = DIR_AVANTI;
  }

/* ----------   Sinistra Avanti ---------- */
void Sinistra_Avanti() {
  motore_posteriore_dx.run(FORWARD);
   motore_posteriore_sx.run(FORWARD);
  motore_anteriore_sx. run(FORWARD);
   motore_anteriore_dx. run(FORWARD);
  motore_anteriore_sx. setSpeed(potenza   / RIDUZIONE_POTENZA_LOW);
  motore_posteriore_sx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
   motore_anteriore_dx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
  motore_posteriore_dx.setSpeed(potenza   * RIDUZIONE_POTENZA_LOW);
  direzione = DIR_AVANTI;
}

/* ----------   Destra Indietro ---------- */
void Destra_Indietro() {
  motore_posteriore_dx.run(BACKWARD);
   motore_posteriore_sx.run(BACKWARD);
  motore_anteriore_sx. run(BACKWARD);
   motore_anteriore_dx. run(BACKWARD);
  motore_anteriore_dx. setSpeed(potenza   / RIDUZIONE_POTENZA_LOW);
  motore_posteriore_dx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
   motore_anteriore_sx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
  motore_posteriore_sx.setSpeed(potenza   * RIDUZIONE_POTENZA_LOW);
  direzione = DIR_INDIETRO;
}

/* ----------   Sinistra Indietro -------- */
void Sinistra_Indietro() {
  motore_posteriore_dx.run(BACKWARD);
   motore_posteriore_sx.run(BACKWARD);
  motore_anteriore_sx. run(BACKWARD);
   motore_anteriore_dx. run(BACKWARD);
  motore_anteriore_sx. setSpeed(potenza   / RIDUZIONE_POTENZA_LOW);
  motore_posteriore_sx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
   motore_anteriore_dx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
  motore_posteriore_dx.setSpeed(potenza   * RIDUZIONE_POTENZA_LOW);
  direzione = DIR_INDIETRO;
}
 

/* ----------   Accellera ---------------- */
void Accelera() { 
  potenza += Accelera_Decelera;
   if ( potenza > 250) { potenza = 250;  }
  
  Potenza(potenza);
}
   
/* ---------- Decelera ----------------- */
void Decelera() { 
  potenza   -= Accelera_Decelera;
  if ( potenza < 0) { potenza = 0;  }
  
  Potenza(potenza);
}

/*   ---------- Misura distanza ---------- */
int Misura_Distanza(int a_r) {
  int   distanza_cm;
  distanza_cm = sonar[a_r].ping_cm();
  if ( distanza_cm <= 0   ) { distanza_cm = MAX_DISTANCE;  }
  return distanza_cm;
}

/* ----------   Impostazioni ---------- */
void Impostazioni() {
      while (Serial2.available()   < 1 ){ delay(10); }
      Distanza_Frontale  = Serial2.read();
      while   (Serial2.available() < 1 ){ delay(10); }
      Distanza_Laterale  = Serial2.read();
       while (Serial2.available() < 1 ){ delay(10); }
      Distanza_Minima    =   Serial2.read();
      while (Serial2.available() < 1 ){ delay(10); }
      Potenza_Automatico   = Serial2.read();
      while (Serial2.available() < 1 ){ delay(10); }
      Potenza_Minima      = Serial2.read();
      while (Serial2.available() < 1 ){ delay(10); }
       Accelera_Decelera  = Serial2.read();
      while (Serial2.available() <   1 ){ delay(10); }
      offset             = (Serial2.read() - 90);
      while   (Serial2.available() < 1 ){ delay(10); }
      Tempo_Rotazione    = (Serial2.read()*10);
       MyServoWriteNew(90);
  }


/* ---------- Avanzamento Automatico   --- */
int Automatico() {
int returnvalue = 0;

  while (returnvalue   != AUTOMATICO ) {
    Gradi = 0;
    if (Serial2.available() > 0){ returnvalue   = Serial2.read();}
    Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
    while   ( Dist_Avanti > Distanza_Frontale  && returnvalue != AUTOMATICO ){  
      if   (Serial2.available() > 0){ returnvalue = Serial2.read();}
      // guarda a destra   e sinistra
      if   (DestraSinistra) { Gradi++; }
      // guarda a sinistra
       else                  { Gradi--; }
      
      MyServo.write(90+Gradi+offset);
       if (abs(Gradi) > 30) { DestraSinistra = !DestraSinistra;}
      Avanti();
       Potenza(Potenza_Automatico);
      delay(10);        
      Dist_Avanti   = Misura_Distanza(SENSORE_AVANTI);
    }
    Arresta();
    if (returnvalue   == AUTOMATICO) {break;}
    Distanza_Ostacolo();
    // nel caso viene chiuso   su tre lati va indietro
    if  ( Dist_Destra   < Distanza_Laterale && Dist_Destra_Diag    < Distanza_Frontale &&
          Dist_Sinistra < Distanza_Laterale && Dist_Sinistra_Diag   < Distanza_Frontale &&
          Dist_Avanti   < Distanza_Frontale && (Dist_Dietro   > 25)) { 
      DestraSinistra = false;
      Gradi = 0;
      while (   Dist_Avanti < Distanza_Frontale*2 ){  
        if (DestraSinistra){  Gradi +=   2;  }
        else               {  Gradi -= 2;  }
        MyServoWriteNew(90+Gradi);
         if (abs(Gradi) > 70) {
          Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
           if (Dist_Avanti > Distanza_Laterale*2)  {
            Potenza(Potenza_Automatico*3);
             if ( Gradi > 0 ) { Sinistra();        }
            else             {   Destra();          }  
            delay(Tempo_Rotazione); 
            break;
           }
          DestraSinistra = !DestraSinistra;
        }
        Potenza(Potenza_Minima);
         Indietro();
        delay(10);        
        Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
         if (Dist_Dietro < 10) {  break;}
      }
      MyServoWriteNew(90);
       }
          
    // decide se girare a sinistra o destra
    else   if  (Dist_Sinistra < Dist_Destra   && Dist_Sinistra < Dist_Avanti) {
        Potenza(Potenza_Automatico*3);
         Destra(); 
        delay(Tempo_Rotazione*0.75);
     }
    else   if (Dist_Destra < Dist_Sinistra && Dist_Destra   < Dist_Avanti) {
      Potenza(Potenza_Automatico*3);
       Sinistra();
      delay(Tempo_Rotazione*0.75);
      }
    else if   (Dist_Destra        < Distanza_Laterale)                     {
      Potenza(Potenza_Automatico*3);
       Sinistra();
      delay(Tempo_Rotazione);
      }
    else if (Dist_Sinistra       < Distanza_Laterale)                     {
      Potenza(Potenza_Automatico*3);
       Destra();
      delay(Tempo_Rotazione);
      } 
    else if (Dist_Sinistra_Diag   < Distanza_Frontale)                     {
      Potenza(Potenza_Automatico*3);
       Destra();
      delay(Tempo_Rotazione);
      } 
    else if (Dist_Destra_Diag    < Distanza_Frontale)                     {
      Potenza(Potenza_Automatico*3);
       Sinistra();
      delay(Tempo_Rotazione);
      }
    else if (Dist_Destra>Dist_Sinistra   && Dist_Destra>50)                {
      Potenza(Potenza_Automatico*3);
       Destra();
      delay(Tempo_Rotazione); }
    else if (Dist_Sinistra>Dist_Destra   && Dist_Sinistra>50)              {
      Potenza(Potenza_Automatico*3);
       Sinistra();
      delay(Tempo_Rotazione); }
      
    else if (Dist_Avanti        <= Distanza_Frontale)                     {  
      DestraSinistra = false;
       Gradi = 0;
      while ( Dist_Avanti < Distanza_Frontale*2 ){  
        if   (DestraSinistra){  Gradi += 2;  }
        else               {  Gradi -= 2;  }
         MyServoWriteNew(90+Gradi);
        if (abs(Gradi) > 70) {
          Dist_Avanti   = Misura_Distanza(SENSORE_AVANTI);
          if (Dist_Avanti > Distanza_Laterale*2)   {
            Potenza(Potenza_Automatico*3);
            if ( Gradi > 0   ) { Sinistra();        }
            else             { Destra();          }   
            delay(Tempo_Rotazione); 
            break;
          }
           DestraSinistra = !DestraSinistra;
        }
        Potenza(Potenza_Minima);
         Indietro();
        delay(10);        
        Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
         if (Dist_Dietro < 10) {  break;}
      }
      MyServoWriteNew(90);
     }
  }
  MyServoWriteNew(90);
  Arresta();
}

/* ----------   Distanza Ostacolo -------- */
void Distanza_Ostacolo()
{ 
  //Misura la   distanza degli ostacoli, destra, sinistra, destra diagonale, sinistra diagonale,   Avanti e dietro. 
  // Dist_Sinistra, Dist_Destra, Dist_Avanti; Dist_Dietro,   Dist_Sinistra_Diag, Dist_Destra_Diag
  detachInterrupt(digitalPinToInterrupt(pin_velocita));
   detachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP));
  Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
   
  MyServoWriteNew(130);
  Dist_Sinistra_Diag = Misura_Distanza(SENSORE_AVANTI);
   
  MyServoWriteNew(170);
  Dist_Sinistra = Misura_Distanza(SENSORE_AVANTI);

   MyServoWriteNew(50);
  Dist_Destra_Diag = Misura_Distanza(SENSORE_AVANTI);
   
  MyServoWriteNew(10);
  Dist_Destra = Misura_Distanza(SENSORE_AVANTI);
   
  MyServoWriteNew(90);
  Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
   attachInterrupt(digitalPinToInterrupt(pin_velocita), counter, FALLING);
  attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP),   Print_Info, FALLING);
}

/* ---------- Stampa informazioni in APP --------   */
 void Print_Info() {
    if (millis() - timeold2 >= 500) {
      detachInterrupt(digitalPinToInterrupt(pin_velocita));
       detachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP));
      cm_al_secondo   = pulses*2/((millis() - timeold2)/500);
      pulses = 0; 
      timeold2   = millis();
      
      if (direzione != DIR_STOP && cm_al_secondo <= 5)   {
        variabile += 1;
        if(variabile >= 10){
          variabile   = 0;
          if (bytericevuto == 58) {Distanza_Ostacolo();}
          else   {Arresta();}
       }
      }
      else {variabile = 0;}
      attachInterrupt(digitalPinToInterrupt(pin_velocita),   counter, FALLING);
      attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP),   Print_Info, FALLING);
    }
    if (millis() - timeold >= 100) {
       detachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP));
        detachInterrupt(digitalPinToInterrupt(pin_velocita));
      if (direzione   == DIR_INDIETRO) { cDistanza = String(Dist_Dietro);  }     
      else                           {   cDistanza = String(Dist_Avanti);  }
      
      while (cDistanza.length()   < 3) {  cDistanza = " " + cDistanza;     }
      Serial2.print("D"+cDistanza);
       
      cPotenza = String( map(potenza, 0, 250, 0, 100));
      while   (cPotenza.length() < 3 ) {  cPotenza = " " + cPotenza;       }     
      Serial2.print("P"   + cPotenza);
      
      cVelocita = String(cm_al_secondo);
      while   (cVelocita.length() < 3) {  cVelocita = " " + cVelocita;     }      
      Serial2.print("V"   + cVelocita);

      if ( Attivo_GPS ) {
         while (gps.available(gpsPort)   ) {
           fix = gps.read(); // save the latest
           if (fix.valid.location)   {
             latitudine_car = fix.latitudeL();
             longitudine_car   = fix.longitudeL();   
           }      
         }
         String cAngolo_tc   = String(angolo_tc,0);
         while (cAngolo_tc.length() < 3 ) {  cAngolo_tc   = " " + cAngolo_tc; } 
         Serial2.print("C" + cAngolo_tc);
         
          //Angolo = QMC5883L_Angolo();
         cAngolo = String(Angolo);
          while (cAngolo.length() < 3 ) {  cAngolo = " " + cAngolo; } 
         Serial2.print("A"   + cAngolo);
         
         cLatitudine = String(latitudine_car);
         while   (cLatitudine.length() < 10) {  cLatitudine = " " + cLatitudine; } 
         Serial2.print("L"   + cLatitudine);
      
         cLongitudine = String(longitudine_car);
          while (cLongitudine.length() < 10) {  cLongitudine = " " + cLongitudine;   } 
         Serial2.print("G" + cLongitudine);

         cDistanza_tc   = String(distanza_tc);
         while (cDistanza_tc.length() < 10 ) {  cDistanza_tc   = " " + cDistanza_tc; } 
         Serial2.print("M" + cDistanza_tc);
       }
      timeold = millis();
      attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP),   Print_Info, FALLING);
      attachInterrupt(digitalPinToInterrupt(pin_velocita),   counter, FALLING);
    }
 }

 /* ---------- Avanzamento Graduale Servo   ------------------ */
void MyServoWriteNew(int Gradi) {  
  int oldposition   = MyServo.read();
  if (oldposition > Gradi) {
    for (int i = oldposition;   i > Gradi; i -=1) {
      MyServo.write(i+offset);
      delay(3);
    }
   } 
  else if (oldposition < Gradi) {
    for (int i = oldposition; i < Gradi;   i +=1) {
      MyServo.write(i+offset);
      delay(3);
    }
  }
   MyServo.write(Gradi+offset);
}


 /* ---------- Configura sensore   QMC5883L  ------------------ */ 
 void  QMC5883L_Configura(){
   MyWire.beginTransmission(0x0D);
    MyWire.write(0x0B);
   MyWire.write(0x01);
   MyWire.endTransmission();   
   MyWire.beginTransmission(0x0D);
   MyWire.write(0x09);
   MyWire.write(0b11000000|0b00000000|0b00000100|0b00000001);
    MyWire.endTransmission();
 }

 /* ---------- Calibrazione sensore QMC5883L   ------------------ */ 
void QMC5883L_Calibrazione () {
  int returnvalue   = 0;
  xlow = ylow = xhigh = yhigh = 0;
  unsigned long timeold = millis()+20000;
   Potenza(Potenza_Automatico*2);
    
  while ( (millis() < timeold ) && (returnvalue   != ARRESTA ))  {    
       QMC5883L_Angolo(); 
       Destra();
       delay(40);   
       if (Serial2.available() > 0){ returnvalue = Serial2.read();} 
    }
     Arresta();
}

 /* ---------- Lettura sensore QMC5883L  ------------------   */ 
int QMC5883L_Angolo () {
  int nX,nY,nZ;
  float fx,fy;
  
  Arresta();
   delay(10);
  MyWire.beginTransmission(0x0D);
  MyWire.write(0x00);
  MyWire.endTransmission();
   MyWire.requestFrom(0x0D, 6);
  nX = MyWire.read() | (MyWire.read()<<8);
   nY = MyWire.read() | (MyWire.read()<<8);
  nZ = MyWire.read() | (MyWire.read()<<8);
   MyWire.endTransmission();
  
  if(nX<xlow)  xlow  += (nX - xlow )*0.2;
   if(nX>xhigh) xhigh += (nX - xhigh)*0.2;
  if(nY<ylow)  ylow  += (nY - ylow   )*0.2;
  if(nY>yhigh) yhigh += (nY - yhigh)*0.2; 
 
  nX -= (xhigh+xlow)/2;
   nY -= (yhigh+ylow)/2;
  fx = (float)nX/(xhigh-xlow);
  fy = (float)nY/(yhigh-ylow);
   
  Angolo = (3.81667f + 180.0*atan2(fy,fx)/3.14159265358979323846264338327950288)-offset_bussola;
   if(Angolo<=0) Angolo += 360;
  if(Angolo>=360) Angolo -= 360;
  return Angolo;
}

/*   ----------  GPS  ------------------ */ 

void Gps () {
  int returnvalue   = 0;
  while (Serial2.available() < 1 ){ delay(10); }
  dataingps   = ""   ;
  dataingps   = Serial2.readString();  
  latitudine_tablet  = (dataingps.substring(dataingps.indexOf("LAT")+3,   dataingps.indexOf("LONG")).toInt())+offset_lat;
  longitudine_tablet = (dataingps.substring(dataingps.indexOf("LONG")+4,   dataingps.length()).toInt())+offset_long;
  NeoGPS::Location_t tablet( latitudine_tablet,   longitudine_tablet );
  NeoGPS::Location_t car   ( latitudine_car,    longitudine_car     );
  distanza_tc =  fix.location.DistanceKm      ( car, tablet );    
   angolo_tc   =  fix.location.BearingToDegrees( car, tablet );
  
  while   (returnvalue != ARRESTA ) {
    if (Serial2.available() > 0){ returnvalue = Serial2.read();}
     Potenza(Potenza_Automatico*2);
    while (!(((Angolo+10)>=angolo_tc) && ((Angolo-10)<=angolo_tc))   && (returnvalue != ARRESTA) ) { 
      if (Serial2.available() > 0){ returnvalue   = Serial2.read();}
      QMC5883L_Angolo();
      if(((Angolo-angolo_tc)<=   180) && ((Angolo-angolo_tc) >= 0)){Sinistra();}
      else {Destra();}
      delay(40);
     }
    if (distanza_tc<=1){
      Arresta();
      returnvalue = ARRESTA;
     }
    else {
      returnvalue=Automatico_GPS();
      if (distanza_tc<=1){
         Arresta();
        returnvalue = ARRESTA;
      }
    }
  }
}

   /* ---------- Avanzamento Automatico --- */
int Automatico_GPS() {
int returnvalue   = 0;

    Gradi = 0;
    Potenza(Potenza_Automatico);
    Dist_Avanti   = Misura_Distanza(SENSORE_AVANTI);
    while ( (Dist_Avanti > Distanza_Frontale)   && (returnvalue != ARRESTA) && (distanza_tc>1) ){  
      if (Serial2.available()   > 0){ returnvalue = Serial2.read();}
      NeoGPS::Location_t tablet( latitudine_tablet,   longitudine_tablet );
      NeoGPS::Location_t car   ( latitudine_car,    longitudine_car     );
      distanza_tc =  fix.location.DistanceKm      ( car, tablet );    
       angolo_tc   =  fix.location.BearingToDegrees( car, tablet );
      // guarda   a destra e sinistra
      if   (DestraSinistra) { Gradi++; }
      // guarda   a sinistra
      else                  { Gradi--; }
      
      MyServo.write(90+Gradi+offset);
       if (abs(Gradi) > 30) { DestraSinistra = !DestraSinistra;}
      Avanti();
       Potenza(Potenza_Automatico);
      delay(10);        
      Dist_Avanti   = Misura_Distanza(SENSORE_AVANTI);
    }
    Arresta();
    if ((returnvalue   != ARRESTA) && (distanza_tc>1)) {
      Distanza_Ostacolo();
      
      //   nel caso viene chiuso su tre lati va indietro
      if  ( Dist_Destra   < Distanza_Laterale   && Dist_Destra_Diag   < Distanza_Frontale &&
            Dist_Sinistra < Distanza_Laterale   && Dist_Sinistra_Diag < Distanza_Frontale &&
            Dist_Avanti   < Distanza_Frontale   && (Dist_Dietro > 25)) { 
        DestraSinistra = false;
        Gradi =   0;
        while ( Dist_Avanti < Distanza_Frontale*2 ){  
          if (DestraSinistra){   Gradi += 2;  }
          else               {  Gradi -= 2;  }
          MyServoWriteNew(90+Gradi);
           if (abs(Gradi) > 70) {
            Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
             if (Dist_Avanti > Distanza_Laterale*2)  {
              Potenza(Potenza_Automatico*3);
               if ( Gradi > 0 ) { Sinistra();        }
              else             {   Destra();          }  
              delay(Tempo_Rotazione); 
              break;
             }
            DestraSinistra = !DestraSinistra;
          }
           Potenza(Potenza_Minima);
          Indietro();
          delay(10);         
          Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
          if   (Dist_Dietro < 10) {  break;}
        }
        MyServoWriteNew(90);
      }
             
      // decide se girare a sinistra o destra
      else if  (Dist_Sinistra   < Dist_Destra   && Dist_Sinistra < Dist_Avanti) {
          Potenza(Potenza_Automatico*3);
           Destra(); 
          delay(Tempo_Rotazione*0.75);
       }
      else   if (Dist_Destra < Dist_Sinistra && Dist_Destra   < Dist_Avanti) {
        Potenza(Potenza_Automatico*3);
         Sinistra();
        delay(Tempo_Rotazione*0.75);
        }
      else   if (Dist_Destra        < Distanza_Laterale)                     {
        Potenza(Potenza_Automatico*3);
         Sinistra();
        delay(Tempo_Rotazione);
        }
      else   if (Dist_Sinistra      < Distanza_Laterale)                     {
        Potenza(Potenza_Automatico*3);
         Destra();
        delay(Tempo_Rotazione);
        } 
      else   if (Dist_Sinistra_Diag < Distanza_Frontale)                     {
        Potenza(Potenza_Automatico*3);
         Destra();
        delay(Tempo_Rotazione);
        } 
      else   if (Dist_Destra_Diag   < Distanza_Frontale)                     {
        Potenza(Potenza_Automatico*3);
         Sinistra();
        delay(Tempo_Rotazione);
        }
      else   if (Dist_Destra>Dist_Sinistra && Dist_Destra>50)                {
        Potenza(Potenza_Automatico*3);
         Destra();
        delay(Tempo_Rotazione); }
      else if (Dist_Sinistra>Dist_Destra   && Dist_Sinistra>50)              {
        Potenza(Potenza_Automatico*3);
         Sinistra();
        delay(Tempo_Rotazione); }
        
      else   if (Dist_Avanti       <= Distanza_Frontale)                     {  
        DestraSinistra   = false;
        Gradi = 0;
        while ( Dist_Avanti < Distanza_Frontale*2   ){  
          if (DestraSinistra){  Gradi += 2;  }
          else               {   Gradi -= 2;  }
          MyServoWriteNew(90+Gradi);
          if (abs(Gradi)   > 70) {
            Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
            if   (Dist_Avanti > Distanza_Laterale*2)  {
              Potenza(Potenza_Automatico*3);
               if ( Gradi > 0 ) { Sinistra();        }
              else             {   Destra();          }  
              delay(Tempo_Rotazione); 
              break;
             }
            DestraSinistra = !DestraSinistra;
          }
           Potenza(Potenza_Minima);
          Indietro();
          delay(10);         
          Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
          if   (Dist_Dietro < 10) {  break;}
        }
     }
  motore_posteriore_dx.run(RELEASE);
   motore_posteriore_sx.run(RELEASE);
  motore_anteriore_sx. run(RELEASE);
   motore_anteriore_dx. run(RELEASE);
  Potenza(potenza);
  Distanza_Ostacolo();
   if (direzione == DIR_DESTRA){
    while(Dist_Sinistra < Distanza_Laterale*3){
       Potenza(Potenza_Automatico);
      Avanti();
      MyServoWriteNew(170);
       Dist_Sinistra = Misura_Distanza(SENSORE_AVANTI);
      if( Dist_Sinistra   < Distanza_Laterale) {
        Potenza(Potenza_Automatico*3);
        Destra();
         delay(50);
        Arresta();
        }
      MyServoWriteNew(90);
       Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
      if (Dist_Avanti <   Distanza_Frontale){
        Potenza(Potenza_Automatico*3);
        Destra();   
        delay(Tempo_Rotazione*0.75);
        }
    }
    Potenza(Potenza_Automatico*3);
     Sinistra();
    delay(Tempo_Rotazione);
    Potenza(Potenza_Automatico);
     Avanti();
    delay(Tempo_Rotazione*2);
  }
  else if(direzione ==   DIR_SINISTRA){
    while(Dist_Destra < Distanza_Laterale*3){
      Potenza(Potenza_Automatico);
       Avanti();
      MyServoWriteNew(10);
      Dist_Destra = Misura_Distanza(SENSORE_AVANTI);
       if( Dist_Destra < Distanza_Laterale) {
        Potenza(Potenza_Automatico*3);
         Sinistra();
        delay(50);
        Arresta();
        }
       MyServoWriteNew(90);
      Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
       if (Dist_Avanti < Distanza_Frontale){
        Potenza(Potenza_Automatico*3);
         Sinistra(); 
        delay(Tempo_Rotazione*0.75);
        }
    }   
    Potenza(Potenza_Automatico*3);
    Destra();
    delay(Tempo_Rotazione);
     Potenza(Potenza_Automatico);
    Avanti();
    delay(Tempo_Rotazione*2);
     }
  
  }
  MyServoWriteNew(90);
  QMC5883L_Angolo();
  return   returnvalue ; 
}

Source for MIT APP INVENTOR 2

java

1inary file (no preview

inary file (no preview

Arduino IDE

arduino

1#include <NMEAGPS.h>
2#include <SoftwareWire.h>
3#include <Servo.h>
4#include  <AFMotor.h>
5#include <NewPing.h>
6#define GPSport_h
7#define gpsPort Serial3
8#define  GPS_PORT_NAME "Serial3"
9
10SoftwareWire MyWire( 20, 21);
11#define TRIG_PIN_A  41 
12#define ECHO_PIN_A 40 
13#define ECHO_PIN_R 39
14#define TRIG_PIN_R 38  
15#define MAX_DISTANCE 300
16#define SENSORE_AVANTI 0
17#define SENSORE_INDIETRO  1 
18#define RIDUZIONE_POTENZA_LOW 2
19#define DIR_INDIETRO 0
20#define DIR_AVANTI  1
21#define DIR_STOP 2
22#define DIR_DESTRA 3
23#define DIR_SINISTRA 4
24
25/*  Definizione Comandi ricevuti dall'App */
26#define ARRESTA           48
27#define  AVANTI            49
28#define INDIETRO          50
29#define DESTRA            51
30#define  SINISTRA          52
31#define DESTRA_AVANTI     53
32#define SINISTRA_AVANTI   54
33#define DESTRA_INDIETRO   55
34#define SINISTRA_INDIETRO 56
35#define  ACCELLERA         57
36#define DECELERA          58
37#define AUTOMATICO        59
38#define  STOP              79
39#define POTENZA           80
40#define SETTING           90
41#define  OFFSET_GPS        96
42#define OFFSET_BUSSOLA    97
43#define CALIBRA_GPS       98
44#define  ATTIVA_GPS        99
45#define GPS              100
46
47#define PIN_PRINT_APP     18
48
49NewPing sonar[2] = {
50  NewPing(TRIG_PIN_A, ECHO_PIN_A, MAX_DISTANCE),
51  NewPing(TRIG_PIN_R, ECHO_PIN_R, MAX_DISTANCE)
52  };
53AF_DCMotor motore_anteriore_dx(2);
54AF_DCMotor  motore_anteriore_sx(3);
55AF_DCMotor motore_posteriore_sx(4);
56AF_DCMotor motore_posteriore_dx(1);
57Servo  MyServo;
58Servo MyServo_Info;
59static NMEAGPS  gps; // This parses the GPS characters
60static  gps_fix  fix;
61int direzione; 
62int potenza;
63int bytericevuto = 0;
64String  cPotenza;
65String cDistanza;
66String cVelocita;
67int pin_velocita = 19;
68unsigned  int cm_al_secondo; 
69volatile unsigned int pulses;
70unsigned long timeold;  //  controllo ogni 100mS per print info
71unsigned long timeold1; // controllo ogni  Secondo per l'avanzamento automatico GPS
72unsigned long timeold2; // controllo  ogni 500mS per il calcolo della velocit
73unsigned int pulsesperturn = 20;
74int  Dist_Sinistra      = 0;
75int Dist_Destra        = 0;
76int Dist_Avanti        =  0;
77int Dist_Dietro        = 0;
78int Dist_Sinistra_Diag = 0;
79int Dist_Destra_Diag   = 0;
80boolean DestraSinistra = false;
81int Gradi              = 0;
82int  nX;
83int Distanza_Frontale  = 30;
84int Distanza_Laterale  = 20;
85int Distanza_Minima    = 50;
86int Potenza_Automatico = 65;
87int Potenza_Minima     = 50;
88int  Accelera_Decelera  = 5;
89int offset             = 5;
90int Tempo_Rotazione    =  200;
91int variabile          = 0;
92String dataingps       = "";
93long latitudine_car  =  0;
94long longitudine_car = 0;
95long latitudine_tablet = 0;
96long longitudine_tablet  = 0;
97long distanza_tc = 0;
98double angolo_tc = 0;
99String cLatitudine = "";
100String  cLongitudine = "";
101String cAngolo = "";
102String cDistanza_tc = "";
103int  Angolo;
104boolean Attivo_GPS = false;
105int offset_bussola = 0;
106long offset_lat  = 0;
107long offset_long = 0;
108static double xlow  = 0;
109static double ylow  = 0;
110static double xhigh = 0;
111static double yhigh = 0;
112
113/* ----------  Interupt1----------------- */
114void counter()  {
115  pulses++;
116}
117
118/*  ---------- Setup -------------------- */
119void setup() {
120  MyWire.begin();
121  QMC5883L_Configura();
122  Arresta();
123  int Distanza = 0;
124  Serial.begin(115200);     // seriale utilizzata per il debug      
125  Serial2.begin(115200);    //  seriale utilizzata per la trasmissione all'applicazione android
126  gpsPort.begin(115200);      // seriale utilizzata per il colloquio con GPS
127  potenza = Potenza_Minima;
128  motore_posteriore_dx.setSpeed(potenza);
129  motore_posteriore_sx.setSpeed(potenza);
130  motore_anteriore_sx. setSpeed(potenza);
131  motore_anteriore_dx. setSpeed(potenza);
132
133  motore_posteriore_dx.run(RELEASE);
134  motore_posteriore_sx.run(RELEASE);
135  motore_anteriore_sx. run(RELEASE);
136  motore_anteriore_dx. run(RELEASE);
137  
138  // interupt utilizzato per calcolare la velocit
139  pinMode(pin_velocita,  INPUT);
140  digitalWrite(pin_velocita, HIGH);
141  attachInterrupt(digitalPinToInterrupt(pin_velocita),  counter, FALLING);
142  pulses = 0;
143  
144  // interupt utilizzato per inviare  le informazioni all'APP sul tablet
145  pinMode(PIN_PRINT_APP, INPUT);
146  digitalWrite(PIN_PRINT_APP,  HIGH);
147  attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP), Print_Info, FALLING);
148
149  cm_al_secondo = 0;
150  timeold = 0;
151  timeold2 = 0;
152  
153  MyServo_Info.attach(9);
154  MyServo_Info.write(90);
155  
156  MyServo.attach(10);
157  MyServoWriteNew(20);
158  MyServoWriteNew(160);
159  MyServoWriteNew(90);
160}
161
162/* ---------- Loop  Principale ---------- */
163void loop() {
164  bytericevuto = 0;
165  int appoggio;
166  if (Serial2.available() > 0) { bytericevuto = Serial2.read();   }
167  else                         {  delay(10);                       }
168  
169  Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
170  if (Dist_Avanti < Distanza_Minima && (direzione == DIR_AVANTI)) {
171   if (potenza  > Potenza_Minima) {
172    if (potenza > 200)      { potenza *= 0.50; }
173    else  if (potenza > 150) { potenza *= 0.65; }
174    else                    { potenza  *= 0.80; }
175    Potenza(potenza);
176    }
177    if (Dist_Avanti <= 20) {
178      Indietro();
179      delay(20);
180      Arresta(); 
181      
182    }      
183  }
184    
185  if ( direzione == DIR_INDIETRO ) {
186    Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
187    if (Dist_Dietro < Distanza_Minima) {
188      if (potenza > 200)      { potenza  *= 0.50; }
189      else if (potenza > 150) { potenza *= 0.65; }
190      else                    {  potenza *= 0.80; }
191      Potenza(potenza);
192      }
193    if (Dist_Dietro  <= 20) {
194      Avanti();
195      delay(20);
196      Arresta();
197      Potenza(Potenza_Minima);  
198    } 
199  }
200    
201  switch (bytericevuto) {
202    case ARRESTA:
203      Arresta();
204      break;
205    case AVANTI:
206      Avanti();
207      break;
208    case INDIETRO:
209      Indietro(); 
210      break;
211    case DESTRA:
212      Destra();
213      break;
214    case SINISTRA:
215      Sinistra();
216      break;
217    case DESTRA_AVANTI:
218      Destra_Avanti();
219      break;
220    case SINISTRA_AVANTI:
221      Sinistra_Avanti();
222      break;
223    case DESTRA_INDIETRO:
224      Destra_Indietro();
225      break;
226    case SINISTRA_INDIETRO:
227      Sinistra_Indietro();
228      break;
229    case ACCELLERA:
230      Accelera();
231      break;
232    case DECELERA:
233      Decelera();
234      break;
235    case AUTOMATICO:
236      Automatico();
237      break;    
238    case STOP:
239      Arresta();
240      Potenza(0);
241      break;
242    case POTENZA:
243      while (Serial2.available() < 1 ){ delay(10);  }
244      potenza = Serial2.read();
245      Potenza(potenza);
246      break;
247    case SETTING:
248      Impostazioni();
249      break;
250    case OFFSET_GPS:
251      offset_lat  = (latitudine_car-latitudine_tablet);
252      offset_long = (longitudine_car-longitudine_tablet);
253      break;
254    case OFFSET_BUSSOLA:
255      offset_bussola = 0;
256      offset_bussola  = QMC5883L_Angolo();
257      break;
258    case CALIBRA_GPS:
259      QMC5883L_Calibrazione();
260      break;
261    case ATTIVA_GPS:
262      while (Serial2.available() < 1 ){  delay(10); }
263      if (Serial2.peek() == 1 || Serial2.peek() == 2) {
264         appoggio  = Serial2.read();
265         if      ( appoggio == 1) { Attivo_GPS = true ;  }  
266         else if ( appoggio == 2) { Attivo_GPS = false ; } 
267      }     
268      break;    
269    case GPS:
270       Gps();
271       break;
272    default:
273      break;
274   }        
275}
276
277/* ---------- Avanti -------------------  */
278void Avanti(){
279  if ( potenza < Potenza_Minima) {potenza = Potenza_Minima;  }
280  
281  Potenza(potenza);
282  motore_posteriore_dx.run(FORWARD);
283  motore_posteriore_sx.run(FORWARD);
284  motore_anteriore_sx. run(FORWARD);
285  motore_anteriore_dx. run(FORWARD);
286  direzione = DIR_AVANTI;
287}
288
289/* ---------- Indietro ----------------- */
290void  Indietro() {
291  if ( potenza < Potenza_Minima) {potenza = Potenza_Minima;  }
292  
293  Potenza(potenza);
294  motore_posteriore_dx.run(BACKWARD);
295  motore_posteriore_sx.run(BACKWARD);
296  motore_anteriore_sx. run(BACKWARD);
297  motore_anteriore_dx. run(BACKWARD);
298  direzione = DIR_INDIETRO;
299}
300
301/* ---------- Arresta ------------------  */
302void Arresta() {
303  motore_posteriore_dx.run(RELEASE);
304  motore_posteriore_sx.run(RELEASE);
305  motore_anteriore_sx. run(RELEASE);
306  motore_anteriore_dx. run(RELEASE);
307  Potenza(potenza);
308  direzione = DIR_STOP;
309}
310
311/* ---------- Potenza  ------------------ */
312void Potenza(int Speed) {  
313  motore_posteriore_dx.setSpeed(Speed);
314  motore_posteriore_sx.setSpeed(Speed);
315  motore_anteriore_sx. setSpeed(Speed);
316  motore_anteriore_dx. setSpeed(Speed); 
317  potenza = Speed;
318}
319
320/* ----------  Destra ------------------- */
321void Destra() {
322  motore_posteriore_dx.run(BACKWARD);
323  motore_posteriore_sx.run(FORWARD);
324  motore_anteriore_sx. run(FORWARD);
325  motore_anteriore_dx. run(BACKWARD); 
326  direzione = DIR_DESTRA;
327}
328
329/*  ---------- Sinistra ----------------- */
330void Sinistra() {
331  motore_posteriore_dx.run(FORWARD  );
332  motore_posteriore_sx.run(BACKWARD);
333  motore_anteriore_sx. run(BACKWARD);
334  motore_anteriore_dx. run(FORWARD );
335  direzione = DIR_SINISTRA;  
336}
337
338/*  ---------- Destra Avanti ------------ */
339void Destra_Avanti() {
340  motore_posteriore_dx.run(FORWARD);
341  motore_posteriore_sx.run(FORWARD);
342  motore_anteriore_sx. run(FORWARD);
343  motore_anteriore_dx. run(FORWARD);
344  motore_anteriore_dx. setSpeed(potenza  / RIDUZIONE_POTENZA_LOW);
345  motore_posteriore_dx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
346  motore_anteriore_sx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
347  motore_posteriore_sx.setSpeed(potenza  * RIDUZIONE_POTENZA_LOW);
348  direzione = DIR_AVANTI;
349  }
350
351/* ----------  Sinistra Avanti ---------- */
352void Sinistra_Avanti() {
353  motore_posteriore_dx.run(FORWARD);
354  motore_posteriore_sx.run(FORWARD);
355  motore_anteriore_sx. run(FORWARD);
356  motore_anteriore_dx. run(FORWARD);
357  motore_anteriore_sx. setSpeed(potenza  / RIDUZIONE_POTENZA_LOW);
358  motore_posteriore_sx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
359  motore_anteriore_dx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
360  motore_posteriore_dx.setSpeed(potenza  * RIDUZIONE_POTENZA_LOW);
361  direzione = DIR_AVANTI;
362}
363
364/* ----------  Destra Indietro ---------- */
365void Destra_Indietro() {
366  motore_posteriore_dx.run(BACKWARD);
367  motore_posteriore_sx.run(BACKWARD);
368  motore_anteriore_sx. run(BACKWARD);
369  motore_anteriore_dx. run(BACKWARD);
370  motore_anteriore_dx. setSpeed(potenza  / RIDUZIONE_POTENZA_LOW);
371  motore_posteriore_dx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
372  motore_anteriore_sx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
373  motore_posteriore_sx.setSpeed(potenza  * RIDUZIONE_POTENZA_LOW);
374  direzione = DIR_INDIETRO;
375}
376
377/* ----------  Sinistra Indietro -------- */
378void Sinistra_Indietro() {
379  motore_posteriore_dx.run(BACKWARD);
380  motore_posteriore_sx.run(BACKWARD);
381  motore_anteriore_sx. run(BACKWARD);
382  motore_anteriore_dx. run(BACKWARD);
383  motore_anteriore_sx. setSpeed(potenza  / RIDUZIONE_POTENZA_LOW);
384  motore_posteriore_sx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
385  motore_anteriore_dx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
386  motore_posteriore_dx.setSpeed(potenza  * RIDUZIONE_POTENZA_LOW);
387  direzione = DIR_INDIETRO;
388}
389 
390
391/* ----------  Accellera ---------------- */
392void Accelera() { 
393  potenza += Accelera_Decelera;
394  if ( potenza > 250) { potenza = 250;  }
395  
396  Potenza(potenza);
397}
398  
399/* ---------- Decelera ----------------- */
400void Decelera() { 
401  potenza  -= Accelera_Decelera;
402  if ( potenza < 0) { potenza = 0;  }
403  
404  Potenza(potenza);
405}
406
407/*  ---------- Misura distanza ---------- */
408int Misura_Distanza(int a_r) {
409  int  distanza_cm;
410  distanza_cm = sonar[a_r].ping_cm();
411  if ( distanza_cm <= 0  ) { distanza_cm = MAX_DISTANCE;  }
412  return distanza_cm;
413}
414
415/* ----------  Impostazioni ---------- */
416void Impostazioni() {
417      while (Serial2.available()  < 1 ){ delay(10); }
418      Distanza_Frontale  = Serial2.read();
419      while  (Serial2.available() < 1 ){ delay(10); }
420      Distanza_Laterale  = Serial2.read();
421      while (Serial2.available() < 1 ){ delay(10); }
422      Distanza_Minima    =  Serial2.read();
423      while (Serial2.available() < 1 ){ delay(10); }
424      Potenza_Automatico  = Serial2.read();
425      while (Serial2.available() < 1 ){ delay(10); }
426      Potenza_Minima     = Serial2.read();
427      while (Serial2.available() < 1 ){ delay(10); }
428      Accelera_Decelera  = Serial2.read();
429      while (Serial2.available() <  1 ){ delay(10); }
430      offset             = (Serial2.read() - 90);
431      while  (Serial2.available() < 1 ){ delay(10); }
432      Tempo_Rotazione    = (Serial2.read()*10);
433      MyServoWriteNew(90);
434  }
435
436
437/* ---------- Avanzamento Automatico  --- */
438int Automatico() {
439int returnvalue = 0;
440
441  while (returnvalue  != AUTOMATICO ) {
442    Gradi = 0;
443    if (Serial2.available() > 0){ returnvalue  = Serial2.read();}
444    Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
445    while  ( Dist_Avanti > Distanza_Frontale  && returnvalue != AUTOMATICO ){  
446      if  (Serial2.available() > 0){ returnvalue = Serial2.read();}
447      // guarda a destra  e sinistra
448      if   (DestraSinistra) { Gradi++; }
449      // guarda a sinistra
450      else                  { Gradi--; }
451      
452      MyServo.write(90+Gradi+offset);
453      if (abs(Gradi) > 30) { DestraSinistra = !DestraSinistra;}
454      Avanti();
455      Potenza(Potenza_Automatico);
456      delay(10);        
457      Dist_Avanti  = Misura_Distanza(SENSORE_AVANTI);
458    }
459    Arresta();
460    if (returnvalue  == AUTOMATICO) {break;}
461    Distanza_Ostacolo();
462    // nel caso viene chiuso  su tre lati va indietro
463    if  ( Dist_Destra   < Distanza_Laterale && Dist_Destra_Diag   < Distanza_Frontale &&
464          Dist_Sinistra < Distanza_Laterale && Dist_Sinistra_Diag  < Distanza_Frontale &&
465          Dist_Avanti   < Distanza_Frontale && (Dist_Dietro  > 25)) { 
466      DestraSinistra = false;
467      Gradi = 0;
468      while (  Dist_Avanti < Distanza_Frontale*2 ){  
469        if (DestraSinistra){  Gradi +=  2;  }
470        else               {  Gradi -= 2;  }
471        MyServoWriteNew(90+Gradi);
472        if (abs(Gradi) > 70) {
473          Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
474          if (Dist_Avanti > Distanza_Laterale*2)  {
475            Potenza(Potenza_Automatico*3);
476            if ( Gradi > 0 ) { Sinistra();        }
477            else             {  Destra();          }  
478            delay(Tempo_Rotazione); 
479            break;
480          }
481          DestraSinistra = !DestraSinistra;
482        }
483        Potenza(Potenza_Minima);
484        Indietro();
485        delay(10);        
486        Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
487        if (Dist_Dietro < 10) {  break;}
488      }
489      MyServoWriteNew(90);
490      }
491          
492    // decide se girare a sinistra o destra
493    else  if  (Dist_Sinistra < Dist_Destra   && Dist_Sinistra < Dist_Avanti) {
494        Potenza(Potenza_Automatico*3);
495        Destra(); 
496        delay(Tempo_Rotazione*0.75);
497     }
498    else  if (Dist_Destra < Dist_Sinistra && Dist_Destra   < Dist_Avanti) {
499      Potenza(Potenza_Automatico*3);
500      Sinistra();
501      delay(Tempo_Rotazione*0.75);
502      }
503    else if  (Dist_Destra        < Distanza_Laterale)                     {
504      Potenza(Potenza_Automatico*3);
505      Sinistra();
506      delay(Tempo_Rotazione);
507      }
508    else if (Dist_Sinistra      < Distanza_Laterale)                     {
509      Potenza(Potenza_Automatico*3);
510      Destra();
511      delay(Tempo_Rotazione);
512      } 
513    else if (Dist_Sinistra_Diag  < Distanza_Frontale)                     {
514      Potenza(Potenza_Automatico*3);
515      Destra();
516      delay(Tempo_Rotazione);
517      } 
518    else if (Dist_Destra_Diag   < Distanza_Frontale)                     {
519      Potenza(Potenza_Automatico*3);
520      Sinistra();
521      delay(Tempo_Rotazione);
522      }
523    else if (Dist_Destra>Dist_Sinistra  && Dist_Destra>50)                {
524      Potenza(Potenza_Automatico*3);
525      Destra();
526      delay(Tempo_Rotazione); }
527    else if (Dist_Sinistra>Dist_Destra  && Dist_Sinistra>50)              {
528      Potenza(Potenza_Automatico*3);
529      Sinistra();
530      delay(Tempo_Rotazione); }
531      
532    else if (Dist_Avanti       <= Distanza_Frontale)                     {  
533      DestraSinistra = false;
534      Gradi = 0;
535      while ( Dist_Avanti < Distanza_Frontale*2 ){  
536        if  (DestraSinistra){  Gradi += 2;  }
537        else               {  Gradi -= 2;  }
538        MyServoWriteNew(90+Gradi);
539        if (abs(Gradi) > 70) {
540          Dist_Avanti  = Misura_Distanza(SENSORE_AVANTI);
541          if (Dist_Avanti > Distanza_Laterale*2)  {
542            Potenza(Potenza_Automatico*3);
543            if ( Gradi > 0  ) { Sinistra();        }
544            else             { Destra();          }  
545            delay(Tempo_Rotazione); 
546            break;
547          }
548          DestraSinistra = !DestraSinistra;
549        }
550        Potenza(Potenza_Minima);
551        Indietro();
552        delay(10);        
553        Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
554        if (Dist_Dietro < 10) {  break;}
555      }
556      MyServoWriteNew(90);
557    }
558  }
559  MyServoWriteNew(90);
560  Arresta();
561}
562
563/* ----------  Distanza Ostacolo -------- */
564void Distanza_Ostacolo()
565{ 
566  //Misura la  distanza degli ostacoli, destra, sinistra, destra diagonale, sinistra diagonale,  Avanti e dietro. 
567  // Dist_Sinistra, Dist_Destra, Dist_Avanti; Dist_Dietro,  Dist_Sinistra_Diag, Dist_Destra_Diag
568  detachInterrupt(digitalPinToInterrupt(pin_velocita));
569  detachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP));
570  Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
571  
572  MyServoWriteNew(130);
573  Dist_Sinistra_Diag = Misura_Distanza(SENSORE_AVANTI);
574  
575  MyServoWriteNew(170);
576  Dist_Sinistra = Misura_Distanza(SENSORE_AVANTI);
577
578  MyServoWriteNew(50);
579  Dist_Destra_Diag = Misura_Distanza(SENSORE_AVANTI);
580  
581  MyServoWriteNew(10);
582  Dist_Destra = Misura_Distanza(SENSORE_AVANTI);
583  
584  MyServoWriteNew(90);
585  Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
586  attachInterrupt(digitalPinToInterrupt(pin_velocita), counter, FALLING);
587  attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP),  Print_Info, FALLING);
588}
589
590/* ---------- Stampa informazioni in APP --------  */
591 void Print_Info() {
592    if (millis() - timeold2 >= 500) {
593      detachInterrupt(digitalPinToInterrupt(pin_velocita));
594      detachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP));
595      cm_al_secondo  = pulses*2/((millis() - timeold2)/500);
596      pulses = 0; 
597      timeold2  = millis();
598      
599      if (direzione != DIR_STOP && cm_al_secondo <= 5)  {
600        variabile += 1;
601        if(variabile >= 10){
602          variabile  = 0;
603          if (bytericevuto == 58) {Distanza_Ostacolo();}
604          else  {Arresta();}
605       }
606      }
607      else {variabile = 0;}
608      attachInterrupt(digitalPinToInterrupt(pin_velocita),  counter, FALLING);
609      attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP),  Print_Info, FALLING);
610    }
611    if (millis() - timeold >= 100) {
612       detachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP));
613       detachInterrupt(digitalPinToInterrupt(pin_velocita));
614      if (direzione  == DIR_INDIETRO) { cDistanza = String(Dist_Dietro);  }     
615      else                           {  cDistanza = String(Dist_Avanti);  }
616      
617      while (cDistanza.length()  < 3) {  cDistanza = " " + cDistanza;     }
618      Serial2.print("D"+cDistanza);
619      
620      cPotenza = String( map(potenza, 0, 250, 0, 100));
621      while  (cPotenza.length() < 3 ) {  cPotenza = " " + cPotenza;       }     
622      Serial2.print("P"  + cPotenza);
623      
624      cVelocita = String(cm_al_secondo);
625      while  (cVelocita.length() < 3) {  cVelocita = " " + cVelocita;     }      
626      Serial2.print("V"  + cVelocita);
627
628      if ( Attivo_GPS ) {
629         while (gps.available(gpsPort)  ) {
630           fix = gps.read(); // save the latest
631           if (fix.valid.location)  {
632             latitudine_car = fix.latitudeL();
633             longitudine_car  = fix.longitudeL();   
634           }      
635         }
636         String cAngolo_tc  = String(angolo_tc,0);
637         while (cAngolo_tc.length() < 3 ) {  cAngolo_tc  = " " + cAngolo_tc; } 
638         Serial2.print("C" + cAngolo_tc);
639         
640         //Angolo = QMC5883L_Angolo();
641         cAngolo = String(Angolo);
642         while (cAngolo.length() < 3 ) {  cAngolo = " " + cAngolo; } 
643         Serial2.print("A"  + cAngolo);
644         
645         cLatitudine = String(latitudine_car);
646         while  (cLatitudine.length() < 10) {  cLatitudine = " " + cLatitudine; } 
647         Serial2.print("L"  + cLatitudine);
648      
649         cLongitudine = String(longitudine_car);
650         while (cLongitudine.length() < 10) {  cLongitudine = " " + cLongitudine;  } 
651         Serial2.print("G" + cLongitudine);
652
653         cDistanza_tc  = String(distanza_tc);
654         while (cDistanza_tc.length() < 10 ) {  cDistanza_tc  = " " + cDistanza_tc; } 
655         Serial2.print("M" + cDistanza_tc);
656      }
657      timeold = millis();
658      attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP),  Print_Info, FALLING);
659      attachInterrupt(digitalPinToInterrupt(pin_velocita),  counter, FALLING);
660    }
661 }
662
663 /* ---------- Avanzamento Graduale Servo  ------------------ */
664void MyServoWriteNew(int Gradi) {  
665  int oldposition  = MyServo.read();
666  if (oldposition > Gradi) {
667    for (int i = oldposition;  i > Gradi; i -=1) {
668      MyServo.write(i+offset);
669      delay(3);
670    }
671  } 
672  else if (oldposition < Gradi) {
673    for (int i = oldposition; i < Gradi;  i +=1) {
674      MyServo.write(i+offset);
675      delay(3);
676    }
677  }
678  MyServo.write(Gradi+offset);
679}
680
681
682 /* ---------- Configura sensore  QMC5883L  ------------------ */ 
683 void  QMC5883L_Configura(){
684   MyWire.beginTransmission(0x0D);
685   MyWire.write(0x0B);
686   MyWire.write(0x01);
687   MyWire.endTransmission();  
688   MyWire.beginTransmission(0x0D);
689   MyWire.write(0x09);
690   MyWire.write(0b11000000|0b00000000|0b00000100|0b00000001);
691   MyWire.endTransmission();
692 }
693
694 /* ---------- Calibrazione sensore QMC5883L  ------------------ */ 
695void QMC5883L_Calibrazione () {
696  int returnvalue  = 0;
697  xlow = ylow = xhigh = yhigh = 0;
698  unsigned long timeold = millis()+20000;
699  Potenza(Potenza_Automatico*2);
700    
701  while ( (millis() < timeold ) && (returnvalue  != ARRESTA ))  {    
702       QMC5883L_Angolo(); 
703       Destra();
704       delay(40);  
705       if (Serial2.available() > 0){ returnvalue = Serial2.read();} 
706    }
707    Arresta();
708}
709
710 /* ---------- Lettura sensore QMC5883L  ------------------  */ 
711int QMC5883L_Angolo () {
712  int nX,nY,nZ;
713  float fx,fy;
714  
715  Arresta();
716  delay(10);
717  MyWire.beginTransmission(0x0D);
718  MyWire.write(0x00);
719  MyWire.endTransmission();
720  MyWire.requestFrom(0x0D, 6);
721  nX = MyWire.read() | (MyWire.read()<<8);
722  nY = MyWire.read() | (MyWire.read()<<8);
723  nZ = MyWire.read() | (MyWire.read()<<8);
724  MyWire.endTransmission();
725  
726  if(nX<xlow)  xlow  += (nX - xlow )*0.2;
727  if(nX>xhigh) xhigh += (nX - xhigh)*0.2;
728  if(nY<ylow)  ylow  += (nY - ylow  )*0.2;
729  if(nY>yhigh) yhigh += (nY - yhigh)*0.2; 
730 
731  nX -= (xhigh+xlow)/2;
732  nY -= (yhigh+ylow)/2;
733  fx = (float)nX/(xhigh-xlow);
734  fy = (float)nY/(yhigh-ylow);
735  
736  Angolo = (3.81667f + 180.0*atan2(fy,fx)/3.14159265358979323846264338327950288)-offset_bussola;
737  if(Angolo<=0) Angolo += 360;
738  if(Angolo>=360) Angolo -= 360;
739  return Angolo;
740}
741
742/*  ----------  GPS  ------------------ */ 
743
744void Gps () {
745  int returnvalue  = 0;
746  while (Serial2.available() < 1 ){ delay(10); }
747  dataingps   = ""  ;
748  dataingps   = Serial2.readString();  
749  latitudine_tablet  = (dataingps.substring(dataingps.indexOf("LAT")+3,  dataingps.indexOf("LONG")).toInt())+offset_lat;
750  longitudine_tablet = (dataingps.substring(dataingps.indexOf("LONG")+4,  dataingps.length()).toInt())+offset_long;
751  NeoGPS::Location_t tablet( latitudine_tablet,  longitudine_tablet );
752  NeoGPS::Location_t car   ( latitudine_car,    longitudine_car    );
753  distanza_tc =  fix.location.DistanceKm      ( car, tablet );    
754  angolo_tc   =  fix.location.BearingToDegrees( car, tablet );
755  
756  while  (returnvalue != ARRESTA ) {
757    if (Serial2.available() > 0){ returnvalue = Serial2.read();}
758    Potenza(Potenza_Automatico*2);
759    while (!(((Angolo+10)>=angolo_tc) && ((Angolo-10)<=angolo_tc))  && (returnvalue != ARRESTA) ) { 
760      if (Serial2.available() > 0){ returnvalue  = Serial2.read();}
761      QMC5883L_Angolo();
762      if(((Angolo-angolo_tc)<=  180) && ((Angolo-angolo_tc) >= 0)){Sinistra();}
763      else {Destra();}
764      delay(40);
765    }
766    if (distanza_tc<=1){
767      Arresta();
768      returnvalue = ARRESTA;
769    }
770    else {
771      returnvalue=Automatico_GPS();
772      if (distanza_tc<=1){
773        Arresta();
774        returnvalue = ARRESTA;
775      }
776    }
777  }
778}
779
780  /* ---------- Avanzamento Automatico --- */
781int Automatico_GPS() {
782int returnvalue  = 0;
783
784    Gradi = 0;
785    Potenza(Potenza_Automatico);
786    Dist_Avanti  = Misura_Distanza(SENSORE_AVANTI);
787    while ( (Dist_Avanti > Distanza_Frontale)  && (returnvalue != ARRESTA) && (distanza_tc>1) ){  
788      if (Serial2.available()  > 0){ returnvalue = Serial2.read();}
789      NeoGPS::Location_t tablet( latitudine_tablet,  longitudine_tablet );
790      NeoGPS::Location_t car   ( latitudine_car,    longitudine_car    );
791      distanza_tc =  fix.location.DistanceKm      ( car, tablet );    
792      angolo_tc   =  fix.location.BearingToDegrees( car, tablet );
793      // guarda  a destra e sinistra
794      if   (DestraSinistra) { Gradi++; }
795      // guarda  a sinistra
796      else                  { Gradi--; }
797      
798      MyServo.write(90+Gradi+offset);
799      if (abs(Gradi) > 30) { DestraSinistra = !DestraSinistra;}
800      Avanti();
801      Potenza(Potenza_Automatico);
802      delay(10);        
803      Dist_Avanti  = Misura_Distanza(SENSORE_AVANTI);
804    }
805    Arresta();
806    if ((returnvalue  != ARRESTA) && (distanza_tc>1)) {
807      Distanza_Ostacolo();
808      
809      //  nel caso viene chiuso su tre lati va indietro
810      if  ( Dist_Destra   < Distanza_Laterale  && Dist_Destra_Diag   < Distanza_Frontale &&
811            Dist_Sinistra < Distanza_Laterale  && Dist_Sinistra_Diag < Distanza_Frontale &&
812            Dist_Avanti   < Distanza_Frontale  && (Dist_Dietro > 25)) { 
813        DestraSinistra = false;
814        Gradi =  0;
815        while ( Dist_Avanti < Distanza_Frontale*2 ){  
816          if (DestraSinistra){  Gradi += 2;  }
817          else               {  Gradi -= 2;  }
818          MyServoWriteNew(90+Gradi);
819          if (abs(Gradi) > 70) {
820            Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
821            if (Dist_Avanti > Distanza_Laterale*2)  {
822              Potenza(Potenza_Automatico*3);
823              if ( Gradi > 0 ) { Sinistra();        }
824              else             {  Destra();          }  
825              delay(Tempo_Rotazione); 
826              break;
827            }
828            DestraSinistra = !DestraSinistra;
829          }
830          Potenza(Potenza_Minima);
831          Indietro();
832          delay(10);        
833          Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
834          if  (Dist_Dietro < 10) {  break;}
835        }
836        MyServoWriteNew(90);
837      }
838            
839      // decide se girare a sinistra o destra
840      else if  (Dist_Sinistra  < Dist_Destra   && Dist_Sinistra < Dist_Avanti) {
841          Potenza(Potenza_Automatico*3);
842          Destra(); 
843          delay(Tempo_Rotazione*0.75);
844       }
845      else  if (Dist_Destra < Dist_Sinistra && Dist_Destra   < Dist_Avanti) {
846        Potenza(Potenza_Automatico*3);
847        Sinistra();
848        delay(Tempo_Rotazione*0.75);
849        }
850      else  if (Dist_Destra        < Distanza_Laterale)                     {
851        Potenza(Potenza_Automatico*3);
852        Sinistra();
853        delay(Tempo_Rotazione);
854        }
855      else  if (Dist_Sinistra      < Distanza_Laterale)                     {
856        Potenza(Potenza_Automatico*3);
857        Destra();
858        delay(Tempo_Rotazione);
859        } 
860      else  if (Dist_Sinistra_Diag < Distanza_Frontale)                     {
861        Potenza(Potenza_Automatico*3);
862        Destra();
863        delay(Tempo_Rotazione);
864        } 
865      else  if (Dist_Destra_Diag   < Distanza_Frontale)                     {
866        Potenza(Potenza_Automatico*3);
867        Sinistra();
868        delay(Tempo_Rotazione);
869        }
870      else  if (Dist_Destra>Dist_Sinistra && Dist_Destra>50)                {
871        Potenza(Potenza_Automatico*3);
872        Destra();
873        delay(Tempo_Rotazione); }
874      else if (Dist_Sinistra>Dist_Destra  && Dist_Sinistra>50)              {
875        Potenza(Potenza_Automatico*3);
876        Sinistra();
877        delay(Tempo_Rotazione); }
878        
879      else  if (Dist_Avanti       <= Distanza_Frontale)                     {  
880        DestraSinistra  = false;
881        Gradi = 0;
882        while ( Dist_Avanti < Distanza_Frontale*2  ){  
883          if (DestraSinistra){  Gradi += 2;  }
884          else               {  Gradi -= 2;  }
885          MyServoWriteNew(90+Gradi);
886          if (abs(Gradi)  > 70) {
887            Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
888            if  (Dist_Avanti > Distanza_Laterale*2)  {
889              Potenza(Potenza_Automatico*3);
890              if ( Gradi > 0 ) { Sinistra();        }
891              else             {  Destra();          }  
892              delay(Tempo_Rotazione); 
893              break;
894            }
895            DestraSinistra = !DestraSinistra;
896          }
897          Potenza(Potenza_Minima);
898          Indietro();
899          delay(10);        
900          Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
901          if  (Dist_Dietro < 10) {  break;}
902        }
903     }
904  motore_posteriore_dx.run(RELEASE);
905  motore_posteriore_sx.run(RELEASE);
906  motore_anteriore_sx. run(RELEASE);
907  motore_anteriore_dx. run(RELEASE);
908  Potenza(potenza);
909  Distanza_Ostacolo();
910  if (direzione == DIR_DESTRA){
911    while(Dist_Sinistra < Distanza_Laterale*3){
912      Potenza(Potenza_Automatico);
913      Avanti();
914      MyServoWriteNew(170);
915      Dist_Sinistra = Misura_Distanza(SENSORE_AVANTI);
916      if( Dist_Sinistra  < Distanza_Laterale) {
917        Potenza(Potenza_Automatico*3);
918        Destra();
919        delay(50);
920        Arresta();
921        }
922      MyServoWriteNew(90);
923      Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
924      if (Dist_Avanti <  Distanza_Frontale){
925        Potenza(Potenza_Automatico*3);
926        Destra();  
927        delay(Tempo_Rotazione*0.75);
928        }
929    }
930    Potenza(Potenza_Automatico*3);
931    Sinistra();
932    delay(Tempo_Rotazione);
933    Potenza(Potenza_Automatico);
934    Avanti();
935    delay(Tempo_Rotazione*2);
936  }
937  else if(direzione ==  DIR_SINISTRA){
938    while(Dist_Destra < Distanza_Laterale*3){
939      Potenza(Potenza_Automatico);
940      Avanti();
941      MyServoWriteNew(10);
942      Dist_Destra = Misura_Distanza(SENSORE_AVANTI);
943      if( Dist_Destra < Distanza_Laterale) {
944        Potenza(Potenza_Automatico*3);
945        Sinistra();
946        delay(50);
947        Arresta();
948        }
949      MyServoWriteNew(90);
950      Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
951      if (Dist_Avanti < Distanza_Frontale){
952        Potenza(Potenza_Automatico*3);
953        Sinistra(); 
954        delay(Tempo_Rotazione*0.75);
955        }
956    }  
957    Potenza(Potenza_Automatico*3);
958    Destra();
959    delay(Tempo_Rotazione);
960    Potenza(Potenza_Automatico);
961    Avanti();
962    delay(Tempo_Rotazione*2);
963    }
964  
965  }
966  MyServoWriteNew(90);
967  QMC5883L_Angolo();
968  return  returnvalue ; 
969}
970

#include <NMEAGPS.h>
#include <SoftwareWire.h>
#include <Servo.h>
#include  <AFMotor.h>
#include <NewPing.h>
#define GPSport_h
#define gpsPort Serial3
#define  GPS_PORT_NAME "Serial3"

SoftwareWire MyWire( 20, 21);
#define TRIG_PIN_A  41 
#define ECHO_PIN_A 40 
#define ECHO_PIN_R 39
#define TRIG_PIN_R 38  
#define MAX_DISTANCE 300
#define SENSORE_AVANTI 0
#define SENSORE_INDIETRO  1 
#define RIDUZIONE_POTENZA_LOW 2
#define DIR_INDIETRO 0
#define DIR_AVANTI  1
#define DIR_STOP 2
#define DIR_DESTRA 3
#define DIR_SINISTRA 4

/*  Definizione Comandi ricevuti dall'App */
#define ARRESTA           48
#define  AVANTI            49
#define INDIETRO          50
#define DESTRA            51
#define  SINISTRA          52
#define DESTRA_AVANTI     53
#define SINISTRA_AVANTI   54
#define DESTRA_INDIETRO   55
#define SINISTRA_INDIETRO 56
#define  ACCELLERA         57
#define DECELERA          58
#define AUTOMATICO        59
#define  STOP              79
#define POTENZA           80
#define SETTING           90
#define  OFFSET_GPS        96
#define OFFSET_BUSSOLA    97
#define CALIBRA_GPS       98
#define  ATTIVA_GPS        99
#define GPS              100

#define PIN_PRINT_APP     18

NewPing sonar[2] = {
  NewPing(TRIG_PIN_A, ECHO_PIN_A, MAX_DISTANCE),
  NewPing(TRIG_PIN_R, ECHO_PIN_R, MAX_DISTANCE)
  };
AF_DCMotor motore_anteriore_dx(2);
AF_DCMotor  motore_anteriore_sx(3);
AF_DCMotor motore_posteriore_sx(4);
AF_DCMotor motore_posteriore_dx(1);
Servo  MyServo;
Servo MyServo_Info;
static NMEAGPS  gps; // This parses the GPS characters
static  gps_fix  fix;
int direzione; 
int potenza;
int bytericevuto = 0;
String  cPotenza;
String cDistanza;
String cVelocita;
int pin_velocita = 19;
unsigned  int cm_al_secondo; 
volatile unsigned int pulses;
unsigned long timeold;  //  controllo ogni 100mS per print info
unsigned long timeold1; // controllo ogni  Secondo per l'avanzamento automatico GPS
unsigned long timeold2; // controllo  ogni 500mS per il calcolo della velocit
unsigned int pulsesperturn = 20;
int  Dist_Sinistra      = 0;
int Dist_Destra        = 0;
int Dist_Avanti        =  0;
int Dist_Dietro        = 0;
int Dist_Sinistra_Diag = 0;
int Dist_Destra_Diag   = 0;
boolean DestraSinistra = false;
int Gradi              = 0;
int  nX;
int Distanza_Frontale  = 30;
int Distanza_Laterale  = 20;
int Distanza_Minima    = 50;
int Potenza_Automatico = 65;
int Potenza_Minima     = 50;
int  Accelera_Decelera  = 5;
int offset             = 5;
int Tempo_Rotazione    =  200;
int variabile          = 0;
String dataingps       = "";
long latitudine_car  =  0;
long longitudine_car = 0;
long latitudine_tablet = 0;
long longitudine_tablet  = 0;
long distanza_tc = 0;
double angolo_tc = 0;
String cLatitudine = "";
String  cLongitudine = "";
String cAngolo = "";
String cDistanza_tc = "";
int  Angolo;
boolean Attivo_GPS = false;
int offset_bussola = 0;
long offset_lat  = 0;
long offset_long = 0;
static double xlow  = 0;
static double ylow  = 0;
static double xhigh = 0;
static double yhigh = 0;

/* ----------  Interupt1----------------- */
void counter()  {
  pulses++;
}

/*  ---------- Setup -------------------- */
void setup() {
  MyWire.begin();
  QMC5883L_Configura();
  Arresta();
  int Distanza = 0;
  Serial.begin(115200);     // seriale utilizzata per il debug      
  Serial2.begin(115200);    //  seriale utilizzata per la trasmissione all'applicazione android
  gpsPort.begin(115200);      // seriale utilizzata per il colloquio con GPS
  potenza = Potenza_Minima;
  motore_posteriore_dx.setSpeed(potenza);
  motore_posteriore_sx.setSpeed(potenza);
  motore_anteriore_sx. setSpeed(potenza);
  motore_anteriore_dx. setSpeed(potenza);

  motore_posteriore_dx.run(RELEASE);
  motore_posteriore_sx.run(RELEASE);
  motore_anteriore_sx. run(RELEASE);
  motore_anteriore_dx. run(RELEASE);
  
  // interupt utilizzato per calcolare la velocit
  pinMode(pin_velocita,  INPUT);
  digitalWrite(pin_velocita, HIGH);
  attachInterrupt(digitalPinToInterrupt(pin_velocita),  counter, FALLING);
  pulses = 0;
  
  // interupt utilizzato per inviare  le informazioni all'APP sul tablet
  pinMode(PIN_PRINT_APP, INPUT);
  digitalWrite(PIN_PRINT_APP,  HIGH);
  attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP), Print_Info, FALLING);

  cm_al_secondo = 0;
  timeold = 0;
  timeold2 = 0;
  
  MyServo_Info.attach(9);
  MyServo_Info.write(90);
  
  MyServo.attach(10);
  MyServoWriteNew(20);
  MyServoWriteNew(160);
  MyServoWriteNew(90);
}

/* ---------- Loop  Principale ---------- */
void loop() {
  bytericevuto = 0;
  int appoggio;
  if (Serial2.available() > 0) { bytericevuto = Serial2.read();   }
  else                         {  delay(10);                       }
  
  Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
  if (Dist_Avanti < Distanza_Minima && (direzione == DIR_AVANTI)) {
   if (potenza  > Potenza_Minima) {
    if (potenza > 200)      { potenza *= 0.50; }
    else  if (potenza > 150) { potenza *= 0.65; }
    else                    { potenza  *= 0.80; }
    Potenza(potenza);
    }
    if (Dist_Avanti <= 20) {
      Indietro();
      delay(20);
      Arresta(); 
      
    }      
  }
    
  if ( direzione == DIR_INDIETRO ) {
    Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
    if (Dist_Dietro < Distanza_Minima) {
      if (potenza > 200)      { potenza  *= 0.50; }
      else if (potenza > 150) { potenza *= 0.65; }
      else                    {  potenza *= 0.80; }
      Potenza(potenza);
      }
    if (Dist_Dietro  <= 20) {
      Avanti();
      delay(20);
      Arresta();
      Potenza(Potenza_Minima);  
    } 
  }
    
  switch (bytericevuto) {
    case ARRESTA:
      Arresta();
      break;
    case AVANTI:
      Avanti();
      break;
    case INDIETRO:
      Indietro(); 
      break;
    case DESTRA:
      Destra();
      break;
    case SINISTRA:
      Sinistra();
      break;
    case DESTRA_AVANTI:
      Destra_Avanti();
      break;
    case SINISTRA_AVANTI:
      Sinistra_Avanti();
      break;
    case DESTRA_INDIETRO:
      Destra_Indietro();
      break;
    case SINISTRA_INDIETRO:
      Sinistra_Indietro();
      break;
    case ACCELLERA:
      Accelera();
      break;
    case DECELERA:
      Decelera();
      break;
    case AUTOMATICO:
      Automatico();
      break;    
    case STOP:
      Arresta();
      Potenza(0);
      break;
    case POTENZA:
      while (Serial2.available() < 1 ){ delay(10);  }
      potenza = Serial2.read();
      Potenza(potenza);
      break;
    case SETTING:
      Impostazioni();
      break;
    case OFFSET_GPS:
      offset_lat  = (latitudine_car-latitudine_tablet);
      offset_long = (longitudine_car-longitudine_tablet);
      break;
    case OFFSET_BUSSOLA:
      offset_bussola = 0;
      offset_bussola  = QMC5883L_Angolo();
      break;
    case CALIBRA_GPS:
      QMC5883L_Calibrazione();
      break;
    case ATTIVA_GPS:
      while (Serial2.available() < 1 ){  delay(10); }
      if (Serial2.peek() == 1 || Serial2.peek() == 2) {
         appoggio  = Serial2.read();
         if      ( appoggio == 1) { Attivo_GPS = true ;  }  
         else if ( appoggio == 2) { Attivo_GPS = false ; } 
      }     
      break;    
    case GPS:
       Gps();
       break;
    default:
      break;
   }        
}

/* ---------- Avanti -------------------  */
void Avanti(){
  if ( potenza < Potenza_Minima) {potenza = Potenza_Minima;  }
  
  Potenza(potenza);
  motore_posteriore_dx.run(FORWARD);
  motore_posteriore_sx.run(FORWARD);
  motore_anteriore_sx. run(FORWARD);
  motore_anteriore_dx. run(FORWARD);
  direzione = DIR_AVANTI;
}

/* ---------- Indietro ----------------- */
void  Indietro() {
  if ( potenza < Potenza_Minima) {potenza = Potenza_Minima;  }
  
  Potenza(potenza);
  motore_posteriore_dx.run(BACKWARD);
  motore_posteriore_sx.run(BACKWARD);
  motore_anteriore_sx. run(BACKWARD);
  motore_anteriore_dx. run(BACKWARD);
  direzione = DIR_INDIETRO;
}

/* ---------- Arresta ------------------  */
void Arresta() {
  motore_posteriore_dx.run(RELEASE);
  motore_posteriore_sx.run(RELEASE);
  motore_anteriore_sx. run(RELEASE);
  motore_anteriore_dx. run(RELEASE);
  Potenza(potenza);
  direzione = DIR_STOP;
}

/* ---------- Potenza  ------------------ */
void Potenza(int Speed) {  
  motore_posteriore_dx.setSpeed(Speed);
  motore_posteriore_sx.setSpeed(Speed);
  motore_anteriore_sx. setSpeed(Speed);
  motore_anteriore_dx. setSpeed(Speed); 
  potenza = Speed;
}

/* ----------  Destra ------------------- */
void Destra() {
  motore_posteriore_dx.run(BACKWARD);
  motore_posteriore_sx.run(FORWARD);
  motore_anteriore_sx. run(FORWARD);
  motore_anteriore_dx. run(BACKWARD); 
  direzione = DIR_DESTRA;
}

/*  ---------- Sinistra ----------------- */
void Sinistra() {
  motore_posteriore_dx.run(FORWARD  );
  motore_posteriore_sx.run(BACKWARD);
  motore_anteriore_sx. run(BACKWARD);
  motore_anteriore_dx. run(FORWARD );
  direzione = DIR_SINISTRA;  
}

/*  ---------- Destra Avanti ------------ */
void Destra_Avanti() {
  motore_posteriore_dx.run(FORWARD);
  motore_posteriore_sx.run(FORWARD);
  motore_anteriore_sx. run(FORWARD);
  motore_anteriore_dx. run(FORWARD);
  motore_anteriore_dx. setSpeed(potenza  / RIDUZIONE_POTENZA_LOW);
  motore_posteriore_dx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
  motore_anteriore_sx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
  motore_posteriore_sx.setSpeed(potenza  * RIDUZIONE_POTENZA_LOW);
  direzione = DIR_AVANTI;
  }

/* ----------  Sinistra Avanti ---------- */
void Sinistra_Avanti() {
  motore_posteriore_dx.run(FORWARD);
  motore_posteriore_sx.run(FORWARD);
  motore_anteriore_sx. run(FORWARD);
  motore_anteriore_dx. run(FORWARD);
  motore_anteriore_sx. setSpeed(potenza  / RIDUZIONE_POTENZA_LOW);
  motore_posteriore_sx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
  motore_anteriore_dx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
  motore_posteriore_dx.setSpeed(potenza  * RIDUZIONE_POTENZA_LOW);
  direzione = DIR_AVANTI;
}

/* ----------  Destra Indietro ---------- */
void Destra_Indietro() {
  motore_posteriore_dx.run(BACKWARD);
  motore_posteriore_sx.run(BACKWARD);
  motore_anteriore_sx. run(BACKWARD);
  motore_anteriore_dx. run(BACKWARD);
  motore_anteriore_dx. setSpeed(potenza  / RIDUZIONE_POTENZA_LOW);
  motore_posteriore_dx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
  motore_anteriore_sx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
  motore_posteriore_sx.setSpeed(potenza  * RIDUZIONE_POTENZA_LOW);
  direzione = DIR_INDIETRO;
}

/* ----------  Sinistra Indietro -------- */
void Sinistra_Indietro() {
  motore_posteriore_dx.run(BACKWARD);
  motore_posteriore_sx.run(BACKWARD);
  motore_anteriore_sx. run(BACKWARD);
  motore_anteriore_dx. run(BACKWARD);
  motore_anteriore_sx. setSpeed(potenza  / RIDUZIONE_POTENZA_LOW);
  motore_posteriore_sx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
  motore_anteriore_dx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
  motore_posteriore_dx.setSpeed(potenza  * RIDUZIONE_POTENZA_LOW);
  direzione = DIR_INDIETRO;
}
 

/* ----------  Accellera ---------------- */
void Accelera() { 
  potenza += Accelera_Decelera;
  if ( potenza > 250) { potenza = 250;  }
  
  Potenza(potenza);
}
  
/* ---------- Decelera ----------------- */
void Decelera() { 
  potenza  -= Accelera_Decelera;
  if ( potenza < 0) { potenza = 0;  }
  
  Potenza(potenza);
}

/*  ---------- Misura distanza ---------- */
int Misura_Distanza(int a_r) {
  int  distanza_cm;
  distanza_cm = sonar[a_r].ping_cm();
  if ( distanza_cm <= 0  ) { distanza_cm = MAX_DISTANCE;  }
  return distanza_cm;
}

/* ----------  Impostazioni ---------- */
void Impostazioni() {
      while (Serial2.available()  < 1 ){ delay(10); }
      Distanza_Frontale  = Serial2.read();
      while  (Serial2.available() < 1 ){ delay(10); }
      Distanza_Laterale  = Serial2.read();
      while (Serial2.available() < 1 ){ delay(10); }
      Distanza_Minima    =  Serial2.read();
      while (Serial2.available() < 1 ){ delay(10); }
      Potenza_Automatico  = Serial2.read();
      while (Serial2.available() < 1 ){ delay(10); }
      Potenza_Minima     = Serial2.read();
      while (Serial2.available() < 1 ){ delay(10); }
      Accelera_Decelera  = Serial2.read();
      while (Serial2.available() <  1 ){ delay(10); }
      offset             = (Serial2.read() - 90);
      while  (Serial2.available() < 1 ){ delay(10); }
      Tempo_Rotazione    = (Serial2.read()*10);
      MyServoWriteNew(90);
  }


/* ---------- Avanzamento Automatico  --- */
int Automatico() {
int returnvalue = 0;

  while (returnvalue  != AUTOMATICO ) {
    Gradi = 0;
    if (Serial2.available() > 0){ returnvalue  = Serial2.read();}
    Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
    while  ( Dist_Avanti > Distanza_Frontale  && returnvalue != AUTOMATICO ){  
      if  (Serial2.available() > 0){ returnvalue = Serial2.read();}
      // guarda a destra  e sinistra
      if   (DestraSinistra) { Gradi++; }
      // guarda a sinistra
      else                  { Gradi--; }
      
      MyServo.write(90+Gradi+offset);
      if (abs(Gradi) > 30) { DestraSinistra = !DestraSinistra;}
      Avanti();
      Potenza(Potenza_Automatico);
      delay(10);        
      Dist_Avanti  = Misura_Distanza(SENSORE_AVANTI);
    }
    Arresta();
    if (returnvalue  == AUTOMATICO) {break;}
    Distanza_Ostacolo();
    // nel caso viene chiuso  su tre lati va indietro
    if  ( Dist_Destra   < Distanza_Laterale && Dist_Destra_Diag   < Distanza_Frontale &&
          Dist_Sinistra < Distanza_Laterale && Dist_Sinistra_Diag  < Distanza_Frontale &&
          Dist_Avanti   < Distanza_Frontale && (Dist_Dietro  > 25)) { 
      DestraSinistra = false;
      Gradi = 0;
      while (  Dist_Avanti < Distanza_Frontale*2 ){  
        if (DestraSinistra){  Gradi +=  2;  }
        else               {  Gradi -= 2;  }
        MyServoWriteNew(90+Gradi);
        if (abs(Gradi) > 70) {
          Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
          if (Dist_Avanti > Distanza_Laterale*2)  {
            Potenza(Potenza_Automatico*3);
            if ( Gradi > 0 ) { Sinistra();        }
            else             {  Destra();          }  
            delay(Tempo_Rotazione); 
            break;
          }
          DestraSinistra = !DestraSinistra;
        }
        Potenza(Potenza_Minima);
        Indietro();
        delay(10);        
        Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
        if (Dist_Dietro < 10) {  break;}
      }
      MyServoWriteNew(90);
      }
          
    // decide se girare a sinistra o destra
    else  if  (Dist_Sinistra < Dist_Destra   && Dist_Sinistra < Dist_Avanti) {
        Potenza(Potenza_Automatico*3);
        Destra(); 
        delay(Tempo_Rotazione*0.75);
     }
    else  if (Dist_Destra < Dist_Sinistra && Dist_Destra   < Dist_Avanti) {
      Potenza(Potenza_Automatico*3);
      Sinistra();
      delay(Tempo_Rotazione*0.75);
      }
    else if  (Dist_Destra        < Distanza_Laterale)                     {
      Potenza(Potenza_Automatico*3);
      Sinistra();
      delay(Tempo_Rotazione);
      }
    else if (Dist_Sinistra      < Distanza_Laterale)                     {
      Potenza(Potenza_Automatico*3);
      Destra();
      delay(Tempo_Rotazione);
      } 
    else if (Dist_Sinistra_Diag  < Distanza_Frontale)                     {
      Potenza(Potenza_Automatico*3);
      Destra();
      delay(Tempo_Rotazione);
      } 
    else if (Dist_Destra_Diag   < Distanza_Frontale)                     {
      Potenza(Potenza_Automatico*3);
      Sinistra();
      delay(Tempo_Rotazione);
      }
    else if (Dist_Destra>Dist_Sinistra  && Dist_Destra>50)                {
      Potenza(Potenza_Automatico*3);
      Destra();
      delay(Tempo_Rotazione); }
    else if (Dist_Sinistra>Dist_Destra  && Dist_Sinistra>50)              {
      Potenza(Potenza_Automatico*3);
      Sinistra();
      delay(Tempo_Rotazione); }
      
    else if (Dist_Avanti       <= Distanza_Frontale)                     {  
      DestraSinistra = false;
      Gradi = 0;
      while ( Dist_Avanti < Distanza_Frontale*2 ){  
        if  (DestraSinistra){  Gradi += 2;  }
        else               {  Gradi -= 2;  }
        MyServoWriteNew(90+Gradi);
        if (abs(Gradi) > 70) {
          Dist_Avanti  = Misura_Distanza(SENSORE_AVANTI);
          if (Dist_Avanti > Distanza_Laterale*2)  {
            Potenza(Potenza_Automatico*3);
            if ( Gradi > 0  ) { Sinistra();        }
            else             { Destra();          }  
            delay(Tempo_Rotazione); 
            break;
          }
          DestraSinistra = !DestraSinistra;
        }
        Potenza(Potenza_Minima);
        Indietro();
        delay(10);        
        Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
        if (Dist_Dietro < 10) {  break;}
      }
      MyServoWriteNew(90);
    }
  }
  MyServoWriteNew(90);
  Arresta();
}

/* ----------  Distanza Ostacolo -------- */
void Distanza_Ostacolo()
{ 
  //Misura la  distanza degli ostacoli, destra, sinistra, destra diagonale, sinistra diagonale,  Avanti e dietro. 
  // Dist_Sinistra, Dist_Destra, Dist_Avanti; Dist_Dietro,  Dist_Sinistra_Diag, Dist_Destra_Diag
  detachInterrupt(digitalPinToInterrupt(pin_velocita));
  detachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP));
  Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
  
  MyServoWriteNew(130);
  Dist_Sinistra_Diag = Misura_Distanza(SENSORE_AVANTI);
  
  MyServoWriteNew(170);
  Dist_Sinistra = Misura_Distanza(SENSORE_AVANTI);

  MyServoWriteNew(50);
  Dist_Destra_Diag = Misura_Distanza(SENSORE_AVANTI);
  
  MyServoWriteNew(10);
  Dist_Destra = Misura_Distanza(SENSORE_AVANTI);
  
  MyServoWriteNew(90);
  Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
  attachInterrupt(digitalPinToInterrupt(pin_velocita), counter, FALLING);
  attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP),  Print_Info, FALLING);
}

/* ---------- Stampa informazioni in APP --------  */
 void Print_Info() {
    if (millis() - timeold2 >= 500) {
      detachInterrupt(digitalPinToInterrupt(pin_velocita));
      detachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP));
      cm_al_secondo  = pulses*2/((millis() - timeold2)/500);
      pulses = 0; 
      timeold2  = millis();
      
      if (direzione != DIR_STOP && cm_al_secondo <= 5)  {
        variabile += 1;
        if(variabile >= 10){
          variabile  = 0;
          if (bytericevuto == 58) {Distanza_Ostacolo();}
          else  {Arresta();}
       }
      }
      else {variabile = 0;}
      attachInterrupt(digitalPinToInterrupt(pin_velocita),  counter, FALLING);
      attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP),  Print_Info, FALLING);
    }
    if (millis() - timeold >= 100) {
       detachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP));
       detachInterrupt(digitalPinToInterrupt(pin_velocita));
      if (direzione  == DIR_INDIETRO) { cDistanza = String(Dist_Dietro);  }     
      else                           {  cDistanza = String(Dist_Avanti);  }
      
      while (cDistanza.length()  < 3) {  cDistanza = " " + cDistanza;     }
      Serial2.print("D"+cDistanza);
      
      cPotenza = String( map(potenza, 0, 250, 0, 100));
      while  (cPotenza.length() < 3 ) {  cPotenza = " " + cPotenza;       }     
      Serial2.print("P"  + cPotenza);
      
      cVelocita = String(cm_al_secondo);
      while  (cVelocita.length() < 3) {  cVelocita = " " + cVelocita;     }      
      Serial2.print("V"  + cVelocita);

      if ( Attivo_GPS ) {
         while (gps.available(gpsPort)  ) {
           fix = gps.read(); // save the latest
           if (fix.valid.location)  {
             latitudine_car = fix.latitudeL();
             longitudine_car  = fix.longitudeL();   
           }      
         }
         String cAngolo_tc  = String(angolo_tc,0);
         while (cAngolo_tc.length() < 3 ) {  cAngolo_tc  = " " + cAngolo_tc; } 
         Serial2.print("C" + cAngolo_tc);
         
         //Angolo = QMC5883L_Angolo();
         cAngolo = String(Angolo);
         while (cAngolo.length() < 3 ) {  cAngolo = " " + cAngolo; } 
         Serial2.print("A"  + cAngolo);
         
         cLatitudine = String(latitudine_car);
         while  (cLatitudine.length() < 10) {  cLatitudine = " " + cLatitudine; } 
         Serial2.print("L"  + cLatitudine);
      
         cLongitudine = String(longitudine_car);
         while (cLongitudine.length() < 10) {  cLongitudine = " " + cLongitudine;  } 
         Serial2.print("G" + cLongitudine);

         cDistanza_tc  = String(distanza_tc);
         while (cDistanza_tc.length() < 10 ) {  cDistanza_tc  = " " + cDistanza_tc; } 
         Serial2.print("M" + cDistanza_tc);
      }
      timeold = millis();
      attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP),  Print_Info, FALLING);
      attachInterrupt(digitalPinToInterrupt(pin_velocita),  counter, FALLING);
    }
 }

 /* ---------- Avanzamento Graduale Servo  ------------------ */
void MyServoWriteNew(int Gradi) {  
  int oldposition  = MyServo.read();
  if (oldposition > Gradi) {
    for (int i = oldposition;  i > Gradi; i -=1) {
      MyServo.write(i+offset);
      delay(3);
    }
  } 
  else if (oldposition < Gradi) {
    for (int i = oldposition; i < Gradi;  i +=1) {
      MyServo.write(i+offset);
      delay(3);
    }
  }
  MyServo.write(Gradi+offset);
}


 /* ---------- Configura sensore  QMC5883L  ------------------ */ 
 void  QMC5883L_Configura(){
   MyWire.beginTransmission(0x0D);
   MyWire.write(0x0B);
   MyWire.write(0x01);
   MyWire.endTransmission();  
   MyWire.beginTransmission(0x0D);
   MyWire.write(0x09);
   MyWire.write(0b11000000|0b00000000|0b00000100|0b00000001);
   MyWire.endTransmission();
 }

 /* ---------- Calibrazione sensore QMC5883L  ------------------ */ 
void QMC5883L_Calibrazione () {
  int returnvalue  = 0;
  xlow = ylow = xhigh = yhigh = 0;
  unsigned long timeold = millis()+20000;
  Potenza(Potenza_Automatico*2);
    
  while ( (millis() < timeold ) && (returnvalue  != ARRESTA ))  {    
       QMC5883L_Angolo(); 
       Destra();
       delay(40);  
       if (Serial2.available() > 0){ returnvalue = Serial2.read();} 
    }
    Arresta();
}

 /* ---------- Lettura sensore QMC5883L  ------------------  */ 
int QMC5883L_Angolo () {
  int nX,nY,nZ;
  float fx,fy;
  
  Arresta();
  delay(10);
  MyWire.beginTransmission(0x0D);
  MyWire.write(0x00);
  MyWire.endTransmission();
  MyWire.requestFrom(0x0D, 6);
  nX = MyWire.read() | (MyWire.read()<<8);
  nY = MyWire.read() | (MyWire.read()<<8);
  nZ = MyWire.read() | (MyWire.read()<<8);
  MyWire.endTransmission();
  
  if(nX<xlow)  xlow  += (nX - xlow )*0.2;
  if(nX>xhigh) xhigh += (nX - xhigh)*0.2;
  if(nY<ylow)  ylow  += (nY - ylow  )*0.2;
  if(nY>yhigh) yhigh += (nY - yhigh)*0.2; 
 
  nX -= (xhigh+xlow)/2;
  nY -= (yhigh+ylow)/2;
  fx = (float)nX/(xhigh-xlow);
  fy = (float)nY/(yhigh-ylow);
  
  Angolo = (3.81667f + 180.0*atan2(fy,fx)/3.14159265358979323846264338327950288)-offset_bussola;
  if(Angolo<=0) Angolo += 360;
  if(Angolo>=360) Angolo -= 360;
  return Angolo;
}

/*  ----------  GPS  ------------------ */ 

void Gps () {
  int returnvalue  = 0;
  while (Serial2.available() < 1 ){ delay(10); }
  dataingps   = ""  ;
  dataingps   = Serial2.readString();  
  latitudine_tablet  = (dataingps.substring(dataingps.indexOf("LAT")+3,  dataingps.indexOf("LONG")).toInt())+offset_lat;
  longitudine_tablet = (dataingps.substring(dataingps.indexOf("LONG")+4,  dataingps.length()).toInt())+offset_long;
  NeoGPS::Location_t tablet( latitudine_tablet,  longitudine_tablet );
  NeoGPS::Location_t car   ( latitudine_car,    longitudine_car    );
  distanza_tc =  fix.location.DistanceKm      ( car, tablet );    
  angolo_tc   =  fix.location.BearingToDegrees( car, tablet );
  
  while  (returnvalue != ARRESTA ) {
    if (Serial2.available() > 0){ returnvalue = Serial2.read();}
    Potenza(Potenza_Automatico*2);
    while (!(((Angolo+10)>=angolo_tc) && ((Angolo-10)<=angolo_tc))  && (returnvalue != ARRESTA) ) { 
      if (Serial2.available() > 0){ returnvalue  = Serial2.read();}
      QMC5883L_Angolo();
      if(((Angolo-angolo_tc)<=  180) && ((Angolo-angolo_tc) >= 0)){Sinistra();}
      else {Destra();}
      delay(40);
    }
    if (distanza_tc<=1){
      Arresta();
      returnvalue = ARRESTA;
    }
    else {
      returnvalue=Automatico_GPS();
      if (distanza_tc<=1){
        Arresta();
        returnvalue = ARRESTA;
      }
    }
  }
}

  /* ---------- Avanzamento Automatico --- */
int Automatico_GPS() {
int returnvalue  = 0;

    Gradi = 0;
    Potenza(Potenza_Automatico);
    Dist_Avanti  = Misura_Distanza(SENSORE_AVANTI);
    while ( (Dist_Avanti > Distanza_Frontale)  && (returnvalue != ARRESTA) && (distanza_tc>1) ){  
      if (Serial2.available()  > 0){ returnvalue = Serial2.read();}
      NeoGPS::Location_t tablet( latitudine_tablet,  longitudine_tablet );
      NeoGPS::Location_t car   ( latitudine_car,    longitudine_car    );
      distanza_tc =  fix.location.DistanceKm      ( car, tablet );    
      angolo_tc   =  fix.location.BearingToDegrees( car, tablet );
      // guarda  a destra e sinistra
      if   (DestraSinistra) { Gradi++; }
      // guarda  a sinistra
      else                  { Gradi--; }
      
      MyServo.write(90+Gradi+offset);
      if (abs(Gradi) > 30) { DestraSinistra = !DestraSinistra;}
      Avanti();
      Potenza(Potenza_Automatico);
      delay(10);        
      Dist_Avanti  = Misura_Distanza(SENSORE_AVANTI);
    }
    Arresta();
    if ((returnvalue  != ARRESTA) && (distanza_tc>1)) {
      Distanza_Ostacolo();
      
      //  nel caso viene chiuso su tre lati va indietro
      if  ( Dist_Destra   < Distanza_Laterale  && Dist_Destra_Diag   < Distanza_Frontale &&
            Dist_Sinistra < Distanza_Laterale  && Dist_Sinistra_Diag < Distanza_Frontale &&
            Dist_Avanti   < Distanza_Frontale  && (Dist_Dietro > 25)) { 
        DestraSinistra = false;
        Gradi =  0;
        while ( Dist_Avanti < Distanza_Frontale*2 ){  
          if (DestraSinistra){  Gradi += 2;  }
          else               {  Gradi -= 2;  }
          MyServoWriteNew(90+Gradi);
          if (abs(Gradi) > 70) {
            Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
            if (Dist_Avanti > Distanza_Laterale*2)  {
              Potenza(Potenza_Automatico*3);
              if ( Gradi > 0 ) { Sinistra();        }
              else             {  Destra();          }  
              delay(Tempo_Rotazione); 
              break;
            }
            DestraSinistra = !DestraSinistra;
          }
          Potenza(Potenza_Minima);
          Indietro();
          delay(10);        
          Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
          if  (Dist_Dietro < 10) {  break;}
        }
        MyServoWriteNew(90);
      }
            
      // decide se girare a sinistra o destra
      else if  (Dist_Sinistra  < Dist_Destra   && Dist_Sinistra < Dist_Avanti) {
          Potenza(Potenza_Automatico*3);
          Destra(); 
          delay(Tempo_Rotazione*0.75);
       }
      else  if (Dist_Destra < Dist_Sinistra && Dist_Destra   < Dist_Avanti) {
        Potenza(Potenza_Automatico*3);
        Sinistra();
        delay(Tempo_Rotazione*0.75);
        }
      else  if (Dist_Destra        < Distanza_Laterale)                     {
        Potenza(Potenza_Automatico*3);
        Sinistra();
        delay(Tempo_Rotazione);
        }
      else  if (Dist_Sinistra      < Distanza_Laterale)                     {
        Potenza(Potenza_Automatico*3);
        Destra();
        delay(Tempo_Rotazione);
        } 
      else  if (Dist_Sinistra_Diag < Distanza_Frontale)                     {
        Potenza(Potenza_Automatico*3);
        Destra();
        delay(Tempo_Rotazione);
        } 
      else  if (Dist_Destra_Diag   < Distanza_Frontale)                     {
        Potenza(Potenza_Automatico*3);
        Sinistra();
        delay(Tempo_Rotazione);
        }
      else  if (Dist_Destra>Dist_Sinistra && Dist_Destra>50)                {
        Potenza(Potenza_Automatico*3);
        Destra();
        delay(Tempo_Rotazione); }
      else if (Dist_Sinistra>Dist_Destra  && Dist_Sinistra>50)              {
        Potenza(Potenza_Automatico*3);
        Sinistra();
        delay(Tempo_Rotazione); }
        
      else  if (Dist_Avanti       <= Distanza_Frontale)                     {  
        DestraSinistra  = false;
        Gradi = 0;
        while ( Dist_Avanti < Distanza_Frontale*2  ){  
          if (DestraSinistra){  Gradi += 2;  }
          else               {  Gradi -= 2;  }
          MyServoWriteNew(90+Gradi);
          if (abs(Gradi)  > 70) {
            Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
            if  (Dist_Avanti > Distanza_Laterale*2)  {
              Potenza(Potenza_Automatico*3);
              if ( Gradi > 0 ) { Sinistra();        }
              else             {  Destra();          }  
              delay(Tempo_Rotazione); 
              break;
            }
            DestraSinistra = !DestraSinistra;
          }
          Potenza(Potenza_Minima);
          Indietro();
          delay(10);        
          Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
          if  (Dist_Dietro < 10) {  break;}
        }
     }
  motore_posteriore_dx.run(RELEASE);
  motore_posteriore_sx.run(RELEASE);
  motore_anteriore_sx. run(RELEASE);
  motore_anteriore_dx. run(RELEASE);
  Potenza(potenza);
  Distanza_Ostacolo();
  if (direzione == DIR_DESTRA){
    while(Dist_Sinistra < Distanza_Laterale*3){
      Potenza(Potenza_Automatico);
      Avanti();
      MyServoWriteNew(170);
      Dist_Sinistra = Misura_Distanza(SENSORE_AVANTI);
      if( Dist_Sinistra  < Distanza_Laterale) {
        Potenza(Potenza_Automatico*3);
        Destra();
        delay(50);
        Arresta();
        }
      MyServoWriteNew(90);
      Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
      if (Dist_Avanti <  Distanza_Frontale){
        Potenza(Potenza_Automatico*3);
        Destra();  
        delay(Tempo_Rotazione*0.75);
        }
    }
    Potenza(Potenza_Automatico*3);
    Sinistra();
    delay(Tempo_Rotazione);
    Potenza(Potenza_Automatico);
    Avanti();
    delay(Tempo_Rotazione*2);
  }
  else if(direzione ==  DIR_SINISTRA){
    while(Dist_Destra < Distanza_Laterale*3){
      Potenza(Potenza_Automatico);
      Avanti();
      MyServoWriteNew(10);
      Dist_Destra = Misura_Distanza(SENSORE_AVANTI);
      if( Dist_Destra < Distanza_Laterale) {
        Potenza(Potenza_Automatico*3);
        Sinistra();
        delay(50);
        Arresta();
        }
      MyServoWriteNew(90);
      Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
      if (Dist_Avanti < Distanza_Frontale){
        Potenza(Potenza_Automatico*3);
        Sinistra(); 
        delay(Tempo_Rotazione*0.75);
        }
    }  
    Potenza(Potenza_Automatico*3);
    Destra();
    delay(Tempo_Rotazione);
    Potenza(Potenza_Automatico);
    Avanti();
    delay(Tempo_Rotazione*2);
    }
  
  }
  MyServoWriteNew(90);
  QMC5883L_Angolo();
  return  returnvalue ; 
}

File APK for Tablet

java

1inary file (no preview

inary file (no preview

Source for MIT APP INVENTOR 2

java

1inary file (no preview

inary file (no preview

Downloadable files

CAR-SCHEMATIC-BOTTOM

CAR-SCHEMATIC-TOP

ELECTRICAL-DIAGRAM

CAR-SCHEMATIC-INSIDE

CAR-SCHEMATIC-TOP

CAR-SCHEMATIC-BOTTOM

ELECTRICAL-DIAGRAM

CAR-SCHEMATIC-INSIDE

Comments

Only logged in users can leave comments

dfilannino03

0 Followers

•

0 Projects

Intro