You can remotely control your car with Bluetooth. Four settings are available: manual, automatic, steering and vocal control.
SHEMATIC :
Components and supplies
1
2
1
1
1
1
1
1
1
1
1
1
Apps and platforms
Project description
I want to control a car, with one software in different ways. For this I use the bluetooth and an APP written with MIT, which allow me to control it in:
1) manual mode, using the directional arrows.
2) automatic mode, using the proximity sensor.
3) voice mode, using google speech recognition.
4) Steering mode, using the orientation sensor of the TABLET.
5) Gps mode, using the GPS of the tablet,the car will be able to follow the tablet.
In the attachments are available the sources for the Arduino IDE and for MIT App inventor 2.
#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 = 413124390;
long longitudine_car = 162859660;
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 ;
}