Auto radiocomandata – Codice in C

Categories:

Codici C

Auto radiocomandata – Codice in C

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 = pulses2/((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_Automatico2);

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_Laterale3){
Potenza(Potenza_Automatico);
Avanti();
MyServoWriteNew(170);
Dist_Sinistra = Misura_Distanza(SENSORE_AVANTI);
if( Dist_Sinistra < Distanza_Laterale) {
Potenza(Potenza_Automatico3);

Destra();
delay(50);
Arresta();
}
MyServoWriteNew(90);
Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
if (Dist_Avanti < Distanza_Frontale){
Potenza(Potenza_Automatico3); Destra(); delay(Tempo_Rotazione0.75);
}
}
Potenza(Potenza_Automatico3);
Sinistra();
delay(Tempo_Rotazione);
Potenza(Potenza_Automatico);
Avanti();
delay(Tempo_Rotazione2);
}
else if(direzione == DIR_SINISTRA){
while(Dist_Destra < Distanza_Laterale3){
Potenza(Potenza_Automatico);
Avanti();
MyServoWriteNew(10);
Dist_Destra = Misura_Distanza(SENSORE_AVANTI);
if( Dist_Destra < Distanza_Laterale) {
Potenza(Potenza_Automatico3);
Sinistra();
delay(50);
Arresta();
}
MyServoWriteNew(90);
Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
if (Dist_Avanti < Distanza_Frontale){
Potenza(Potenza_Automatico3); Sinistra(); delay(Tempo_Rotazione0.75);
}
}
Potenza(Potenza_Automatico3);
Destra();
delay(Tempo_Rotazione);
Potenza(Potenza_Automatico);
Avanti();
delay(Tempo_Rotazione2);
}

}
MyServoWriteNew(90);
QMC5883L_Angolo();
return returnvalue ;
}

Codice in C

Download

Per permettere il funzionamento del circuito, è necessario integrare in Arduino la libreria di seguito disponibile per il download:

Scarica libreria

Tags:

No Tag

Cookie	Durata	Descrizione
cookielawinfo-checkbox-analytics	11 mesi	I cookie vengono utilizzati per memorizzare il consenso dell'utente per i cookie nella categoria "Analitici".
cookielawinfo-checkbox-functional	11 mesi	I cookie vengono utilizzati per memorizzare il consenso dell'utente per i cookie nella categoria "Funzionali".
cookielawinfo-checkbox-necessary	11 mesi	I cookie vengono utilizzati per memorizzare il consenso dell'utente per i cookie nella categoria "Necessari".
cookielawinfo-checkbox-others	11 mesi	I cookie vengono utilizzati per memorizzare il consenso dell'utente per i cookie nella categoria "Altri".
cookielawinfo-checkbox-performance	11 mesi	I cookie vengono utilizzati per memorizzare il consenso dell'utente per i cookie nella categoria "Performance".
viewed_cookie_policy	11 mesi	Il cookie viene utilizzato per memorizzare se l'utente ha acconsentito o meno all'uso dei cookie. Non memorizza alcun dato personale.

Auto radiocomandata – Codice in C

Codice in C

Questo sito utilizza i cookie