MFR16 / Mbed 2 deprecated Servo_6180XA1

Sweep a servo according to Proximity sensor measure

Dependencies:   Servo X_NUCLEO_6180XA1 mbed

Fork of HelloWorld_6180XA1 by ST

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?

UserRevisionLine numberNew 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