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
Source for MIT APP INVENTOR 2
java
1inary 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
File APK for Tablet
java
1inary file (no preview
Source for MIT APP INVENTOR 2
java
1inary file (no preview
Downloadable files
CAR-SCHEMATIC-BOTTOM
CAR-SCHEMATIC-BOTTOM
CAR-SCHEMATIC-TOP
CAR-SCHEMATIC-TOP
ELECTRICAL-DIAGRAM
ELECTRICAL-DIAGRAM
CAR-SCHEMATIC-INSIDE
CAR-SCHEMATIC-INSIDE
CAR-SCHEMATIC-TOP
CAR-SCHEMATIC-TOP
CAR-SCHEMATIC-BOTTOM
CAR-SCHEMATIC-BOTTOM
ELECTRICAL-DIAGRAM
ELECTRICAL-DIAGRAM
CAR-SCHEMATIC-INSIDE
CAR-SCHEMATIC-INSIDE
Comments
Only logged in users can leave comments
Anonymous user
2 years ago
I want help more in my project... My email is shaikh.arman1239090@gmail.com Kindly reply me.
Anonymous user
2 years ago
Hi Danny how can i contact with you? need some help with the project
Anonymous user
2 years ago
hello, it's a good project. i'am a student and i have a project like this ( mobil robot + Arm) can you help me please? can you teach me how to create the Apk with MIT App Inventor for this one and for the arm robot. this is my e-mail: yamicolep@gmail.com thanks in advance
dfilannino03
2 years ago
Regarding the APK with MIT App Inventor, you can modify mine,(download "Source for MIT App Inventor" above) and to control the rotic arm, there are other project on Arduino Hub; among these, I have built a robotic arm too, these are links to my two other project. https://create.arduino.cc/projecthub/DANNY003/robotic-arm-plays-tic-tac-toe-7797b1?ref=user&ref_id=973928&offset=1 https://create.arduino.cc/projecthub/DANNY003/robotic-arm-plays-connect-four-f50a05?ref=user&ref_id=973928&offset=0
Anonymous user
2 years ago
Sir, I want to control two servo motors using IR sensors(as Automatic mode) and also by android app (as manual mode). I am getting success in both but only if am doing these two things separately. Your project is some what similar to my project. Can you please help me. Please give me your mail id , so i can send you my code. Please sir help me. You can also mail me on 143mehul.samal@gmail.com
dfilannino03
2 years ago
Hi, I forwarded my email to your ID I hope I can help
Anonymous user
2 years ago
hello thens very good project coud you help pleas my number whatsapp +996709893410 @elizanasirova885@gmail.com
Anonymous user
2 years ago
Hello, great job, congratulations: I want to use your project, but with Arduino Uno, what should I answer in the code?
Anonymous user
2 years ago
Once again, thank you I would be very grateful if you could help me .. In fact what really interests me in the project is the global positioning system; I can do without the rest I will wait for the answer here if you can help me (sorry for how to write "translated")
dfilannino03
2 years ago
First of all: do you want to build a car with gps or the gps system alone? In the first case, sensors you need are: gps and compass (to know your position and orientation), distance-sensor( for example I used the Ultrasonic Distance Sensor - HC-SR04) and a servo; toghether they will help the car to avoid obstacles. If you want just a normal gps, you need only a sensor and its library.
dfilannino03
2 years ago
Teorecally you could, but there are quite enough sensors so you shouldn't include some of them(such as the gps and the compass) .Moreover you should reorganize inputs and outputs. If you need help, let me know
Anonymous user
2 years ago
Hi sir, I'm making same this project. But I want to control car with manual and automatic mode. Kindly help me what things I have to use & also in coding. Thanks.
Anonymous user
4 years ago
hello thens very good project coud you help pleas my number whatsapp +996709893410 @elizanasirova885@gmail.com
Anonymous user
4 years ago
I want help more in my project... My email is shaikh.arman1239090@gmail.com Kindly reply me.
Anonymous user
4 years ago
Hi sir, I'm making same this project. But I want to control car with manual and automatic mode. Kindly help me what things I have to use & also in coding. Thanks.
gsh7
4 years ago
Hi Danny how can i contact with you? need some help with the project
Anonymous user
4 years ago
Hello, great job, congratulations: I want to use your project, but with Arduino Uno, what should I answer in the code?
Anonymous user
2 years ago
Once again, thank you I would be very grateful if you could help me .. In fact what really interests me in the project is the global positioning system; I can do without the rest I will wait for the answer here if you can help me (sorry for how to write "translated")
dfilannino03
2 years ago
Teorecally you could, but there are quite enough sensors so you shouldn't include some of them(such as the gps and the compass) .Moreover you should reorganize inputs and outputs. If you need help, let me know
dfilannino03
2 years ago
First of all: do you want to build a car with gps or the gps system alone? In the first case, sensors you need are: gps and compass (to know your position and orientation), distance-sensor( for example I used the Ultrasonic Distance Sensor - HC-SR04) and a servo; toghether they will help the car to avoid obstacles. If you want just a normal gps, you need only a sensor and its library.
Anonymous user
5 years ago
hello, it's a good project. i'am a student and i have a project like this ( mobil robot + Arm) can you help me please? can you teach me how to create the Apk with MIT App Inventor for this one and for the arm robot. this is my e-mail: yamicolep@gmail.com thanks in advance
dfilannino03
2 years ago
Regarding the APK with MIT App Inventor, you can modify mine,(download "Source for MIT App Inventor" above) and to control the rotic arm, there are other project on Arduino Hub; among these, I have built a robotic arm too, these are links to my two other project. https://create.arduino.cc/projecthub/DANNY003/robotic-arm-plays-tic-tac-toe-7797b1?ref=user&ref_id=973928&offset=1 https://create.arduino.cc/projecthub/DANNY003/robotic-arm-plays-connect-four-f50a05?ref=user&ref_id=973928&offset=0
Anonymous user
5 years ago
Sir,if I want to make above project which has only manual control so can you send the schmetic and code for that using aurdino mega My email address is diwakarthakur22@gmail.com
dfilannino03
2 years ago
In this project H have already used Arduino Mega, so the schematic is exactly the same (you can not use the Gps or the Compass), regarding the code, you can use the same which I have used; you have just to delete Automatic Control or GPS Control. I hope I have helped you
mehulraj
6 years ago
Sir, I want to control two servo motors using IR sensors(as Automatic mode) and also by android app (as manual mode). I am getting success in both but only if am doing these two things separately. Your project is some what similar to my project. Can you please help me. Please give me your mail id , so i can send you my code. Please sir help me. You can also mail me on 143mehul.samal@gmail.com
dfilannino03
2 years ago
Hi, I forwarded my email to your ID I hope I can help
Bluetooth Controlled Car with Arduino Mega | Arduino Project Hub
rusinov1985
a month ago
Hello! My child and I really liked your project, And we wanted to repeat it, But we are just starting to get acquainted with Arduino Can you help us with this? Could you share the code and library for Arduino and the APK code Thanks in advance