programma steve fine giornata 23-05
Dependencies: HCSR04 mbed-rtos mbed
main.cpp@0:2fa228a32a83, 2017-05-24 (annotated)
- Committer:
- Wonderjack996
- Date:
- Wed May 24 08:04:12 2017 +0000
- Revision:
- 0:2fa228a32a83
programma steve fine giornata 23-05
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Wonderjack996 | 0:2fa228a32a83 | 1 | /*---LIBRERIE---*/ |
Wonderjack996 | 0:2fa228a32a83 | 2 | #include "mbed.h" |
Wonderjack996 | 0:2fa228a32a83 | 3 | #include "rtos.h" |
Wonderjack996 | 0:2fa228a32a83 | 4 | #include "hcsr04.h" |
Wonderjack996 | 0:2fa228a32a83 | 5 | /*--------------*/ |
Wonderjack996 | 0:2fa228a32a83 | 6 | |
Wonderjack996 | 0:2fa228a32a83 | 7 | /*---COSTANTI---*/ |
Wonderjack996 | 0:2fa228a32a83 | 8 | #define PWMB PC_7 //D9 |
Wonderjack996 | 0:2fa228a32a83 | 9 | #define DIRB D8 //D8 |
Wonderjack996 | 0:2fa228a32a83 | 10 | |
Wonderjack996 | 0:2fa228a32a83 | 11 | #define PWMA PB_4 //D5 |
Wonderjack996 | 0:2fa228a32a83 | 12 | #define DIRA D4 //D4 |
Wonderjack996 | 0:2fa228a32a83 | 13 | |
Wonderjack996 | 0:2fa228a32a83 | 14 | #define avantiTrigger PA_5 //D13 |
Wonderjack996 | 0:2fa228a32a83 | 15 | #define avantiEcho PA_6 //D12 |
Wonderjack996 | 0:2fa228a32a83 | 16 | #define sinistraTrigger PA_8 //D7 |
Wonderjack996 | 0:2fa228a32a83 | 17 | #define sinistraEcho PB_10 //D6 |
Wonderjack996 | 0:2fa228a32a83 | 18 | #define destraTrigger PA_10 //D2 |
Wonderjack996 | 0:2fa228a32a83 | 19 | #define destraEcho PB_3 //D3 |
Wonderjack996 | 0:2fa228a32a83 | 20 | |
Wonderjack996 | 0:2fa228a32a83 | 21 | #define CTRL_SIRENA D11 //D11 |
Wonderjack996 | 0:2fa228a32a83 | 22 | /*--------------*/ |
Wonderjack996 | 0:2fa228a32a83 | 23 | |
Wonderjack996 | 0:2fa228a32a83 | 24 | /*---VARIABILI GLOBALI---*/ |
Wonderjack996 | 0:2fa228a32a83 | 25 | PwmOut motorApower(PWMA); //istanzio oggetto per controllo motore A ( vedi scheda per capire quale sia ) |
Wonderjack996 | 0:2fa228a32a83 | 26 | DigitalOut motorAdirection(DIRA); |
Wonderjack996 | 0:2fa228a32a83 | 27 | |
Wonderjack996 | 0:2fa228a32a83 | 28 | PwmOut motorBpower(PWMB); //istanzio oggetto per controllo motore B ( vedi scheda per capire quale sia ) |
Wonderjack996 | 0:2fa228a32a83 | 29 | DigitalOut motorBdirection(DIRB); |
Wonderjack996 | 0:2fa228a32a83 | 30 | |
Wonderjack996 | 0:2fa228a32a83 | 31 | HCSR04 ultrasuoniAvanti(avantiTrigger, avantiEcho); //istanzio oggetti per controllo ultrasuoni |
Wonderjack996 | 0:2fa228a32a83 | 32 | HCSR04 ultrasuoniSinistra(sinistraTrigger, sinistraEcho); |
Wonderjack996 | 0:2fa228a32a83 | 33 | HCSR04 ultrasuoniDestra(destraTrigger, destraEcho); |
Wonderjack996 | 0:2fa228a32a83 | 34 | |
Wonderjack996 | 0:2fa228a32a83 | 35 | |
Wonderjack996 | 0:2fa228a32a83 | 36 | Serial pc(USBTX, USBRX); //istanzio oggetto per utilizzo monitor seriale |
Wonderjack996 | 0:2fa228a32a83 | 37 | |
Wonderjack996 | 0:2fa228a32a83 | 38 | int distanceCtrl = 10; /*decido una distanza, in cm, per i vari controlli dell' ultrasuono, esempio utilizzo come valore |
Wonderjack996 | 0:2fa228a32a83 | 39 | per capire quando la macchina si deve fermare*/ |
Wonderjack996 | 0:2fa228a32a83 | 40 | |
Wonderjack996 | 0:2fa228a32a83 | 41 | bool flagSirena = true; //utilizzato per bloccare il robot nel caso senta una sirena |
Wonderjack996 | 0:2fa228a32a83 | 42 | /*-----------------------*/ |
Wonderjack996 | 0:2fa228a32a83 | 43 | |
Wonderjack996 | 0:2fa228a32a83 | 44 | |
Wonderjack996 | 0:2fa228a32a83 | 45 | /*---PROTOTIPI DELLE FUNZIONI---*/ |
Wonderjack996 | 0:2fa228a32a83 | 46 | void avanti(); |
Wonderjack996 | 0:2fa228a32a83 | 47 | void indietro(); |
Wonderjack996 | 0:2fa228a32a83 | 48 | void destra(); |
Wonderjack996 | 0:2fa228a32a83 | 49 | void sinistra(); |
Wonderjack996 | 0:2fa228a32a83 | 50 | void fermo(); |
Wonderjack996 | 0:2fa228a32a83 | 51 | bool checkFermo(); |
Wonderjack996 | 0:2fa228a32a83 | 52 | void check_sirena_thread(void const *args); |
Wonderjack996 | 0:2fa228a32a83 | 53 | /*-------------------------------*/ |
Wonderjack996 | 0:2fa228a32a83 | 54 | |
Wonderjack996 | 0:2fa228a32a83 | 55 | int main() { |
Wonderjack996 | 0:2fa228a32a83 | 56 | |
Wonderjack996 | 0:2fa228a32a83 | 57 | int avantiDis, sinistraDis; |
Wonderjack996 | 0:2fa228a32a83 | 58 | |
Wonderjack996 | 0:2fa228a32a83 | 59 | Thread sirenaThread(check_sirena_thread); // start Thread controllo sirena |
Wonderjack996 | 0:2fa228a32a83 | 60 | |
Wonderjack996 | 0:2fa228a32a83 | 61 | motorBpower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare ) |
Wonderjack996 | 0:2fa228a32a83 | 62 | motorApower.period_ms(10); |
Wonderjack996 | 0:2fa228a32a83 | 63 | |
Wonderjack996 | 0:2fa228a32a83 | 64 | fermo(); |
Wonderjack996 | 0:2fa228a32a83 | 65 | |
Wonderjack996 | 0:2fa228a32a83 | 66 | while(true){ //utilizzo per mandare in loop la funzione principale ( main tipo ) |
Wonderjack996 | 0:2fa228a32a83 | 67 | while(flagSirena){ //il robot si puo' muovere liberamente finche non sente il suono della sirena tipo ambulanza |
Wonderjack996 | 0:2fa228a32a83 | 68 | ultrasuoniAvanti.start(); |
Wonderjack996 | 0:2fa228a32a83 | 69 | avantiDis = ultrasuoniAvanti.get_dist_cm(); |
Wonderjack996 | 0:2fa228a32a83 | 70 | //pc.printf("avanti: %d \r\n",avantiDis); |
Wonderjack996 | 0:2fa228a32a83 | 71 | while( avantiDis >= distanceCtrl && flagSirena ){ //finche il valore letto dall' ultrasuono e' maggiore della distanza di urto la macchina puo' procedere |
Wonderjack996 | 0:2fa228a32a83 | 72 | avanti(); |
Wonderjack996 | 0:2fa228a32a83 | 73 | ultrasuoniAvanti.start(); |
Wonderjack996 | 0:2fa228a32a83 | 74 | avantiDis = ultrasuoniAvanti.get_dist_cm(); |
Wonderjack996 | 0:2fa228a32a83 | 75 | //pc.printf("avanti: %d \r\n",avantiDis); |
Wonderjack996 | 0:2fa228a32a83 | 76 | wait_ms(10); |
Wonderjack996 | 0:2fa228a32a83 | 77 | } |
Wonderjack996 | 0:2fa228a32a83 | 78 | |
Wonderjack996 | 0:2fa228a32a83 | 79 | fermo(); |
Wonderjack996 | 0:2fa228a32a83 | 80 | |
Wonderjack996 | 0:2fa228a32a83 | 81 | if( checkFermo() ){ |
Wonderjack996 | 0:2fa228a32a83 | 82 | destra(); |
Wonderjack996 | 0:2fa228a32a83 | 83 | wait_ms(1000); |
Wonderjack996 | 0:2fa228a32a83 | 84 | |
Wonderjack996 | 0:2fa228a32a83 | 85 | ultrasuoniSinistra.start(); |
Wonderjack996 | 0:2fa228a32a83 | 86 | sinistraDis = ultrasuoniSinistra.get_dist_cm(); |
Wonderjack996 | 0:2fa228a32a83 | 87 | //pc.printf("sinistra: %d \r\n",sinistraDis); |
Wonderjack996 | 0:2fa228a32a83 | 88 | while(sinistraDis >= distanceCtrl && flagSirena ){ //il robot continua la rotazione per evitare l' ostacolo finche l' ultrasuono di destra lo avverte di aver trovato una superficie piana |
Wonderjack996 | 0:2fa228a32a83 | 89 | ultrasuoniSinistra.start(); |
Wonderjack996 | 0:2fa228a32a83 | 90 | sinistraDis = ultrasuoniSinistra.get_dist_cm(); |
Wonderjack996 | 0:2fa228a32a83 | 91 | //pc.printf("sinistra: %d \r\n",sinistraDis); |
Wonderjack996 | 0:2fa228a32a83 | 92 | } |
Wonderjack996 | 0:2fa228a32a83 | 93 | wait_ms(200); |
Wonderjack996 | 0:2fa228a32a83 | 94 | |
Wonderjack996 | 0:2fa228a32a83 | 95 | fermo(); |
Wonderjack996 | 0:2fa228a32a83 | 96 | } |
Wonderjack996 | 0:2fa228a32a83 | 97 | } |
Wonderjack996 | 0:2fa228a32a83 | 98 | } |
Wonderjack996 | 0:2fa228a32a83 | 99 | |
Wonderjack996 | 0:2fa228a32a83 | 100 | } |
Wonderjack996 | 0:2fa228a32a83 | 101 | |
Wonderjack996 | 0:2fa228a32a83 | 102 | void check_sirena_thread(void const *args) { |
Wonderjack996 | 0:2fa228a32a83 | 103 | /*thread che controlla sempre il pin dedicato alla sirena, se il pin diventa alto comunica il cambiamento |
Wonderjack996 | 0:2fa228a32a83 | 104 | con un flag al main che si adatta alla situazione*/ |
Wonderjack996 | 0:2fa228a32a83 | 105 | DigitalIn in(CTRL_SIRENA); |
Wonderjack996 | 0:2fa228a32a83 | 106 | while(true){ |
Wonderjack996 | 0:2fa228a32a83 | 107 | int val = in.read(); |
Wonderjack996 | 0:2fa228a32a83 | 108 | if( val == 1 ) |
Wonderjack996 | 0:2fa228a32a83 | 109 | flagSirena = false; |
Wonderjack996 | 0:2fa228a32a83 | 110 | else |
Wonderjack996 | 0:2fa228a32a83 | 111 | flagSirena = true; |
Wonderjack996 | 0:2fa228a32a83 | 112 | } |
Wonderjack996 | 0:2fa228a32a83 | 113 | } |
Wonderjack996 | 0:2fa228a32a83 | 114 | |
Wonderjack996 | 0:2fa228a32a83 | 115 | bool checkFermo() |
Wonderjack996 | 0:2fa228a32a83 | 116 | { |
Wonderjack996 | 0:2fa228a32a83 | 117 | /*funzione utilizzata per evitare che disturbi dell ultrasuono vengano presi in considerazione come valori utili |
Wonderjack996 | 0:2fa228a32a83 | 118 | La funzione controlla per un TOT volte se il valore e' attendibile e in caso positivo ritorna TRUE*/ |
Wonderjack996 | 0:2fa228a32a83 | 119 | bool ret = true; |
Wonderjack996 | 0:2fa228a32a83 | 120 | int avantiDis, ctrl = 50; |
Wonderjack996 | 0:2fa228a32a83 | 121 | for(int i = 0; i < ctrl; i++){ |
Wonderjack996 | 0:2fa228a32a83 | 122 | ultrasuoniAvanti.start(); |
Wonderjack996 | 0:2fa228a32a83 | 123 | avantiDis = ultrasuoniAvanti.get_dist_cm(); |
Wonderjack996 | 0:2fa228a32a83 | 124 | if( avantiDis >= distanceCtrl ){ |
Wonderjack996 | 0:2fa228a32a83 | 125 | ret = false; |
Wonderjack996 | 0:2fa228a32a83 | 126 | break; |
Wonderjack996 | 0:2fa228a32a83 | 127 | } |
Wonderjack996 | 0:2fa228a32a83 | 128 | wait_ms(10); |
Wonderjack996 | 0:2fa228a32a83 | 129 | } |
Wonderjack996 | 0:2fa228a32a83 | 130 | return ret; |
Wonderjack996 | 0:2fa228a32a83 | 131 | } |
Wonderjack996 | 0:2fa228a32a83 | 132 | |
Wonderjack996 | 0:2fa228a32a83 | 133 | void avanti() |
Wonderjack996 | 0:2fa228a32a83 | 134 | { |
Wonderjack996 | 0:2fa228a32a83 | 135 | motorBdirection = 1; |
Wonderjack996 | 0:2fa228a32a83 | 136 | motorAdirection = 0; |
Wonderjack996 | 0:2fa228a32a83 | 137 | motorBpower.pulsewidth(0.01); // 100% |
Wonderjack996 | 0:2fa228a32a83 | 138 | motorApower.pulsewidth(0.01); // 100% |
Wonderjack996 | 0:2fa228a32a83 | 139 | } |
Wonderjack996 | 0:2fa228a32a83 | 140 | |
Wonderjack996 | 0:2fa228a32a83 | 141 | void indietro() |
Wonderjack996 | 0:2fa228a32a83 | 142 | { |
Wonderjack996 | 0:2fa228a32a83 | 143 | motorBdirection = 0; |
Wonderjack996 | 0:2fa228a32a83 | 144 | motorAdirection = 1; |
Wonderjack996 | 0:2fa228a32a83 | 145 | motorBpower.pulsewidth(0.01); // 100% |
Wonderjack996 | 0:2fa228a32a83 | 146 | motorApower.pulsewidth(0.01); // 100% |
Wonderjack996 | 0:2fa228a32a83 | 147 | } |
Wonderjack996 | 0:2fa228a32a83 | 148 | |
Wonderjack996 | 0:2fa228a32a83 | 149 | void destra() |
Wonderjack996 | 0:2fa228a32a83 | 150 | { |
Wonderjack996 | 0:2fa228a32a83 | 151 | motorBdirection = 0; |
Wonderjack996 | 0:2fa228a32a83 | 152 | motorAdirection = 0; |
Wonderjack996 | 0:2fa228a32a83 | 153 | motorBpower.pulsewidth(0.01); // 100% |
Wonderjack996 | 0:2fa228a32a83 | 154 | motorApower.pulsewidth(0.007); // 70% |
Wonderjack996 | 0:2fa228a32a83 | 155 | } |
Wonderjack996 | 0:2fa228a32a83 | 156 | |
Wonderjack996 | 0:2fa228a32a83 | 157 | void sinistra() |
Wonderjack996 | 0:2fa228a32a83 | 158 | { |
Wonderjack996 | 0:2fa228a32a83 | 159 | motorBdirection = 1; |
Wonderjack996 | 0:2fa228a32a83 | 160 | motorAdirection = 1; |
Wonderjack996 | 0:2fa228a32a83 | 161 | motorBpower.pulsewidth(0.007); // 100% |
Wonderjack996 | 0:2fa228a32a83 | 162 | motorApower.pulsewidth(0.01); // 100% |
Wonderjack996 | 0:2fa228a32a83 | 163 | } |
Wonderjack996 | 0:2fa228a32a83 | 164 | |
Wonderjack996 | 0:2fa228a32a83 | 165 | void fermo() |
Wonderjack996 | 0:2fa228a32a83 | 166 | { |
Wonderjack996 | 0:2fa228a32a83 | 167 | motorBpower.pulsewidth(0); // fermo |
Wonderjack996 | 0:2fa228a32a83 | 168 | motorApower.pulsewidth(0); // fermo |
Wonderjack996 | 0:2fa228a32a83 | 169 | } |