Sweep a servo according to Proximity sensor measure
Dependencies: Servo X_NUCLEO_6180XA1 mbed
Fork of HelloWorld_6180XA1 by
main.cpp
- Committer:
- gallonm
- Date:
- 2015-10-20
- Revision:
- 11:657dbe7bf245
- Parent:
- 10:5319abadb31e
- Child:
- 14:946e62f44f4f
File content as of revision 11:657dbe7bf245:
#include "mbed.h" #include "x_nucleo_6180xa1.h" #include <string.h> #include <stdlib.h> #include <stdio.h> #include <assert.h> #define VL6180X_I2C_SDA I2C_SDA #define VL6180X_I2C_SCL I2C_SCL //#define RANGE_SINGLE_SHOT_POLLING //#define ALS_SINGLE_SHOT_POLLING //#define RANGE_CONTINUOUS_POLLING //#define ALS_CONTINUOUS_POLLING //#define RANGE_CONTINUOUS_INTERRUPT #define ALS_CONTINUOUS_INTERRUPT //#define INTERLEAVED_MODE_INTERRUPT //#define RANGE_CONTINUOUS_POLLING_LOW_THRESHOLD //#define RANGE_CONTINUOUS_POLLING_HIGH_THRESHOLD //#define RANGE_CONTINUOUS_POLLING_OUT_OF_WINDOW //#define ALS_CONTINUOUS_POLLING_LOW_THRESHOLD //#define ALS_CONTINUOUS_POLLING_HIGH_THRESHOLD //#define ALS_CONTINUOUS_POLLING_OUT_OF_WINDOW //#define RANGE_CONTINUOUS_INTERRUPT_LOW_THRESHOLD //#define RANGE_CONTINUOUS_INTERRUPT_HIGH_THRESHOLD //#define RANGE_CONTINUOUS_INTERRUPT_OUT_OF_WINDOW //#define ALS_CONTINUOUS_INTERRUPT_LOW_THRESHOLD //#define ALS_CONTINUOUS_INTERRUPT_HIGH_THRESHOLD //#define ALS_CONTINUOUS_INTERRUPT_OUT_OF_WINDOW /* 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 */ void OnErrLog(void){}; // sistemare la funzione per stampare il messaggio che gli viene passato void DISP_ExecLoopBody(void){}; 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; Timer timer; // timer to debug int start, end; /* 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) { flag_sensor_top=true; board->sensor_top->DisableInterruptMeasureDetectionIRQ(); } void SensorBottomIRQ(void) { flag_sensor_bottom=true; board->sensor_bottom->DisableInterruptMeasureDetectionIRQ(); } void SensorLeftIRQ(void) { flag_sensor_left=true; board->sensor_left->DisableInterruptMeasureDetectionIRQ(); } void SensorRightIRQ(void) { flag_sensor_right=true; board->sensor_right->DisableInterruptMeasureDetectionIRQ(); } int main() { int status, status_t, status_b, status_l, status_r; status=board->InitBoard(); if(status) 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); 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"); #endif #ifdef RANGE_CONTINUOUS_POLLING 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) 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"); #endif #ifdef ALS_CONTINUOUS_POLLING 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) 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"); #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"); #endif #ifdef ALS_CONTINUOUS_INTERRUPT 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"); } } } 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"); } } } 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"); #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"); */ }