3FEN
programma commando motori
Dependencies: mbed HCSR04 mbed-rtos
Diff: main.cpp
- Revision:
- 2:7df0fc7d8d3c
- Parent:
- 1:009a67d6875f
- Child:
- 3:5b91f83642bf
- Child:
- 4:088a0fad279a
--- a/main.cpp Tue May 23 10:46:00 2017 +0000
+++ b/main.cpp Thu May 25 07:12:44 2017 +0000
@@ -1,42 +1,88 @@
-#include "mbed.h"
-
-/*I pin utilizzati per TX e RX sono pin morfo, a livello hardware collegare il pin TX dell HC05 al PA_12 e quello RX a PA_11 ( a livello harware vanno invertiti )*/
-#define TX PA_11
-#define RX PA_12
+/*---LIBRERIE---*/
+ #include "mbed.h"
+ #include "rtos.h"
+ #include "hcsr04.h"
+/*--------------*/
-#define PWMB PC_7 //D9
-#define DIRB D8 //D8
+/*---COSTANTI---*/
+ #define MAX 999999 //valore intero massimo utilizzabile
-#define PWMA PB_4 //D5
-#define DIRA D4 //D4
+ #define CTRL_SIRENA D11 //D11 - pin aggancio sensore sonoro
+
+ #define DISTANCE_CTRL 10 /*decido una distanza, in cm, per i vari controlli dell' ultrasuono, esempio utilizzo come valore
+ per capire quando la macchina si deve fermare*/
-PwmOut motorBpower(PWMB);
-DigitalOut motorBdirection(DIRB);
-
-PwmOut motorApower(PWMA);
-DigitalOut motorAdirection(DIRA);
-
-Serial pc(USBTX, USBRX); //istanzio oggetto per utilizzo monitor seriale
+ /*---PIN BLUETHOOT---*/
+ /*I pin utilizzati per TX e RX sono pin morfo, a livello hardware collegare il pin TX dell HC05 al PA_12 e quello RX a PA_11 ( a livello harware vanno invertiti )*/
+ #define TX PA_11
+ #define RX PA_12
+ /*-------------------*/
+
+ /*---PIN CONTROLLO MOTORI---*/
+ #define PWMB PC_7 //D9
+ #define DIRB D8 //D8
+
+ #define PWMA PB_4 //D5
+ #define DIRA D4 //D4
+ /*--------------------------*/
+
+ /*---PIN ULTRASUONI---*/
+ #define AVANTI_TRIGGER PA_5 //D13
+ #define AVANTI_ECHO PA_6 //D12
+ #define SINISTRA_TRIGGER PA_8 //D7
+ #define SINISTRA_ECHO PB_10 //D6
+ #define DESTRA_TRIGGER PA_10 //D2
+ #define DESTRA_ECHO PB_3 //D3
+ /*--------------------*/
+/*--------------*/
-Serial device(TX, RX); //oggetto per gestione seriale bluethoot
+/*---VARIABILI---*/
+ /*---CONTROLLO MOTORI---*/
+ PwmOut motorBpower(PWMB); //oggetti controllo motori
+ DigitalOut motorBdirection(DIRB);
+
+ PwmOut motorApower(PWMA);
+ DigitalOut motorAdirection(DIRA);
+ /*---------------------*/
+
+ /*---CONTROLLO ULTRASUONI---*/
+ HCSR04 ultrasuoniAvanti(AVANTI_TRIGGER, AVANTI_ECHO); //istanzio oggetti per utilizzo ultrasuoni
+ HCSR04 ultrasuoniSinistra(SINISTRA_TRIGGER, SINISTRA_ECHO);
+ HCSR04 ultrasuoniDestra(DESTRA_TRIGGER, DESTRA_ECHO);
+ /*--------------------------*/
+
+ Serial pc(USBTX, USBRX); //istanzio oggetto per utilizzo monitor seriale
+
+ Serial device(TX, RX); //oggetto per gestione seriale bluethoot
+
+ bool flagSirena = true; //utilizzato per bloccare il robot nel caso senta una sirena
+
+ bool flagAutomatic; //utilizzo per sapere se utilizzare modalita automatica o meno
+/*----------------*/
-void avanti();
-void indietro();
-void destra();
-void sinistra();
-void fermo();
+/*---PROTOTIPI---*/
+ void avanti();
+ void indietro();
+ void destra();
+ void sinistra();
+ void fermo();
+ bool checkFermo();
+ void automaticProgram();
+ void check_sirena_thread(void const *args);
+ void check_stop_thread(void const *args);
+/*---------------*/
int main() {
unsigned char rx;
- motorBpower.period_ms(10);
- motorApower.period_ms(10);
+ motorBpower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare )
+ motorApower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare )
while(true){
if( device.readable() ) { //funzione simile alla Serial.available() di arduino, solo se il monitor e` attivo permette di utilizzarlo
rx = device.getc(); //funzione simile alla Serial.read() di arduino, legge il carattere dal seriale del bluethoot
- pc.printf("recived: %c \r\n",rx); //stampa a video
+ //pc.printf("recived: %c \r\n",rx); //stampa a video
switch(rx){ //in base alla lettera che riceve sa cosa deve fare
case 'a':
avanti();
@@ -53,12 +99,148 @@
case 'e':
fermo();
break;
+ case 'f':
+ flagAutomatic = true;
+ automaticProgram(); //funzione pilota automatico
+ break;
}
}
}
}
+void automaticProgram()
+{
+ int avantiDis, distance; //la variabile direction contiene 0 se il robot deve andare a destra, 1 se deve andare a sinistra
+
+ int distanceDx, distanceSx;
+
+ Thread sirenaThread(check_sirena_thread); // start Thread controllo sirena
+ Thread ctrlStopThread(check_stop_thread); // start Thread controllo sirena
+
+ fermo();
+
+ while(flagAutomatic){ //flag modificabile dal thread che aspetta il segnale di stop
+ while(flagSirena && flagAutomatic){ //il robot si puo' muovere liberamente finche non sente il suono della sirena tipo ambulanza
+ ultrasuoniAvanti.start();
+ avantiDis = ultrasuoniAvanti.get_dist_cm();
+ //pc.printf("avanti: %d \r\n",avantiDis);
+ while( avantiDis >= DISTANCE_CTRL && flagSirena && flagAutomatic ){ //finche il valore letto dall' ultrasuono e' maggiore della distanza di urto la macchina puo' procedere
+ avanti();
+ ultrasuoniAvanti.start();
+ avantiDis = ultrasuoniAvanti.get_dist_cm();
+ //pc.printf("avanti: %d \r\n",avantiDis);
+ wait_ms(10);
+ }
+
+ fermo();
+
+ if( checkFermo() && flagSirena && flagAutomatic ){
+ /*funzione utilizzata per leggere un valore attendibile ( nel range dell ultrasuono ). se dopo 100 tentativi
+ il valore non e' ancora attendibile allora esco dal controllo in modo da non rimanere bloccato*/
+ for( int i = 0; i < 100 && ( distanceDx < 2 || distanceDx > 400 ); i++ ){ //questo perche il range dell'ultrasuono usato va da 2 cm a 4 m ( 400 cm )
+ ultrasuoniDestra.start();
+ distanceDx = ultrasuoniDestra.get_dist_cm();
+ //wait_ms(10);
+ }
+
+ for( int i = 0; i < 100 && ( distanceSx < 2 || distanceSx > 400 ); i++ ){ //questo perche il range dell'ultrasuono usato va da 2 cm a 4 m ( 400 cm )
+ ultrasuoniSinistra.start();
+ distanceSx = ultrasuoniSinistra.get_dist_cm();
+ //wait_ms(10);
+ }
+
+ /*controllo se il valori valori di uscita dalle iterazioni sono attendibili ( nel range )
+ se non soddisfano i criteri allora li setto al valore MAX in modo che i seguenti controlli non li prendano
+ in considerazione*/
+ if( distanceSx < 2 || distanceSx > 400 )
+ distanceSx = MAX;
+
+ if( distanceDx < 2 || distanceDx > 400 )
+ distanceDx = MAX;
+
+ pc.printf("distanceSx: %d distanceDx: %d\r\n",distanceSx, distanceDx);
+
+ //in base ai valori letti capisco da che lato e' piu vicino la superficie piana e in base a cio sono da che parte girare
+ if( distanceSx > distanceDx ){
+ sinistra();
+ wait_ms(1000);
+
+ ultrasuoniDestra.start();
+ distance = ultrasuoniDestra.get_dist_cm();
+ //pc.printf("sinistra: %d \r\n",sinistraDis);
+ while( distance >= DISTANCE_CTRL && flagSirena && flagAutomatic ){ //il robot continua la rotazione per evitare l' ostacolo finche l' ultrasuono di destra lo avverte di aver trovato una superficie piana
+ ultrasuoniDestra.start();
+ distance = ultrasuoniDestra.get_dist_cm();
+ //pc.printf("distance: %d \r\n",distance);
+ }
+ }
+ else{
+ destra();
+ wait_ms(1000);
+
+ ultrasuoniSinistra.start();
+ distance = ultrasuoniSinistra.get_dist_cm();
+ //pc.printf("sinistra: %d \r\n",sinistraDis);
+ while( distance >= DISTANCE_CTRL && flagSirena && flagAutomatic ){ //il robot continua la rotazione per evitare l' ostacolo finche l' ultrasuono di destra lo avverte di aver trovato una superficie piana
+ ultrasuoniSinistra.start();
+ distance = ultrasuoniSinistra.get_dist_cm();
+ //pc.printf("distance: %d \r\n",distance);
+ }
+ }
+
+ wait_ms(200);
+ fermo();
+ }
+ }
+ }
+}
+
+void check_stop_thread(void const *args)
+{
+ /*thread che controlla se l'utente via dispositivi ha chiesto lo stop del proamma automatico*/
+ unsigned char rx;
+ while(true) {
+ if( device.readable() ) { //funzione simile alla Serial.available() di arduino, solo se il monitor e` attivo permette di utilizzarlo
+ rx = device.getc(); //funzione simile alla Serial.read() di arduino, legge il carattere dal seriale del bluethoot
+ if( rx == 'g' )
+ flagAutomatic = false;
+ }
+ }
+}
+
+void check_sirena_thread(void const *args)
+{
+ /*thread che controlla sempre il pin dedicato alla sirena, se il pin diventa alto comunica il cambiamento
+ con un flag al main che si adatta alla situazione*/
+ DigitalIn in(CTRL_SIRENA);
+ while(true){
+ int val = in.read();
+ if( val == 1 )
+ flagSirena = false;
+ else
+ flagSirena = true;
+ }
+}
+
+bool checkFermo()
+{
+ /*funzione utilizzata per evitare che disturbi dell ultrasuono vengano presi in considerazione come valori utili
+ La funzione controlla per un TOT volte se il valore e' attendibile e in caso positivo ritorna TRUE*/
+ bool ret = true;
+ int avantiDis, ctrl = 50;
+ for(int i = 0; i < ctrl; i++){
+ ultrasuoniAvanti.start();
+ avantiDis = ultrasuoniAvanti.get_dist_cm();
+ if( avantiDis >= DISTANCE_CTRL ){
+ ret = false;
+ break;
+ }
+ wait_ms(10);
+ }
+ return ret;
+}
+
void avanti()
{
motorBdirection = 1;