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