Last active
February 9, 2020 14:54
-
-
Save sebadima/a063bf419bfb9dc1bc784192c8658a1c to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
const int PinEchoAnteriore = 7; | |
const int PinTriggerAnteriore = 6; | |
const int PinEchoSinistra = 11; | |
const int PinTriggerSinistra = 10; | |
const int PinEchoDestra = 9; | |
const int PinTriggerDestro = 8; | |
const int motorinoL1 = 2; | |
const int motorinoL2 = 3; | |
const int motorinoR1 = 4; | |
const int motorinoR2 = 5; | |
const int RitardoMedio = 20; | |
volatile float MassimaDistanzaAnteriore = 25.00; | |
volatile float DurataPerAnteriore, DistanzaAnterioreInCm; | |
volatile float DurataPerSinistra, DistanzaSinistraInCm; | |
volatile float DurataPerDestra, DistanzaDestraInCm; | |
volatile float MassimaDistanzaSinistra; | |
volatile float MassimaDistanzaDestra = 20.00; | |
void setup() { | |
// serial | |
Serial.begin(9600); | |
// setta sensori ultrasonici | |
pinMode(PinTriggerAnteriore, OUTPUT); | |
pinMode(PinEchoAnteriore, INPUT); | |
pinMode(PinTriggerSinistra, OUTPUT); | |
pinMode(PinEchoSinistra, INPUT); | |
pinMode(PinTriggerDestro, OUTPUT); | |
pinMode(PinEchoDestra, INPUT); | |
// setta i motorini | |
pinMode(motorinoL1, OUTPUT); | |
pinMode(motorinoL2, OUTPUT); | |
pinMode(motorinoR1, OUTPUT); | |
pinMode(motorinoR2, OUTPUT); | |
} | |
void loop() { | |
ControlloDistanzaAnteriore(); | |
if (DistanzaAnterioreInCm < MassimaDistanzaAnteriore) { | |
Serial.println("Troppo vicino!"); | |
ControlloDistanzaSinistra(); | |
delay(RitardoMedio); | |
ControlloDistanzaDestra(); | |
delay(RitardoMedio); | |
if (DistanzaSinistraInCm < DistanzaDestraInCm) | |
MuoviaDestra(); | |
else if (DistanzaSinistraInCm > DistanzaDestraInCm) { | |
MuoviaSinistra(); | |
} | |
} | |
else { | |
Serial.println("OK"); | |
VaiAvanti(); | |
} | |
ControlloDistanzaSinistra(); | |
if (DistanzaSinistraInCm < MassimaDistanzaSinistra) { | |
Serial.println("a Sinistra sei troppo vicino"); | |
delay(RitardoMedio); | |
ControlloDistanzaSinistra(); | |
delay(RitardoMedio); | |
ControlloDistanzaDestra(); | |
delay(RitardoMedio); | |
if (DistanzaSinistraInCm > DistanzaDestraInCm) | |
VaiAvanti(); | |
else if (DistanzaSinistraInCm < DistanzaDestraInCm) { | |
MuoviaDestra(); | |
} | |
} | |
ControlloDistanzaDestra(); | |
if (DistanzaDestraInCm < MassimaDistanzaDestra) { | |
Serial.println("a Destra sei troppo vicino"); | |
delay(RitardoMedio); | |
ControlloDistanzaDestra(); | |
delay(RitardoMedio); | |
ControlloDistanzaSinistra(); | |
delay(RitardoMedio); | |
if (DistanzaDestraInCm > DistanzaSinistraInCm) | |
VaiAvanti(); | |
else if (DistanzaDestraInCm < DistanzaSinistraInCm) { | |
MuoviaSinistra(); | |
} | |
} | |
} | |
void ControlloDistanzaAnteriore() { | |
digitalWrite(PinTriggerAnteriore, LOW); | |
delayMicroseconds(4); | |
digitalWrite(PinTriggerAnteriore, HIGH); | |
delayMicroseconds(10); | |
digitalWrite(PinTriggerAnteriore, LOW); | |
DurataPerAnteriore = pulseIn(PinEchoAnteriore, HIGH); | |
DistanzaAnterioreInCm = DurataPerAnteriore * 10 / 292 / 2; | |
Serial.print("Distanza: "); | |
Serial.print(DistanzaAnterioreInCm); | |
Serial.println(" cm"); | |
} | |
void ControlloDistanzaSinistra() { | |
digitalWrite(PinTriggerSinistra, LOW); | |
delayMicroseconds(4); | |
digitalWrite(PinTriggerSinistra, HIGH); | |
delayMicroseconds(10); | |
digitalWrite(PinTriggerSinistra, LOW); | |
DurataPerSinistra = pulseIn(PinEchoSinistra, HIGH); | |
DistanzaSinistraInCm = DurataPerSinistra * 10 / 292 / 2; | |
Serial.print("Distanza a sinistra: "); | |
Serial.print(DistanzaSinistraInCm); | |
Serial.println(" cm"); | |
} | |
void ControlloDistanzaDestra() { | |
digitalWrite(PinTriggerDestro, LOW); | |
delayMicroseconds(4); | |
digitalWrite(PinTriggerDestro, HIGH); | |
delayMicroseconds(10); | |
digitalWrite(PinTriggerDestro, LOW); | |
DurataPerDestra = pulseIn(PinEchoDestra, HIGH); | |
DistanzaDestraInCm = DurataPerDestra * 10 / 292 / 2; | |
Serial.print("Distanza a destra: "); | |
Serial.print(DistanzaDestraInCm); | |
Serial.println(" cm"); | |
} | |
void VaiIndietro() { | |
Serial.println("Indietro."); | |
digitalWrite(motorinoL1, HIGH); | |
digitalWrite(motorinoL2, LOW); | |
digitalWrite(motorinoR1, HIGH); | |
digitalWrite(motorinoR2, LOW); | |
} | |
void VaiAvanti() { | |
Serial.println("Avanti."); | |
digitalWrite(motorinoL1, LOW); | |
digitalWrite(motorinoL2, HIGH); | |
digitalWrite(motorinoR1, LOW); | |
digitalWrite(motorinoR2, HIGH); | |
} | |
void MuoviaSinistra() { | |
Serial.println("Sinistra."); | |
digitalWrite(motorinoL1, LOW); | |
digitalWrite(motorinoL2, HIGH); | |
digitalWrite(motorinoR1, HIGH); | |
digitalWrite(motorinoR2, LOW); | |
} | |
void MuoviaDestra() { | |
Serial.println("Destra."); | |
digitalWrite(motorinoL1, HIGH); | |
digitalWrite(motorinoL2, LOW); | |
digitalWrite(motorinoR1, LOW); | |
digitalWrite(motorinoR2, HIGH); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment