3FEN / Mbed 2 deprecated STEVEprogram15-05

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?

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