-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathTest_sensori_ultrasuoni.ino
139 lines (116 loc) · 5.03 KB
/
Test_sensori_ultrasuoni.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
//***************************************************************************
//Programma test per il robot Poor versione 2
//Setto le porte di uscita per le funzioni:
//Integrato trovato sulla macchinetta SM6135W oppure RX2-C
//Avanti PIN 10 ---- Integrato 10
//Indieto PIN 9 ---- Integrato 11
//Destra PIN 5 ---- Integrato 6
//Sinistra PIN 6 ---- Integrato 7
//Sensore ultrasuoni destra PIN 13
//Sensore ultrasuoni centro PIN 12
//Sensore ultrasuoni sinistra PIN 11
//Trigger ultrasuoni PIN 3
//
//Realizzato da: Daniele loddo alias Ivotek
//Data: 07/12/2015
//Web: www.mignololab.org www.ivotek.it
//Email: [email protected] [email protected]
//***************************************************************************
//Dichiaro le variabili per utilizzare i nomi al posto dei numeri
//Spreco memoria ma il codice risulta leggibile al principiante
int Destra = 5;
int Sinistra = 6;
int Avanti = 10;
int Indietro = 9;
int UltrasDx = 13;
int UltrasSx = 11;
int UltrasCx = 12;
int UltrasTr = 3;
//Variabili per contenere i valori dei sensori
int vUltrasDx;
int vUltrasSx;
int vUltrasCx;
int vLineaCSx;
//Variabili per contenee il tempo dell'eco
long TempoechoDx;
long TempoechoSx;
long TempoechoCx;
//Variabili per la conversione in centimetri
long centimetriD;
long centimetriS;
long centimetriC;
void setup() {
//Setto i PIN come out o in per pilotare i motori e i sensori di linea del robot.
pinMode(Avanti,OUTPUT); //Setto la porta Avanti come uscita
pinMode(Indietro,OUTPUT); //Setto la porta Indietro come uscita
pinMode(Destra,OUTPUT); //Setto la porta Destra come uscita
pinMode(Sinistra,OUTPUT); //Setto la porta Sinistra come uscita
pinMode(UltrasDx, INPUT); //Setto la porta UltrasDx come ingresso
pinMode(UltrasSx, INPUT); //Setto la porta UltrasSx come ingresso
pinMode(UltrasCx, INPUT); //Setto la porta UltrasCx come ingresso
pinMode(UltrasTr, OUTPUT); //Setto la porta UltrasT come uscita
}
void loop() {
//Genero un impulso di 10 micro secondi per il trigger
digitalWrite(UltrasTr, LOW ); //Azzero l'uscita
delayMicroseconds(3); //Attendo 3 microsecondis
digitalWrite(UltrasTr, HIGH ); //Alzo il livello a uno
delayMicroseconds( 10 ); //Attendo 10 microsecondi
digitalWrite(UltrasTr, LOW ); //Azzero l'uscita
//Prelevo il tempo dai sensori
TempoechoDx = pulseIn( UltrasDx, HIGH,38000 );
delay(10);
//Genero un impulso di 10 micro secondi per il trigger
digitalWrite(UltrasTr, HIGH ); //Alzo il livello a uno
delayMicroseconds( 10 ); //Attendo 10 microsecondi
digitalWrite(UltrasTr, LOW ); //Azzero l'uscita
TempoechoSx = pulseIn( UltrasSx, HIGH,38000 );
delay(10);
//Genero un impulso di 10 micro secondi per il trigger
digitalWrite(UltrasTr, HIGH ); //Alzo il livello a uno
delayMicroseconds( 10 ); //Attendo 10 microsecondi
digitalWrite(UltrasTr, LOW ); //Azzero l'uscita
TempoechoCx = pulseIn( UltrasCx, HIGH,38000 );
//delay(100);
//Converto il tempo in centimetri
centimetriD = 0.034 * TempoechoDx / 2;
centimetriS = 0.034 * TempoechoSx / 2;
centimetriC = 0.034 * TempoechoCx / 2;
//I sensori sono tutte e tre senza ostacoli
if (centimetriD > 15 and centimetriS > 15 and centimetriC > 15 )
{
//Il robot va avanti, cambiare il numero 255 per variare la velocità
analogWrite(Avanti, 255); // Il robot cammina alla massima velocità
analogWrite(Indietro, 0); // Il robot cammina alla massima velocità
//Lo sterzo è dritto
digitalWrite(Destra, LOW);
digitalWrite(Sinistra, LOW);
}
else if (centimetriD < 15 and centimetriD > 0) // C'è un ostacolo a destra
{
//Il robot gira a sinistra andando sempre avanti
digitalWrite(Destra, LOW);
digitalWrite(Sinistra, HIGH); // Il robot gira tutto a sinistra
//Il robot va avanti, cambiare il numero 255 per variare la velocità
analogWrite(Avanti, 255); // Il robot cammina alla massima velocità
analogWrite(Indietro, 0); // Il robot cammina alla massima velocità
}
else if (centimetriS < 15 and centimetriS > 0) // C'è un ostacolo a sinistra
{
//Il robot gira a destra andando sempre avanti
digitalWrite(Sinistra, LOW);
digitalWrite(Destra, HIGH); // Il robot gira tutto a destra
//Il robot va avanti, cambiare il numero 255 per variare la velocità
analogWrite(Avanti, 255); // Il robot cammina alla massima velocità
analogWrite(Indietro, 0); // Il robot cammina alla massima velocità
}
else if (centimetriC < 15 and centimetriC > 0) // C'è un ostacolo al centro
{
//Il robot si ferma
digitalWrite(Destra, LOW); // Il robot ha lo sterzo diritto
digitalWrite(Sinistra, LOW);
//Il robot si ferma
analogWrite(Avanti, 0);
analogWrite(Indietro, 0);
}
}