Siren
thread
Dependencies: HCSR04 mbed-rtos mbed
Fork of
MotorShield_futura
by 
3FEN
Diff: main.cpp
- Revision:
- 3:2816fec3acee
- Parent:
- 2:7df0fc7d8d3c
diff -r 7df0fc7d8d3c -r 2816fec3acee main.cpp
--- a/main.cpp Thu May 25 07:12:44 2017 +0000
+++ b/main.cpp Thu May 03 14:52:49 2018 +0000
@@ -6,11 +6,27 @@
/*---COSTANTI---*/
#define MAX 999999 //valore intero massimo utilizzabile
-
- #define CTRL_SIRENA D11 //D11 - pin aggancio sensore sonoro
+
+ #define DISTANCE_CTRL 20 /*decido una distanza, in cm, per i vari controlli dell' ultrasuono, esempio utilizzo come valore
+ per capire quando la macchina si deve fermare*/
+
+ #define NUM_CICLIC_CTRL 200 /*costante indicante il numero di volte che acquisisco un valore dall'ultrasuoni prima di dicidere se sia affidabile*/
+
+ #define MIN_RANGE_ULTRASOUND 5 /*indica, in centimetri, il range minimo dell'ultrasuono*/
- #define DISTANCE_CTRL 10 /*decido una distanza, in cm, per i vari controlli dell' ultrasuono, esempio utilizzo come valore
- per capire quando la macchina si deve fermare*/
+ #define MAX_RANGE_ULTRASOUND 400 /*indica, in centimetri, il range massimo dell'ultrasuono*/
+
+ #define NOT_CONTROL_TIME 300 /*tempo in cui il robot esegue movimenti senza controlli da parte dei sensori*/
+
+ #define NUMBER_CRC 9 //numero utilizzato per controlli nell avanzamento automatico (vedi nella funzione che lo utilizza per spiegazione approfondita)
+
+
+ #define LED_AUTOMATIC PB_2 //attacco led controllo modalita automatica
+
+ /*---COSTANTI PER GESTIONE SIRENA---*/
+ #define MIN_CHECK 50
+ #define PIN_SIRENA D11 //D11 - pin aggancio sensore sonoro
+ /*----------------------------------*/
/*---PIN BLUETHOOT---*/
/*I pin utilizzati per TX e RX sono pin morfo, a livello hardware collegare il pin TX dell HC05 al PA_12 e quello RX a PA_11 ( a livello harware vanno invertiti )*/
@@ -51,6 +67,10 @@
HCSR04 ultrasuoniDestra(DESTRA_TRIGGER, DESTRA_ECHO);
/*--------------------------*/
+ DigitalOut ledAutomatic(LED_AUTOMATIC); //pin in rifermineto al led controllo mod automatica
+
+ DigitalIn sirena(PIN_SIRENA); //pin in riferimento all'aggancio del sensore sonoro
+
Serial pc(USBTX, USBRX); //istanzio oggetto per utilizzo monitor seriale
Serial device(TX, RX); //oggetto per gestione seriale bluethoot
@@ -58,6 +78,8 @@
bool flagSirena = true; //utilizzato per bloccare il robot nel caso senta una sirena
bool flagAutomatic; //utilizzo per sapere se utilizzare modalita automatica o meno
+
+ bool flagStopAvanti;
/*----------------*/
/*---PROTOTIPI---*/
@@ -69,23 +91,45 @@
bool checkFermo();
void automaticProgram();
void check_sirena_thread(void const *args);
- void check_stop_thread(void const *args);
+ void check_stop_thread(void const *args);
+ void check_stop_avanti(void const *args);
/*---------------*/
int main() {
unsigned char rx;
+ int distanceTemp;
motorBpower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare )
motorApower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare )
+ Thread sirenaThread(check_sirena_thread); // start Thread controllo sirena
+ Thread ctrlStopThread(check_stop_thread); // start Thread controllo fine processo automatico
+ Thread ctrlStopAvanti(check_stop_avanti); // start Thread controllo fine processo avanzamento
+
while(true){
if( device.readable() ) { //funzione simile alla Serial.available() di arduino, solo se il monitor e` attivo permette di utilizzarlo
rx = device.getc(); //funzione simile alla Serial.read() di arduino, legge il carattere dal seriale del bluethoot
//pc.printf("recived: %c \r\n",rx); //stampa a video
switch(rx){ //in base alla lettera che riceve sa cosa deve fare
case 'a':
- avanti();
+ flagStopAvanti = true; /*il thread check_stop_avanti viene avviato appena tale flag diventa true*/
+ /*a causa di problemi riscontrati con l'utilizzo dell'ultrasuono si e' deciso di ripere l'operazione di acquisizione
+ dati dall'ultrasuono per piu' volte (tante quante NUMBER_CRC) in modo da essere sicuri del valore ottenuto*/
+ for(int j = 0; j < NUMBER_CRC && flagStopAvanti; j++){
+ distanceTemp = MAX;
+ for( int i = 0; i < NUM_CICLIC_CTRL && ( distanceTemp < MIN_RANGE_ULTRASOUND || distanceTemp > MAX_RANGE_ULTRASOUND && flagStopAvanti ); i++ ){ //questo perche il range dell'ultrasuono usato va da 2 cm a 4 m ( 400 cm )
+ ultrasuoniAvanti.start();
+ distanceTemp = ultrasuoniAvanti.get_dist_cm();
+ //wait_ms(10);
+ }
+ while( distanceTemp > 8 && flagStopAvanti ){
+ avanti();
+ ultrasuoniAvanti.start();
+ distanceTemp = ultrasuoniAvanti.get_dist_cm();
+ }
+ fermo();
+ }
break;
case 'b':
indietro();
@@ -101,7 +145,9 @@
break;
case 'f':
flagAutomatic = true;
+ ledAutomatic = 1; //accensione led mod automatica
automaticProgram(); //funzione pilota automatico
+ ledAutomatic = 0; //spegnimento led
break;
}
}
@@ -115,9 +161,6 @@
int distanceDx, distanceSx;
- Thread sirenaThread(check_sirena_thread); // start Thread controllo sirena
- Thread ctrlStopThread(check_stop_thread); // start Thread controllo sirena
-
fermo();
while(flagAutomatic){ //flag modificabile dal thread che aspetta il segnale di stop
@@ -138,13 +181,15 @@
if( checkFermo() && flagSirena && flagAutomatic ){
/*funzione utilizzata per leggere un valore attendibile ( nel range dell ultrasuono ). se dopo 100 tentativi
il valore non e' ancora attendibile allora esco dal controllo in modo da non rimanere bloccato*/
- for( int i = 0; i < 100 && ( distanceDx < 2 || distanceDx > 400 ); i++ ){ //questo perche il range dell'ultrasuono usato va da 2 cm a 4 m ( 400 cm )
+ distanceDx = MAX;
+ distanceSx = MAX;
+ for( int i = 0; i < NUM_CICLIC_CTRL && ( distanceDx < MIN_RANGE_ULTRASOUND || distanceDx > MAX_RANGE_ULTRASOUND ); i++ ){ //questo perche il range dell'ultrasuono usato va da 2 cm a 4 m ( 400 cm )
ultrasuoniDestra.start();
distanceDx = ultrasuoniDestra.get_dist_cm();
//wait_ms(10);
}
- for( int i = 0; i < 100 && ( distanceSx < 2 || distanceSx > 400 ); i++ ){ //questo perche il range dell'ultrasuono usato va da 2 cm a 4 m ( 400 cm )
+ for( int i = 0; i < NUM_CICLIC_CTRL && ( distanceSx < MIN_RANGE_ULTRASOUND || distanceSx > MAX_RANGE_ULTRASOUND ); i++ ){ //questo perche il range dell'ultrasuono usato va da 2 cm a 4 m ( 400 cm )
ultrasuoniSinistra.start();
distanceSx = ultrasuoniSinistra.get_dist_cm();
//wait_ms(10);
@@ -153,16 +198,25 @@
/*controllo se il valori valori di uscita dalle iterazioni sono attendibili ( nel range )
se non soddisfano i criteri allora li setto al valore MAX in modo che i seguenti controlli non li prendano
in considerazione*/
- if( distanceSx < 2 || distanceSx > 400 )
+ if( distanceSx < MIN_RANGE_ULTRASOUND || distanceSx > MAX_RANGE_ULTRASOUND ){
distanceSx = MAX;
+ distanceDx = MAX;
+ continue;
+ }
- if( distanceDx < 2 || distanceDx > 400 )
+ if( distanceDx < MIN_RANGE_ULTRASOUND || distanceDx > MAX_RANGE_ULTRASOUND ){
distanceDx = MAX;
+ distanceSx = MAX;
+ continue;
+ }
- pc.printf("distanceSx: %d distanceDx: %d\r\n",distanceSx, distanceDx);
+ /*if( distanceDx == MAX && distanceSx == MAX ) //mod veloce
+ continue;*/
+
+ //pc.printf("distanceSx: %d distanceDx: %d\r\n",distanceSx, distanceDx);
//in base ai valori letti capisco da che lato e' piu vicino la superficie piana e in base a cio sono da che parte girare
- if( distanceSx > distanceDx ){
+ if( distanceSx > distanceDx && distanceSx != MAX){
sinistra();
wait_ms(1000);
@@ -170,12 +224,12 @@
distance = ultrasuoniDestra.get_dist_cm();
//pc.printf("sinistra: %d \r\n",sinistraDis);
while( distance >= DISTANCE_CTRL && flagSirena && flagAutomatic ){ //il robot continua la rotazione per evitare l' ostacolo finche l' ultrasuono di destra lo avverte di aver trovato una superficie piana
- ultrasuoniDestra.start();
+ ultrasuoniDestra.start() ;
distance = ultrasuoniDestra.get_dist_cm();
//pc.printf("distance: %d \r\n",distance);
}
}
- else{
+ else if( distanceDx != MAX ){
destra();
wait_ms(1000);
@@ -189,38 +243,72 @@
}
}
- wait_ms(200);
+ wait_ms(NOT_CONTROL_TIME);
fermo();
}
}
}
}
+void check_stop_avanti(void const *args)
+{
+ /*questo thread ha il compito di attendere che l utente richieda lo stop del mezzo, quando cio avviene il thread setta il flag a
+ false e interrompe sia se stesso che la funzione avanti*/
+ unsigned char rx;
+ while(true)
+ while( flagStopAvanti )
+ if( device.readable() ) { //funzione simile alla Serial.available() di arduino, solo se il monitor e` attivo permette di utilizzarlo
+ rx = device.getc(); //funzione simile alla Serial.read() di arduino, legge il carattere dal seriale del bluethoot
+ if( rx == 'e' )
+ flagStopAvanti = false;
+ }
+}
+
void check_stop_thread(void const *args)
{
/*thread che controlla se l'utente via dispositivi ha chiesto lo stop del proamma automatico*/
unsigned char rx;
- while(true) {
- if( device.readable() ) { //funzione simile alla Serial.available() di arduino, solo se il monitor e` attivo permette di utilizzarlo
- rx = device.getc(); //funzione simile alla Serial.read() di arduino, legge il carattere dal seriale del bluethoot
- if( rx == 'g' )
- flagAutomatic = false;
- }
- }
+ while(true)
+ while( flagAutomatic )
+ if( device.readable() ) { //funzione simile alla Serial.available() di arduino, solo se il monitor e` attivo permette di utilizzarlo
+ rx = device.getc(); //funzione simile alla Serial.read() di arduino, legge il carattere dal seriale del bluethoot
+ if( rx == 'g' )
+ flagAutomatic = false;
+ }
}
void check_sirena_thread(void const *args)
{
- /*thread che controlla sempre il pin dedicato alla sirena, se il pin diventa alto comunica il cambiamento
- con un flag al main che si adatta alla situazione*/
- DigitalIn in(CTRL_SIRENA);
- while(true){
- int val = in.read();
+ /*la funzione controlla il pin dedicato al sensore sonoro per un lasso di tempo fisso.
+ Se in questo lasso di tempo riceve abbastanza valori che li indicano una sirena allora esce direttamente
+ dalla struttura di controllo e blocca il robot, se in questo lasso invece dopo un po non riceve alcun valore
+ attendibile allora esce della struttura di controllo prima del previsto e ricomincia i suo controlli da capo.*/
+ int val, contaHigh = 0, contaLow = 0;
+ int aumentoLow = 100; //rispetto al controllo dello stato HIGH, lo stato LOW e' controllato per piu' volte
+ while(true) {
+ for(int i = 0; i < 500 && ( contaHigh < MIN_CHECK ) && ( contaLow < MIN_CHECK+aumentoLow ); i++){
+ val = sirena.read();
+ //pc.printf("recived: %d \r\n",conta); //stampa a video
+ if( val == 1 )
+ contaHigh++;
+ else
+ if( val == 0 )
+ contaLow++;
+ wait_ms(5);
+ }
+ if( contaLow >= MIN_CHECK+aumentoLow )
+ flagSirena = true;
+ else
+ if( contaHigh >= MIN_CHECK )
+ flagSirena = true; //mettere FALSE dopo istallazione sensore sonoro
+ contaHigh = 0;
+ contaLow = 0;
+ /*val = sirena.read();
if( val == 1 )
flagSirena = false;
else
- flagSirena = true;
- }
+ flagSirena = true;*/
+ }
}
bool checkFermo()
@@ -269,7 +357,7 @@
{
motorBdirection = 1;
motorAdirection = 1;
- motorBpower.pulsewidth(0.007); // 100%
+ motorBpower.pulsewidth(0.007); // 700%
motorApower.pulsewidth(0.01); // 100%
}