3FEN / Mbed 2 deprecated STEVEprogram23-05

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?

UserRevisionLine numberNew 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 }