Sweep a servo according to Proximity sensor measure
Dependencies: Servo X_NUCLEO_6180XA1 mbed
Fork of HelloWorld_6180XA1 by
Diff: main.cpp
- Revision:
- 14:946e62f44f4f
- Parent:
- 11:657dbe7bf245
- Child:
- 15:b94bc967fecd
--- a/main.cpp Tue Oct 27 15:30:25 2015 +0000 +++ b/main.cpp Tue Oct 27 15:54:05 2015 +0000 @@ -1,11 +1,14 @@ #include "mbed.h" #include "x_nucleo_6180xa1.h" - +#include "HmiBBGesture.h" #include <string.h> #include <stdlib.h> #include <stdio.h> #include <assert.h> +/* Polling operating modes don`t require callback function that handles IRQ + Callback functions are used only for measure that require interrupt */ + #define VL6180X_I2C_SDA I2C_SDA #define VL6180X_I2C_SCL I2C_SCL @@ -14,7 +17,7 @@ //#define RANGE_CONTINUOUS_POLLING //#define ALS_CONTINUOUS_POLLING //#define RANGE_CONTINUOUS_INTERRUPT -#define ALS_CONTINUOUS_INTERRUPT +//#define ALS_CONTINUOUS_INTERRUPT //#define INTERLEAVED_MODE_INTERRUPT //#define RANGE_CONTINUOUS_POLLING_LOW_THRESHOLD //#define RANGE_CONTINUOUS_POLLING_HIGH_THRESHOLD @@ -28,297 +31,306 @@ //#define ALS_CONTINUOUS_INTERRUPT_LOW_THRESHOLD //#define ALS_CONTINUOUS_INTERRUPT_HIGH_THRESHOLD //#define ALS_CONTINUOUS_INTERRUPT_OUT_OF_WINDOW +#define HAND_GESTURE -/* Polling operating modes (1,2,3 and 4) don`t require callback function that handles IRQ - Callback functions are used only for measure that require interrupt */ +/* timer and digital out pin used for debugging */ +//Timer timer; +//int start, end; +//DigitalOut pin(PA_15); -void OnErrLog(void){}; // sistemare la funzione per stampare il messaggio che gli viene passato -void DISP_ExecLoopBody(void){}; +/* 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); -MeasureData_t data_sensor_top, data_sensor_left, data_sensor_right; +HmiBBGesture gesture_reco; -Timer timer; // timer to debug -int start, end; +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; /* callback functions of the sensors */ -void SensorTopIRQ(void) +static void SensorTopIRQ(void) { flag_sensor_top=true; - board->sensor_top->DisableInterruptMeasureDetectionIRQ(); + board->sensor_top->DisableInterruptMeasureDetectionIRQ(); } -void SensorBottomIRQ(void) +static void SensorBottomIRQ(void) { flag_sensor_bottom=true; - board->sensor_bottom->DisableInterruptMeasureDetectionIRQ(); + board->sensor_bottom->DisableInterruptMeasureDetectionIRQ(); } -void SensorLeftIRQ(void) +static void SensorLeftIRQ(void) { flag_sensor_left=true; - board->sensor_left->DisableInterruptMeasureDetectionIRQ(); + board->sensor_left->DisableInterruptMeasureDetectionIRQ(); } -void SensorRightIRQ(void) +static void SensorRightIRQ(void) { flag_sensor_right=true; - board->sensor_right->DisableInterruptMeasureDetectionIRQ(); + 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 + int main() { - int status, status_t, status_b, status_l, status_r; - + int status, status_t, status_b, status_l, status_r; + + //device_i2c.frequency(300000); //change i2c frequncy from 100kHz to 400kHz + pc.baud(115200); //change baudrate of the printf + status=board->InitBoard(); - if(status) - printf("Failed to init the board!\n\r"); + if(status) + VL6180x_ErrLog("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); - if(!status_t) - printf("Top Range: %dmm\n\r",data_sensor_top.range_mm); - else if(status==INVALID_PARAMS) - printf("Failed to start measurement!\n\r"); - else - printf("Invalid range value!\n\r"); - status_t=board->sensor_top->StopMeasurement(range_single_shot_polling); - if(status_t) - printf("Failed to stop measurement!\n\r"); + status_t=board->sensor_top->StartMeasurement(range_single_shot_polling, NULL, &data_sensor_top, NULL, NULL); + if(!status_t) + printf("Top Range: %dmm\n\r",data_sensor_top.range_mm); + else if(status==INVALID_PARAMS) + printf("Failed to start measurement!\n\r"); + else + printf("Invalid range value!\n\r"); + status_t=board->sensor_top->StopMeasurement(range_single_shot_polling); + if(status_t) + printf("Failed to stop measurement!\n\r"); #endif #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); - else if(status==INVALID_PARAMS) - printf("Failed to start measurement!\n\r"); - else - printf("Invalid light value!\n\r"); - status_t=board->sensor_top->StopMeasurement(als_single_shot_polling); - if(status_t) - printf("Failed to stop measurement!\n\r"); + 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); + else if(status==INVALID_PARAMS) + printf("Failed to start measurement!\n\r"); + else + printf("Invalid light value!\n\r"); + status_t=board->sensor_top->StopMeasurement(als_single_shot_polling); + if(status_t) + printf("Failed to stop measurement!\n\r"); #endif #ifdef RANGE_CONTINUOUS_POLLING - status_t=board->sensor_top->StartMeasurement(range_continuous_polling, NULL, &data_sensor_top, NULL, NULL); + status_t=board->sensor_top->StartMeasurement(range_continuous_polling, NULL, &data_sensor_top, NULL, NULL); + if(!status_t) + { + int i; + for(i=0;i<10;i++) + { + status_t=board->sensor_top->GetMeasurement(range_continuous_polling, &data_sensor_top); if(!status_t) - { - int i; - for(i=0;i<10;i++) - { - status_t=board->sensor_top->GetMeasurement(range_continuous_polling, &data_sensor_top); - if(!status_t) - printf("Top Range measure %d: %dmm\n\r",i+1,data_sensor_top.range_mm); - else - printf("Invalid range value!\n\r"); - } + printf("Top Range measure %d: %dmm\n\r",i+1,data_sensor_top.range_mm); + else + printf("Invalid range value!\n\r"); + } } - else - printf("Failed to start measurement!\n\r"); - status_t=board->sensor_top->StopMeasurement(range_continuous_polling); - if(status_t) - printf("Failed to stop measurement!\n\r"); + else + printf("Failed to start measurement!\n\r"); + status_t=board->sensor_top->StopMeasurement(range_continuous_polling); + if(status_t) + printf("Failed to stop measurement!\n\r"); #endif #ifdef ALS_CONTINUOUS_POLLING - status_t=board->sensor_top->StartMeasurement(als_continuous_polling, NULL, &data_sensor_top, NULL, NULL); + status_t=board->sensor_top->StartMeasurement(als_continuous_polling, NULL, &data_sensor_top, NULL, NULL); + if(!status_t) + { + int i; + for(i=0;i<10;i++) + { + status=board->sensor_top->GetMeasurement(als_continuous_polling, &data_sensor_top); if(!status_t) - { - int i; - for(i=0;i<10;i++) - { - 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); - else - printf("Invalid light value!\n\r"); - } + printf("Top Light measure %d: %dlux\n\r",i,data_sensor_top.lux); + else + printf("Invalid light value!\n\r"); + } } - else - printf("Failed to start measurement!\n\r"); - status_t=board->sensor_top->StopMeasurement(als_continuous_polling); - if(status_t) - printf("Failed to stop measurement!\n\r"); + else + printf("Failed to start measurement!\n\r"); + status_t=board->sensor_top->StopMeasurement(als_continuous_polling); + if(status_t) + printf("Failed to stop measurement!\n\r"); #endif - -/*#ifdef RANGE_CONTINUOUS_INTERRUPT - status_t=board->sensor_top->StartMeasurement(range_continuous_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL); - status_l=board->sensor_left->StartMeasurement(range_continuous_interrupt, SensorLeftIRQ, &data_sensor_left, NULL, NULL); - if((!status_t)&&(!status_l)) - { - while(1) - { - if(flag_sensor_top) - { - flag_sensor_top=false; - status_t=board->sensor_top->HandleIRQ(range_continuous_interrupt, &data_sensor_top); - if(!status_t) - printf("Top Range: %dmm\n\r",data_sensor_top.range_mm); - else - printf("Invalid range value!\n\r"); - } - if(flag_sensor_left) - { - flag_sensor_left=false; - status_l=board->sensor_left->HandleIRQ(range_continuous_interrupt, &data_sensor_left); - if(!status_l) - printf("Left Range: %dmm\n\r",data_sensor_left.range_mm); - else - printf("Invalid range value!\n\r"); - } - } - } - else - printf("Failed to start measurement!\n\r"); - status_t=board->sensor_top->StopMeasurement(range_continuous_interrupt); - if(status_l) - printf("Failed to stop sensor_top measurement!\n\r"); - status_l=board->sensor_left->StopMeasurement(range_continuous_interrupt); - if(status_l) - printf("Failed to stop sensor_left measurement!\n\r"); -#endif*/ #ifdef RANGE_CONTINUOUS_INTERRUPT - status=board->sensor_top->StartMeasurement(range_continuous_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL); - if(!status) - { - while(1) - { - if(flag_sensor_top) - { - flag_sensor_top=false; - status=board->sensor_top->HandleIRQ(range_continuous_interrupt, &data_sensor_top); - if(!status) - printf("Range: %dmm\n\r",data_sensor_top.range_mm); - else - printf("Invalid range value!\n\r"); - } - } - } - else - printf("Failed to start measurement!\n\r"); - status=board->sensor_top->StopMeasurement(range_continuous_interrupt); - if(status) - printf("Failed to stop measurement!\n\r"); + status=board->sensor_top->StartMeasurement(range_continuous_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL); + if(!status) + { + while(1) + { + if(flag_sensor_top) + { + flag_sensor_top=false; + status=board->sensor_top->HandleIRQ(range_continuous_interrupt, &data_sensor_top); + if(!status) + printf("Range: %dmm\n\r",data_sensor_top.range_mm); + else + printf("Invalid range value!\n\r"); + } + } + } + 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 + if(status) + printf("Failed to stop measurement!\n\r"); #endif #ifdef ALS_CONTINUOUS_INTERRUPT - status_t=board->sensor_top->StartMeasurement(als_continuous_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL); + status_t=board->sensor_top->StartMeasurement(als_continuous_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL); if(!status_t) - { - while(1) - { - if(flag_sensor_top) - { - 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); - else - printf("Invalid light value!\n\r"); - } - } + { + while(1) + { + if(flag_sensor_top) + { + 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); + else + printf("Invalid light value!\n\r"); } - else - printf("Failed to stop measurement!\n\r"); - status_t=board->sensor_top->StopMeasurement(als_continuous_interrupt); - if(status_t) - printf("Failed to stop measurement!\n\r"); + } + } + else + printf("Failed to stop measurement!\n\r"); + status_t=board->sensor_top->StopMeasurement(als_continuous_interrupt); + if(status_t) + printf("Failed to stop measurement!\n\r"); #endif #ifdef INTERLEAVED_MODE_INTERRUPT - status_t=board->sensor_top->StartMeasurement(interleaved_mode_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL); - if(!status_t) - { - while(1) - { - if(flag_sensor_top) - { - flag_sensor_top=false; - status_t=board->sensor_top->HandleIRQ(interleaved_mode_interrupt, &data_sensor_top); - if(!status_t) - { - printf("Top Range: %dmm ",data_sensor_top.range_mm); - printf("Top Light: %dlux\n\r",data_sensor_top.lux); - } - else - printf("Invalid range or light value!\n\r"); - } - } + status_t=board->sensor_top->StartMeasurement(interleaved_mode_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL); + if(!status_t) + { + while(1) + { + if(flag_sensor_top) + { + 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); + else + printf("Invalid range or light value!\n\r"); } - else - printf("Failed to stop measurement!\n\r"); - status_t=board->sensor_top->StopMeasurement(interleaved_mode_interrupt); - if(status_t) - printf("Failed to stop measurement!\n\r"); + } + } + else + printf("Failed to stop measurement!\n\r"); + status_t=board->sensor_top->StopMeasurement(interleaved_mode_interrupt); + if(status_t) + printf("Failed to stop measurement!\n\r"); #endif #ifdef RANGE_CONTINUOUS_INTERRUPT_LOW_THRESHOLD - status_t=board->sensor_top->StartMeasurement(range_continuous_interrupt_low_threshold, SensorTopIRQ, &data_sensor_top, 80, NULL); - if(!status_t) - { - while(1) - { - if(flag_sensor_top) - { - flag_sensor_top=false; - status_t=board->sensor_top->HandleIRQ(range_continuous_interrupt_low_threshold, &data_sensor_top); - if(!status_t) - printf("Range int low threshold: %dmm\n\r",data_sensor_top.range_mm); - else - printf("Invalid range value!\n\r"); - } - } - } - else - printf("Failed to start measurement!\n\r"); - status_t=board->sensor_top->StopMeasurement(range_continuous_interrupt_low_threshold); - if(status_t) - printf("Failed to stop measurement!\n\r"); + status_t=board->sensor_top->StartMeasurement(range_continuous_interrupt_low_threshold, SensorTopIRQ, &data_sensor_top, 80, NULL); + if(!status_t) + { + while(1) + { + if(flag_sensor_top) + { + flag_sensor_top=false; + status_t=board->sensor_top->HandleIRQ(range_continuous_interrupt_low_threshold, &data_sensor_top); + if(!status_t) + printf("Range int low threshold: %dmm\n\r",data_sensor_top.range_mm); + else + printf("Invalid range value!\n\r"); + } + } + } + else + printf("Failed to start measurement!\n\r"); + status_t=board->sensor_top->StopMeasurement(range_continuous_interrupt_low_threshold); + if(status_t) + printf("Failed to stop measurement!\n\r"); #endif -/* -status_l=board->sensor_left->StartMeasurement(range_continuous_interrupt, SensorLeftIRQ, &data_sensor_left, NULL, NULL); -status_r=board->sensor_right->StartMeasurement(als_continuous_interrupt, SensorRightIRQ, &data_sensor_right, NULL, NULL); -if((!status_l)&&(!status_r)) -{ - while(1) - { - if(flag_sensor_left) - { - flag_sensor_left=false; - status_l=board->sensor_left->HandleIRQ(range_continuous_interrupt, &data_sensor_left); - if(!status_l) - printf("Left Range: %dmm\n\r",data_sensor_left.range_mm); - else - printf("Invalid range value!\n\r"); - } - if(flag_sensor_right) - { - flag_sensor_right=false; - status_r=board->sensor_right->HandleIRQ(als_continuous_interrupt, &data_sensor_right); - if(!status_r) - printf("Right Light: %dlux\n\r",data_sensor_right.lux); - else - printf("Invalid light value!\n\r"); - } - } -} -else - printf("Failed to start measurement!\n\r"); -status_t=board->sensor_top->StopMeasurement(range_continuous_interrupt_low_threshold); -if(status_t) - printf("Failed to stop measurement!\n\r"); -status_t=board->sensor_top->StopMeasurement(als_continuous_interrupt); -if(status_t) - printf("Failed to stop measurement!\n\r"); -*/ +/* Test application Hand Gesture */ +#ifdef HAND_GESTURE + int gest_code, prev_gest_code, gest_duration; + char str[5]; + char valLeft[] = {'E'}; + char valRight[] = {'3'}; + char valTap[] = {'0'}; + 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); + gesture_reco.Init(150, 100, 200, 60, 500); + if((!status_l)&&(!status_r)) + { + while(1) + { + if(flag_sensor_left) + { + 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); + switch(gest_code) + { + case -1: + break; + case 0: //tap and start timer + if(gest_code!=prev_gest_code) + { + prev_gest_code=gest_code; + sprintf(str,"%s","0-"); + board->display->DisplayDigit(valTap, 1); + } + else + { + if(strcmp(str,"0-")==0) + { + sprintf(str,"-0"); + board->display->DisplayDigit(valTap, 2); + } + else + { + sprintf(str,"0-"); + board->display->DisplayDigit(valTap, 1); + } + } + break; + case 1: //left to right + board->display->DisplayDigit(valRight, 3); + break; + case 2: //right to left + board->display->DisplayDigit(valLeft, 0); + break; + default: + printf("Errore!\n\r"); + } + } + } + else + printf("Failed to start measurement!\n\r"); + status_t=board->sensor_top->StopMeasurement(range_continuous_interrupt_low_threshold); + if(status_t) + printf("Failed to stop measurement!\n\r"); + status_t=board->sensor_top->StopMeasurement(als_continuous_interrupt); + if(status_t) + printf("Failed to stop measurement!\n\r"); +#endif }