Sweep a servo according to Proximity sensor measure
Dependencies: Servo X_NUCLEO_6180XA1 mbed
Fork of HelloWorld_6180XA1 by
main.cpp@18:6312be3b32d9, 2015-10-28 (annotated)
- Committer:
- gallonm
- Date:
- Wed Oct 28 15:06:09 2015 +0000
- Revision:
- 18:6312be3b32d9
- Parent:
- 17:68515d0667af
- Child:
- 19:394c9a4a3046
Deleted Middlewares folder and its subfolders.; Deleted HmiBBGesture files.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
gallonm | 0:83c628a58feb | 1 | #include "mbed.h" |
gallonm | 8:4c05f7a5bb60 | 2 | #include "x_nucleo_6180xa1.h" |
gallonm | 4:ccd62fd7e137 | 3 | #include <string.h> |
gallonm | 4:ccd62fd7e137 | 4 | #include <stdlib.h> |
gallonm | 4:ccd62fd7e137 | 5 | #include <stdio.h> |
gallonm | 8:4c05f7a5bb60 | 6 | #include <assert.h> |
gallonm | 8:4c05f7a5bb60 | 7 | |
gallonm | 14:946e62f44f4f | 8 | /* Polling operating modes don`t require callback function that handles IRQ |
gallonm | 14:946e62f44f4f | 9 | Callback functions are used only for measure that require interrupt */ |
gallonm | 14:946e62f44f4f | 10 | |
gallonm | 5:fa65d931bd96 | 11 | #define VL6180X_I2C_SDA I2C_SDA |
gallonm | 5:fa65d931bd96 | 12 | #define VL6180X_I2C_SCL I2C_SCL |
gallonm | 5:fa65d931bd96 | 13 | |
gallonm | 10:5319abadb31e | 14 | //#define RANGE_SINGLE_SHOT_POLLING |
gallonm | 10:5319abadb31e | 15 | //#define ALS_SINGLE_SHOT_POLLING |
gallonm | 10:5319abadb31e | 16 | //#define RANGE_CONTINUOUS_POLLING |
gallonm | 10:5319abadb31e | 17 | //#define ALS_CONTINUOUS_POLLING |
gallonm | 17:68515d0667af | 18 | #define RANGE_CONTINUOUS_INTERRUPT |
gallonm | 14:946e62f44f4f | 19 | //#define ALS_CONTINUOUS_INTERRUPT |
gallonm | 11:657dbe7bf245 | 20 | //#define INTERLEAVED_MODE_INTERRUPT |
gallonm | 10:5319abadb31e | 21 | //#define RANGE_CONTINUOUS_POLLING_LOW_THRESHOLD |
gallonm | 10:5319abadb31e | 22 | //#define RANGE_CONTINUOUS_POLLING_HIGH_THRESHOLD |
gallonm | 10:5319abadb31e | 23 | //#define RANGE_CONTINUOUS_POLLING_OUT_OF_WINDOW |
gallonm | 10:5319abadb31e | 24 | //#define ALS_CONTINUOUS_POLLING_LOW_THRESHOLD |
gallonm | 10:5319abadb31e | 25 | //#define ALS_CONTINUOUS_POLLING_HIGH_THRESHOLD |
gallonm | 10:5319abadb31e | 26 | //#define ALS_CONTINUOUS_POLLING_OUT_OF_WINDOW |
gallonm | 10:5319abadb31e | 27 | //#define RANGE_CONTINUOUS_INTERRUPT_LOW_THRESHOLD |
gallonm | 10:5319abadb31e | 28 | //#define RANGE_CONTINUOUS_INTERRUPT_HIGH_THRESHOLD |
gallonm | 10:5319abadb31e | 29 | //#define RANGE_CONTINUOUS_INTERRUPT_OUT_OF_WINDOW |
gallonm | 10:5319abadb31e | 30 | //#define ALS_CONTINUOUS_INTERRUPT_LOW_THRESHOLD |
gallonm | 10:5319abadb31e | 31 | //#define ALS_CONTINUOUS_INTERRUPT_HIGH_THRESHOLD |
gallonm | 10:5319abadb31e | 32 | //#define ALS_CONTINUOUS_INTERRUPT_OUT_OF_WINDOW |
gallonm | 10:5319abadb31e | 33 | |
gallonm | 14:946e62f44f4f | 34 | /* timer and digital out pin used for debugging */ |
gallonm | 14:946e62f44f4f | 35 | //Timer timer; |
gallonm | 14:946e62f44f4f | 36 | //int start, end; |
gallonm | 14:946e62f44f4f | 37 | //DigitalOut pin(PA_15); |
gallonm | 5:fa65d931bd96 | 38 | |
gallonm | 8:4c05f7a5bb60 | 39 | DevI2C device_i2c(VL6180X_I2C_SDA, VL6180X_I2C_SCL); |
gallonm | 9:1d0e839edee8 | 40 | static X_NUCLEO_6180XA1 *board=X_NUCLEO_6180XA1::Instance(&device_i2c); |
gallonm | 9:1d0e839edee8 | 41 | |
gallonm | 14:946e62f44f4f | 42 | MeasureData_t data_sensor_top, data_sensor_bottom, data_sensor_left, data_sensor_right; |
gallonm | 11:657dbe7bf245 | 43 | |
gallonm | 15:b94bc967fecd | 44 | /* flags that handle interrupt request */ |
gallonm | 15:b94bc967fecd | 45 | bool flag_sensor_top=false, flag_sensor_bottom=false, flag_sensor_left=false, flag_sensor_right=false, flag_stop_measure=false; |
gallonm | 15:b94bc967fecd | 46 | |
gallonm | 15:b94bc967fecd | 47 | /* user button used to stop measurements in interrupt mode */ |
gallonm | 15:b94bc967fecd | 48 | InterruptIn stop_button(USER_BUTTON); |
gallonm | 9:1d0e839edee8 | 49 | |
gallonm | 9:1d0e839edee8 | 50 | /* callback functions of the sensors */ |
gallonm | 15:b94bc967fecd | 51 | void SensorTopIRQ(void) |
gallonm | 9:1d0e839edee8 | 52 | { |
gallonm | 11:657dbe7bf245 | 53 | flag_sensor_top=true; |
gallonm | 14:946e62f44f4f | 54 | board->sensor_top->DisableInterruptMeasureDetectionIRQ(); |
gallonm | 9:1d0e839edee8 | 55 | } |
gallonm | 9:1d0e839edee8 | 56 | |
gallonm | 15:b94bc967fecd | 57 | void SensorBottomIRQ(void) |
gallonm | 11:657dbe7bf245 | 58 | { |
gallonm | 11:657dbe7bf245 | 59 | flag_sensor_bottom=true; |
gallonm | 14:946e62f44f4f | 60 | board->sensor_bottom->DisableInterruptMeasureDetectionIRQ(); |
gallonm | 11:657dbe7bf245 | 61 | } |
gallonm | 11:657dbe7bf245 | 62 | |
gallonm | 15:b94bc967fecd | 63 | void SensorLeftIRQ(void) |
gallonm | 11:657dbe7bf245 | 64 | { |
gallonm | 11:657dbe7bf245 | 65 | flag_sensor_left=true; |
gallonm | 14:946e62f44f4f | 66 | board->sensor_left->DisableInterruptMeasureDetectionIRQ(); |
gallonm | 11:657dbe7bf245 | 67 | } |
gallonm | 11:657dbe7bf245 | 68 | |
gallonm | 15:b94bc967fecd | 69 | void SensorRightIRQ(void) |
gallonm | 11:657dbe7bf245 | 70 | { |
gallonm | 11:657dbe7bf245 | 71 | flag_sensor_right=true; |
gallonm | 14:946e62f44f4f | 72 | board->sensor_right->DisableInterruptMeasureDetectionIRQ(); |
gallonm | 11:657dbe7bf245 | 73 | } |
gallonm | 9:1d0e839edee8 | 74 | |
gallonm | 15:b94bc967fecd | 75 | void StopMeasureIRQ(void) |
gallonm | 15:b94bc967fecd | 76 | { |
gallonm | 15:b94bc967fecd | 77 | flag_stop_measure=true; |
gallonm | 15:b94bc967fecd | 78 | } |
gallonm | 14:946e62f44f4f | 79 | |
gallonm | 4:ccd62fd7e137 | 80 | int main() |
gallonm | 5:fa65d931bd96 | 81 | { |
gallonm | 14:946e62f44f4f | 82 | int status, status_t, status_b, status_l, status_r; |
gallonm | 14:946e62f44f4f | 83 | |
gallonm | 15:b94bc967fecd | 84 | //device_i2c.frequency(400000); //change i2c frequncy from 100kHz to 400kHz |
gallonm | 14:946e62f44f4f | 85 | pc.baud(115200); //change baudrate of the printf |
gallonm | 14:946e62f44f4f | 86 | |
gallonm | 10:5319abadb31e | 87 | status=board->InitBoard(); |
gallonm | 14:946e62f44f4f | 88 | if(status) |
gallonm | 15:b94bc967fecd | 89 | printf("Failed to init the board!\n\r"); |
gallonm | 10:5319abadb31e | 90 | |
gallonm | 10:5319abadb31e | 91 | #ifdef RANGE_SINGLE_SHOT_POLLING |
gallonm | 14:946e62f44f4f | 92 | status_t=board->sensor_top->StartMeasurement(range_single_shot_polling, NULL, &data_sensor_top, NULL, NULL); |
gallonm | 14:946e62f44f4f | 93 | if(!status_t) |
gallonm | 14:946e62f44f4f | 94 | printf("Top Range: %dmm\n\r",data_sensor_top.range_mm); |
gallonm | 14:946e62f44f4f | 95 | else if(status==INVALID_PARAMS) |
gallonm | 14:946e62f44f4f | 96 | printf("Failed to start measurement!\n\r"); |
gallonm | 14:946e62f44f4f | 97 | else |
gallonm | 14:946e62f44f4f | 98 | printf("Invalid range value!\n\r"); |
gallonm | 14:946e62f44f4f | 99 | status_t=board->sensor_top->StopMeasurement(range_single_shot_polling); |
gallonm | 14:946e62f44f4f | 100 | if(status_t) |
gallonm | 14:946e62f44f4f | 101 | printf("Failed to stop measurement!\n\r"); |
gallonm | 10:5319abadb31e | 102 | #endif |
gallonm | 9:1d0e839edee8 | 103 | |
gallonm | 10:5319abadb31e | 104 | #ifdef ALS_SINGLE_SHOT_POLLING |
gallonm | 14:946e62f44f4f | 105 | status_t=board->sensor_top->StartMeasurement(als_single_shot_polling, NULL, &data_sensor_top, NULL, NULL); |
gallonm | 14:946e62f44f4f | 106 | if(!status_t) |
gallonm | 15:b94bc967fecd | 107 | printf("Top Light: %d lux\n\r",data_sensor_top.lux); |
gallonm | 14:946e62f44f4f | 108 | else if(status==INVALID_PARAMS) |
gallonm | 14:946e62f44f4f | 109 | printf("Failed to start measurement!\n\r"); |
gallonm | 14:946e62f44f4f | 110 | else |
gallonm | 14:946e62f44f4f | 111 | printf("Invalid light value!\n\r"); |
gallonm | 14:946e62f44f4f | 112 | status_t=board->sensor_top->StopMeasurement(als_single_shot_polling); |
gallonm | 14:946e62f44f4f | 113 | if(status_t) |
gallonm | 14:946e62f44f4f | 114 | printf("Failed to stop measurement!\n\r"); |
gallonm | 10:5319abadb31e | 115 | #endif |
gallonm | 10:5319abadb31e | 116 | |
gallonm | 10:5319abadb31e | 117 | #ifdef RANGE_CONTINUOUS_POLLING |
gallonm | 14:946e62f44f4f | 118 | status_t=board->sensor_top->StartMeasurement(range_continuous_polling, NULL, &data_sensor_top, NULL, NULL); |
gallonm | 14:946e62f44f4f | 119 | if(!status_t) |
gallonm | 14:946e62f44f4f | 120 | { |
gallonm | 14:946e62f44f4f | 121 | int i; |
gallonm | 14:946e62f44f4f | 122 | for(i=0;i<10;i++) |
gallonm | 14:946e62f44f4f | 123 | { |
gallonm | 14:946e62f44f4f | 124 | status_t=board->sensor_top->GetMeasurement(range_continuous_polling, &data_sensor_top); |
gallonm | 11:657dbe7bf245 | 125 | if(!status_t) |
gallonm | 14:946e62f44f4f | 126 | printf("Top Range measure %d: %dmm\n\r",i+1,data_sensor_top.range_mm); |
gallonm | 14:946e62f44f4f | 127 | else |
gallonm | 14:946e62f44f4f | 128 | printf("Invalid range value!\n\r"); |
gallonm | 14:946e62f44f4f | 129 | } |
gallonm | 10:5319abadb31e | 130 | } |
gallonm | 14:946e62f44f4f | 131 | else |
gallonm | 14:946e62f44f4f | 132 | printf("Failed to start measurement!\n\r"); |
gallonm | 14:946e62f44f4f | 133 | status_t=board->sensor_top->StopMeasurement(range_continuous_polling); |
gallonm | 14:946e62f44f4f | 134 | if(status_t) |
gallonm | 14:946e62f44f4f | 135 | printf("Failed to stop measurement!\n\r"); |
gallonm | 10:5319abadb31e | 136 | #endif |
gallonm | 10:5319abadb31e | 137 | |
gallonm | 10:5319abadb31e | 138 | #ifdef ALS_CONTINUOUS_POLLING |
gallonm | 14:946e62f44f4f | 139 | status_t=board->sensor_top->StartMeasurement(als_continuous_polling, NULL, &data_sensor_top, NULL, NULL); |
gallonm | 14:946e62f44f4f | 140 | if(!status_t) |
gallonm | 14:946e62f44f4f | 141 | { |
gallonm | 14:946e62f44f4f | 142 | int i; |
gallonm | 14:946e62f44f4f | 143 | for(i=0;i<10;i++) |
gallonm | 14:946e62f44f4f | 144 | { |
gallonm | 14:946e62f44f4f | 145 | status=board->sensor_top->GetMeasurement(als_continuous_polling, &data_sensor_top); |
gallonm | 11:657dbe7bf245 | 146 | if(!status_t) |
gallonm | 15:b94bc967fecd | 147 | printf("Top Light measure %d: %d lux\n\r",i,data_sensor_top.lux); |
gallonm | 14:946e62f44f4f | 148 | else |
gallonm | 14:946e62f44f4f | 149 | printf("Invalid light value!\n\r"); |
gallonm | 14:946e62f44f4f | 150 | } |
gallonm | 9:1d0e839edee8 | 151 | } |
gallonm | 14:946e62f44f4f | 152 | else |
gallonm | 14:946e62f44f4f | 153 | printf("Failed to start measurement!\n\r"); |
gallonm | 14:946e62f44f4f | 154 | status_t=board->sensor_top->StopMeasurement(als_continuous_polling); |
gallonm | 14:946e62f44f4f | 155 | if(status_t) |
gallonm | 14:946e62f44f4f | 156 | printf("Failed to stop measurement!\n\r"); |
gallonm | 10:5319abadb31e | 157 | #endif |
gallonm | 11:657dbe7bf245 | 158 | |
gallonm | 11:657dbe7bf245 | 159 | #ifdef RANGE_CONTINUOUS_INTERRUPT |
gallonm | 15:b94bc967fecd | 160 | stop_button.rise(&StopMeasureIRQ); |
gallonm | 14:946e62f44f4f | 161 | status=board->sensor_top->StartMeasurement(range_continuous_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL); |
gallonm | 14:946e62f44f4f | 162 | if(!status) |
gallonm | 14:946e62f44f4f | 163 | { |
gallonm | 14:946e62f44f4f | 164 | while(1) |
gallonm | 14:946e62f44f4f | 165 | { |
gallonm | 14:946e62f44f4f | 166 | if(flag_sensor_top) |
gallonm | 14:946e62f44f4f | 167 | { |
gallonm | 14:946e62f44f4f | 168 | flag_sensor_top=false; |
gallonm | 14:946e62f44f4f | 169 | status=board->sensor_top->HandleIRQ(range_continuous_interrupt, &data_sensor_top); |
gallonm | 14:946e62f44f4f | 170 | if(!status) |
gallonm | 14:946e62f44f4f | 171 | printf("Range: %dmm\n\r",data_sensor_top.range_mm); |
gallonm | 14:946e62f44f4f | 172 | else |
gallonm | 14:946e62f44f4f | 173 | printf("Invalid range value!\n\r"); |
gallonm | 14:946e62f44f4f | 174 | } |
gallonm | 15:b94bc967fecd | 175 | if(flag_stop_measure) |
gallonm | 15:b94bc967fecd | 176 | break; |
gallonm | 14:946e62f44f4f | 177 | } |
gallonm | 14:946e62f44f4f | 178 | } |
gallonm | 14:946e62f44f4f | 179 | else |
gallonm | 14:946e62f44f4f | 180 | printf("Failed to start measurement!\n\r"); |
gallonm | 15:b94bc967fecd | 181 | status=board->sensor_top->StopMeasurement(range_continuous_interrupt); |
gallonm | 14:946e62f44f4f | 182 | if(status) |
gallonm | 14:946e62f44f4f | 183 | printf("Failed to stop measurement!\n\r"); |
gallonm | 10:5319abadb31e | 184 | #endif |
gallonm | 10:5319abadb31e | 185 | |
gallonm | 10:5319abadb31e | 186 | #ifdef ALS_CONTINUOUS_INTERRUPT |
gallonm | 15:b94bc967fecd | 187 | stop_button.rise(&StopMeasureIRQ); |
gallonm | 14:946e62f44f4f | 188 | status_t=board->sensor_top->StartMeasurement(als_continuous_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL); |
gallonm | 11:657dbe7bf245 | 189 | if(!status_t) |
gallonm | 14:946e62f44f4f | 190 | { |
gallonm | 14:946e62f44f4f | 191 | while(1) |
gallonm | 14:946e62f44f4f | 192 | { |
gallonm | 14:946e62f44f4f | 193 | if(flag_sensor_top) |
gallonm | 14:946e62f44f4f | 194 | { |
gallonm | 14:946e62f44f4f | 195 | flag_sensor_top=false; |
gallonm | 14:946e62f44f4f | 196 | status_t=board->sensor_top->HandleIRQ(als_continuous_interrupt, &data_sensor_top); |
gallonm | 14:946e62f44f4f | 197 | if(!status_t) |
gallonm | 15:b94bc967fecd | 198 | printf("Top Light: %d lux\n\r",data_sensor_top.lux); |
gallonm | 14:946e62f44f4f | 199 | else |
gallonm | 14:946e62f44f4f | 200 | printf("Invalid light value!\n\r"); |
gallonm | 10:5319abadb31e | 201 | } |
gallonm | 15:b94bc967fecd | 202 | if(flag_stop_measure) |
gallonm | 15:b94bc967fecd | 203 | break; |
gallonm | 14:946e62f44f4f | 204 | } |
gallonm | 14:946e62f44f4f | 205 | } |
gallonm | 14:946e62f44f4f | 206 | else |
gallonm | 14:946e62f44f4f | 207 | printf("Failed to stop measurement!\n\r"); |
gallonm | 14:946e62f44f4f | 208 | status_t=board->sensor_top->StopMeasurement(als_continuous_interrupt); |
gallonm | 14:946e62f44f4f | 209 | if(status_t) |
gallonm | 14:946e62f44f4f | 210 | printf("Failed to stop measurement!\n\r"); |
gallonm | 10:5319abadb31e | 211 | #endif |
gallonm | 9:1d0e839edee8 | 212 | |
gallonm | 15:b94bc967fecd | 213 | #ifdef INTERLEAVED_MODE_INTERRUPT |
gallonm | 15:b94bc967fecd | 214 | stop_button.rise(&StopMeasureIRQ); |
gallonm | 14:946e62f44f4f | 215 | status_t=board->sensor_top->StartMeasurement(interleaved_mode_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL); |
gallonm | 14:946e62f44f4f | 216 | if(!status_t) |
gallonm | 14:946e62f44f4f | 217 | { |
gallonm | 14:946e62f44f4f | 218 | while(1) |
gallonm | 14:946e62f44f4f | 219 | { |
gallonm | 14:946e62f44f4f | 220 | if(flag_sensor_top) |
gallonm | 14:946e62f44f4f | 221 | { |
gallonm | 14:946e62f44f4f | 222 | flag_sensor_top=false; |
gallonm | 14:946e62f44f4f | 223 | status_t=board->sensor_top->HandleIRQ(interleaved_mode_interrupt, &data_sensor_top); |
gallonm | 14:946e62f44f4f | 224 | if(!status_t) |
gallonm | 15:b94bc967fecd | 225 | printf("Top Range: %dmm\tTop Light: %d lux\n\r",data_sensor_top.range_mm, data_sensor_top.lux); |
gallonm | 14:946e62f44f4f | 226 | else |
gallonm | 14:946e62f44f4f | 227 | printf("Invalid range or light value!\n\r"); |
gallonm | 9:1d0e839edee8 | 228 | } |
gallonm | 15:b94bc967fecd | 229 | if(flag_stop_measure) |
gallonm | 15:b94bc967fecd | 230 | break; |
gallonm | 14:946e62f44f4f | 231 | } |
gallonm | 14:946e62f44f4f | 232 | } |
gallonm | 14:946e62f44f4f | 233 | else |
gallonm | 14:946e62f44f4f | 234 | printf("Failed to stop measurement!\n\r"); |
gallonm | 14:946e62f44f4f | 235 | status_t=board->sensor_top->StopMeasurement(interleaved_mode_interrupt); |
gallonm | 14:946e62f44f4f | 236 | if(status_t) |
gallonm | 14:946e62f44f4f | 237 | printf("Failed to stop measurement!\n\r"); |
gallonm | 10:5319abadb31e | 238 | #endif |
gallonm | 10:5319abadb31e | 239 | |
gallonm | 10:5319abadb31e | 240 | #ifdef RANGE_CONTINUOUS_INTERRUPT_LOW_THRESHOLD |
gallonm | 15:b94bc967fecd | 241 | stop_button.rise(&StopMeasureIRQ); |
gallonm | 14:946e62f44f4f | 242 | status_t=board->sensor_top->StartMeasurement(range_continuous_interrupt_low_threshold, SensorTopIRQ, &data_sensor_top, 80, NULL); |
gallonm | 14:946e62f44f4f | 243 | if(!status_t) |
gallonm | 14:946e62f44f4f | 244 | { |
gallonm | 14:946e62f44f4f | 245 | while(1) |
gallonm | 14:946e62f44f4f | 246 | { |
gallonm | 14:946e62f44f4f | 247 | if(flag_sensor_top) |
gallonm | 14:946e62f44f4f | 248 | { |
gallonm | 14:946e62f44f4f | 249 | flag_sensor_top=false; |
gallonm | 14:946e62f44f4f | 250 | status_t=board->sensor_top->HandleIRQ(range_continuous_interrupt_low_threshold, &data_sensor_top); |
gallonm | 14:946e62f44f4f | 251 | if(!status_t) |
gallonm | 14:946e62f44f4f | 252 | printf("Range int low threshold: %dmm\n\r",data_sensor_top.range_mm); |
gallonm | 14:946e62f44f4f | 253 | else |
gallonm | 14:946e62f44f4f | 254 | printf("Invalid range value!\n\r"); |
gallonm | 14:946e62f44f4f | 255 | } |
gallonm | 15:b94bc967fecd | 256 | if(flag_stop_measure) |
gallonm | 15:b94bc967fecd | 257 | break; |
gallonm | 14:946e62f44f4f | 258 | } |
gallonm | 14:946e62f44f4f | 259 | } |
gallonm | 14:946e62f44f4f | 260 | else |
gallonm | 14:946e62f44f4f | 261 | printf("Failed to start measurement!\n\r"); |
gallonm | 14:946e62f44f4f | 262 | status_t=board->sensor_top->StopMeasurement(range_continuous_interrupt_low_threshold); |
gallonm | 14:946e62f44f4f | 263 | if(status_t) |
gallonm | 14:946e62f44f4f | 264 | printf("Failed to stop measurement!\n\r"); |
gallonm | 10:5319abadb31e | 265 | #endif |
gallonm | 5:fa65d931bd96 | 266 | |
gallonm | 15:b94bc967fecd | 267 | delete board; |
gallonm | 15:b94bc967fecd | 268 | |
gallonm | 4:ccd62fd7e137 | 269 | } |
gallonm | 5:fa65d931bd96 | 270 | |
gallonm | 5:fa65d931bd96 | 271 |