3FEN / Mbed 2 deprecated STEVEprogram22-05

programma steve 22-05

Dependencies:   HCSR04 mbed-rtos mbed

main.cpp@0:d8dd32751c38, 2017-05-23 (annotated)

Committer:
Wonderjack996
Date:
Tue May 23 06:49:48 2017 +0000
Revision:
0:d8dd32751c38
programma steve 22-05

Who changed what in which revision?

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