Sweep a servo according to Proximity sensor measure
Dependencies: Servo X_NUCLEO_6180XA1 mbed
Fork of HelloWorld_6180XA1 by
Diff: main.cpp
- Revision:
- 15:b94bc967fecd
- Parent:
- 14:946e62f44f4f
- Child:
- 16:fbf61b7c65ba
--- a/main.cpp Tue Oct 27 15:54:05 2015 +0000 +++ b/main.cpp Wed Oct 28 13:23:58 2015 +0100 @@ -38,9 +38,6 @@ //int start, end; //DigitalOut pin(PA_15); -/* Create a serial object */ -Serial pc(USBTX, USBRX); - DevI2C device_i2c(VL6180X_I2C_SDA, VL6180X_I2C_SCL); static X_NUCLEO_6180XA1 *board=X_NUCLEO_6180XA1::Instance(&device_i2c); @@ -48,48 +45,52 @@ MeasureData_t data_sensor_top, data_sensor_bottom, data_sensor_left, data_sensor_right; -/* flags that handle the call to HandleIRQ function */ -bool flag_sensor_top=false, flag_sensor_bottom=false, flag_sensor_left=false, flag_sensor_right=false; +/* flags that handle interrupt request */ +bool flag_sensor_top=false, flag_sensor_bottom=false, flag_sensor_left=false, flag_sensor_right=false, flag_stop_measure=false; + +/* user button used to stop measurements in interrupt mode */ +InterruptIn stop_button(USER_BUTTON); /* callback functions of the sensors */ -static void SensorTopIRQ(void) +void SensorTopIRQ(void) { flag_sensor_top=true; board->sensor_top->DisableInterruptMeasureDetectionIRQ(); } -static void SensorBottomIRQ(void) +void SensorBottomIRQ(void) { flag_sensor_bottom=true; board->sensor_bottom->DisableInterruptMeasureDetectionIRQ(); } -static void SensorLeftIRQ(void) +void SensorLeftIRQ(void) { flag_sensor_left=true; board->sensor_left->DisableInterruptMeasureDetectionIRQ(); } -static void SensorRightIRQ(void) +void SensorRightIRQ(void) { flag_sensor_right=true; board->sensor_right->DisableInterruptMeasureDetectionIRQ(); } -void OnErrLog(void){}; // FIXME sistemare la funzione per stampare il messaggio che gli viene passato - // posso ridirigere la OnErrLog come pc.printf(__VA_ARGS__), ma deve essere OnErrLog(...) -void DISP_ExecLoopBody(void){}; //FIXME sistemare questa funzione +void StopMeasureIRQ(void) +{ + flag_stop_measure=true; +} int main() { int status, status_t, status_b, status_l, status_r; - //device_i2c.frequency(300000); //change i2c frequncy from 100kHz to 400kHz + //device_i2c.frequency(400000); //change i2c frequncy from 100kHz to 400kHz pc.baud(115200); //change baudrate of the printf status=board->InitBoard(); if(status) - VL6180x_ErrLog("Failed to init the board!\n\r"); + printf("Failed to init the board!\n\r"); #ifdef RANGE_SINGLE_SHOT_POLLING status_t=board->sensor_top->StartMeasurement(range_single_shot_polling, NULL, &data_sensor_top, NULL, NULL); @@ -107,7 +108,7 @@ #ifdef ALS_SINGLE_SHOT_POLLING status_t=board->sensor_top->StartMeasurement(als_single_shot_polling, NULL, &data_sensor_top, NULL, NULL); if(!status_t) - printf("Top Light: %dlux\n\r",data_sensor_top.lux); + printf("Top Light: %d lux\n\r",data_sensor_top.lux); else if(status==INVALID_PARAMS) printf("Failed to start measurement!\n\r"); else @@ -147,7 +148,7 @@ { status=board->sensor_top->GetMeasurement(als_continuous_polling, &data_sensor_top); if(!status_t) - printf("Top Light measure %d: %dlux\n\r",i,data_sensor_top.lux); + printf("Top Light measure %d: %d lux\n\r",i,data_sensor_top.lux); else printf("Invalid light value!\n\r"); } @@ -160,6 +161,7 @@ #endif #ifdef RANGE_CONTINUOUS_INTERRUPT + stop_button.rise(&StopMeasureIRQ); status=board->sensor_top->StartMeasurement(range_continuous_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL); if(!status) { @@ -174,16 +176,19 @@ else printf("Invalid range value!\n\r"); } + if(flag_stop_measure) + break; } } else printf("Failed to start measurement!\n\r"); - status=board->sensor_top->StopMeasurement(range_continuous_interrupt); //FIXME per questo modo operativo e per i successivi ad interrupt si deve sistemare la funzione di StopMeasurement in quanto il main sta sempre dentro il ciclo while e quindi non viene mai chiamata StopMeasurement + status=board->sensor_top->StopMeasurement(range_continuous_interrupt); if(status) printf("Failed to stop measurement!\n\r"); #endif #ifdef ALS_CONTINUOUS_INTERRUPT + stop_button.rise(&StopMeasureIRQ); status_t=board->sensor_top->StartMeasurement(als_continuous_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL); if(!status_t) { @@ -194,10 +199,12 @@ flag_sensor_top=false; status_t=board->sensor_top->HandleIRQ(als_continuous_interrupt, &data_sensor_top); if(!status_t) - printf("Top Light: %dlux\n\r",data_sensor_top.lux); + printf("Top Light: %d lux\n\r",data_sensor_top.lux); else printf("Invalid light value!\n\r"); } + if(flag_stop_measure) + break; } } else @@ -207,7 +214,8 @@ printf("Failed to stop measurement!\n\r"); #endif -#ifdef INTERLEAVED_MODE_INTERRUPT +#ifdef INTERLEAVED_MODE_INTERRUPT + stop_button.rise(&StopMeasureIRQ); status_t=board->sensor_top->StartMeasurement(interleaved_mode_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL); if(!status_t) { @@ -218,10 +226,12 @@ flag_sensor_top=false; status_t=board->sensor_top->HandleIRQ(interleaved_mode_interrupt, &data_sensor_top); if(!status_t) - printf("Top Range: %dmm\tTop Light: %dlux\n\r",data_sensor_top.range_mm, data_sensor_top.lux); + printf("Top Range: %dmm\tTop Light: %d lux\n\r",data_sensor_top.range_mm, data_sensor_top.lux); else printf("Invalid range or light value!\n\r"); } + if(flag_stop_measure) + break; } } else @@ -232,6 +242,7 @@ #endif #ifdef RANGE_CONTINUOUS_INTERRUPT_LOW_THRESHOLD + stop_button.rise(&StopMeasureIRQ); status_t=board->sensor_top->StartMeasurement(range_continuous_interrupt_low_threshold, SensorTopIRQ, &data_sensor_top, 80, NULL); if(!status_t) { @@ -246,6 +257,8 @@ else printf("Invalid range value!\n\r"); } + if(flag_stop_measure) + break; } } else @@ -255,15 +268,19 @@ printf("Failed to stop measurement!\n\r"); #endif -/* Test application Hand Gesture */ +/* Hand Gesture test application */ #ifdef HAND_GESTURE +#define ALS_DELAY 5000 int gest_code, prev_gest_code, gest_duration; char str[5]; char valLeft[] = {'E'}; char valRight[] = {'3'}; - char valTap[] = {'0'}; + char valTap[] = {'0'}; + unsigned int timeout = 10000; + status_l=board->sensor_left->StartMeasurement(range_continuous_interrupt, SensorLeftIRQ, &data_sensor_left, NULL, NULL); status_r=board->sensor_right->StartMeasurement(range_continuous_interrupt, SensorRightIRQ, &data_sensor_right, NULL, NULL); + status_t=board->sensor_top->StartMeasurement(als_continuous_polling, NULL, &data_sensor_top, NULL, NULL); gesture_reco.Init(150, 100, 200, 60, 500); if((!status_l)&&(!status_r)) { @@ -273,22 +290,31 @@ { flag_sensor_left=false; status_l=board->sensor_left->HandleIRQ(range_continuous_interrupt, &data_sensor_left); - //printf("Left range=%dmm\n\r",data_sensor_left.range_mm); } if(flag_sensor_right) { flag_sensor_right=false; status_r=board->sensor_right->HandleIRQ(range_continuous_interrupt, &data_sensor_right); - //printf("Right range=%dmm\n\r",data_sensor_right.range_mm); } if(data_sensor_left.range_mm>400) data_sensor_left.range_mm=400; if(data_sensor_right.range_mm>400) data_sensor_right.range_mm=400; gest_code=gesture_reco.Update(data_sensor_right.range_mm, data_sensor_left.range_mm, &gest_duration); + if(timeout==ALS_DELAY && gest_code==-1) + { + status=board->sensor_top->GetMeasurement(als_continuous_polling, &data_sensor_top); + if(!status_t) + printf("Light measure: %d lux\n\r",data_sensor_top.lux); + else + printf("Invalid light value!\n\r"); + } switch(gest_code) { case -1: + timeout++; + if(timeout>=ALS_DELAY) + timeout=ALS_DELAY; break; case 0: //tap and start timer if(gest_code!=prev_gest_code) @@ -310,12 +336,15 @@ board->display->DisplayDigit(valTap, 1); } } + timeout=0; break; case 1: //left to right - board->display->DisplayDigit(valRight, 3); + board->display->DisplayDigit(valRight, 3); + timeout=0; break; case 2: //right to left board->display->DisplayDigit(valLeft, 0); + timeout=0; break; default: printf("Errore!\n\r"); @@ -332,6 +361,8 @@ printf("Failed to stop measurement!\n\r"); #endif +delete board; + }