programma commando motori
Dependencies: mbed HCSR04 mbed-rtos
main.cpp
- Committer:
- Wonderjack996
- Date:
- 2017-06-09
- Revision:
- 4:088a0fad279a
- Parent:
- 2:7df0fc7d8d3c
- Child:
- 5:f4923705f199
File content as of revision 4:088a0fad279a:
/*---LIBRERIE---*/ #include "mbed.h" #include "rtos.h" #include "hcsr04.h" /*--------------*/ /*---COSTANTI---*/ #define MAX 999999 //valore intero massimo utilizzabile #define CTRL_SIRENA D11 //D11 - pin aggancio sensore sonoro #define DISTANCE_CTRL 15 /*decido una distanza, in cm, per i vari controlli dell' ultrasuono, esempio utilizzo come valore per capire quando la macchina si deve fermare*/ #define NUMBER_CRC 9 //numero utilizzato per controlli nell avanzamento automatico (vedi nella funzione che lo utilizza per spiegazione approfondita) #define LED_AUTOMATIC PB_2 //attacco led controllo modalita automatica /*---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 /*--------------------*/ /*--------------*/ /*---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); /*--------------------------*/ DigitalOut ledAutomatic(LED_AUTOMATIC); //pin in rifermineto al led controllo mod automatica 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 bool flagStopAvanti; /*----------------*/ /*---PROTOTIPI---*/ void avanti(); void indietro(); void destra(); void sinistra(); void fermo(); bool checkFermo(); void automaticProgram(); void setup(); void check_sirena_thread(void const *args); void check_stop_thread(void const *args); void check_stop_avanti(void const *args); /*---------------*/ int main() { unsigned char rx; int distanceTemp; setup(); //settaggio iniziale del robot Thread sirenaThread(check_sirena_thread); // start Thread controllo sirena Thread ctrlStopThread(check_stop_thread); // start Thread controllo fine processo automatico Thread ctrlStopAvanti(check_stop_avanti); // start Thread controllo fine processo avanzamentofunn 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 switch(rx){ //in base alla lettera che riceve sa cosa deve fare case 'a': flagStopAvanti = true; /*il thread check_stop_avanti viene avviato appena tale flag diventa true*/ /*a causa di problemi riscontrati con l'utilizzo dell'ultrasuono si e' deciso di ripere l'operazione di acquisizione dati dall'ultrasuono per piu' volte (tante quante NUMBER_CRC) in modo da essere sicuri del valore ottenuto*/ for(int j = 0; j < NUMBER_CRC && flagStopAvanti; j++){ distanceTemp = MAX; for( int i = 0; i < 100 && ( distanceTemp < 2 || distanceTemp > 400 && flagStopAvanti ); i++ ){ //questo perche il range dell'ultrasuono usato va da 2 cm a 4 m ( 400 cm ) ultrasuoniAvanti.start(); distanceTemp = ultrasuoniAvanti.get_dist_cm(); //wait_ms(10); } while( distanceTemp > 8 && flagStopAvanti ){ avanti(); ultrasuoniAvanti.start(); distanceTemp = ultrasuoniAvanti.get_dist_cm(); } fermo(); } break; case 'b': indietro(); break; case 'c': destra(); break; case 'd': sinistra(); break; case 'e': fermo(); break; case 'f': flagAutomatic = true; ledAutomatic = 1; //accensione led mod automatica automaticProgram(); //funzione pilota automatico ledAutomatic = 0; //spegnimento led break; } } } } void setup() { 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 ) fermo(); } 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; 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*/ distanceDx = MAX; distanceSx = MAX; 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(150); fermo(); } } } } void check_stop_avanti(void const *args) { /*questo thread ha il compito di attendere che l utente richieda lo stop del mezzo, quando cio avviene il thread setta il flag a false e interrompe sia se stesso che la funzione avanti*/ unsigned char rx; while(true) while( flagStopAvanti ) 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 == 'e' ) flagStopAvanti = false; } } 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) while( flagAutomatic ) 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; motorAdirection = 0; motorBpower.pulsewidth(0.01); // 100% motorApower.pulsewidth(0.01); // 100% } void indietro() { motorBdirection = 0; motorAdirection = 1; motorBpower.pulsewidth(0.01); // 100% motorApower.pulsewidth(0.01); // 100% } void destra() { motorBdirection = 0; motorAdirection = 0; motorBpower.pulsewidth(0.01); // 100% motorApower.pulsewidth(0.007); // 70% } void sinistra() { motorBdirection = 1; motorAdirection = 1; motorBpower.pulsewidth(0.007); // 100% motorApower.pulsewidth(0.01); // 100% } void fermo() { motorBpower.pulsewidth(0); // fermo motorApower.pulsewidth(0); // fermo }