Harrison Statham / Mbed 2 deprecated AltControlOfRemoteVehicles

Dependencies:   mbed

main.cpp@0:9e0195f256b1, 2018-12-13 (annotated)

Committer:
harrisonstatham
Date:
Thu Dec 13 15:04:55 2018 +0000
Revision:
0:9e0195f256b1
Initial

Who changed what in which revision?

UserRevisionLine numberNew contents of line
harrisonstatham 0:9e0195f256b1 1 #include "mbed.h"
harrisonstatham 0:9e0195f256b1 2 #include "XNucleo53L0A1.h"
harrisonstatham 0:9e0195f256b1 3 #include <stdio.h>
harrisonstatham 0:9e0195f256b1 4
harrisonstatham 0:9e0195f256b1 5 Serial pc(USBTX,USBRX);
harrisonstatham 0:9e0195f256b1 6
harrisonstatham 0:9e0195f256b1 7 // Setup the shutdown pins for the VL53L0Xs
harrisonstatham 0:9e0195f256b1 8 // When these pins are pulled LOW, the sensor goes into shutdown.
harrisonstatham 0:9e0195f256b1 9
harrisonstatham 0:9e0195f256b1 10 DigitalOut center_shutx(p26);
harrisonstatham 0:9e0195f256b1 11
harrisonstatham 0:9e0195f256b1 12 DigitalOut left_shdn(p18);
harrisonstatham 0:9e0195f256b1 13
harrisonstatham 0:9e0195f256b1 14 DigitalOut center_shdn(p19);
harrisonstatham 0:9e0195f256b1 15 DigitalOut right_shdn(p20);
harrisonstatham 0:9e0195f256b1 16
harrisonstatham 0:9e0195f256b1 17
harrisonstatham 0:9e0195f256b1 18 // This VL53L0X board test application performs a range measurement in polling mode
harrisonstatham 0:9e0195f256b1 19 // Use 3.3(Vout) for Vin, p28 for SDA, p27 for SCL, P26 for shdn on mbed LPC1768
harrisonstatham 0:9e0195f256b1 20
harrisonstatham 0:9e0195f256b1 21 //I2C sensor pins
harrisonstatham 0:9e0195f256b1 22 #define VL53L0_I2C_SDA p28
harrisonstatham 0:9e0195f256b1 23 #define VL53L0_I2C_SCL p27
harrisonstatham 0:9e0195f256b1 24
harrisonstatham 0:9e0195f256b1 25 #define MY_DEBUG 0
harrisonstatham 0:9e0195f256b1 26
harrisonstatham 0:9e0195f256b1 27 static XNucleo53L0A1 *board=NULL;
harrisonstatham 0:9e0195f256b1 28
harrisonstatham 0:9e0195f256b1 29 // Orange = left
harrisonstatham 0:9e0195f256b1 30 // Blue = right
harrisonstatham 0:9e0195f256b1 31 // Yellow = forward
harrisonstatham 0:9e0195f256b1 32 // Green = backwards
harrisonstatham 0:9e0195f256b1 33
harrisonstatham 0:9e0195f256b1 34 DigitalOut TurnLeft(p21);
harrisonstatham 0:9e0195f256b1 35 DigitalOut TurnRight(p22);
harrisonstatham 0:9e0195f256b1 36 //
harrisonstatham 0:9e0195f256b1 37 DigitalOut Forward(p23);
harrisonstatham 0:9e0195f256b1 38 DigitalOut Reverse(p24);
harrisonstatham 0:9e0195f256b1 39
harrisonstatham 0:9e0195f256b1 40 void InitChannel(DevI2C *device_i2c, int WhichChannel)
harrisonstatham 0:9e0195f256b1 41 {
harrisonstatham 0:9e0195f256b1 42 int status;
harrisonstatham 0:9e0195f256b1 43
harrisonstatham 0:9e0195f256b1 44 if(board != NULL)
harrisonstatham 0:9e0195f256b1 45 {
harrisonstatham 0:9e0195f256b1 46 // Call destructor
harrisonstatham 0:9e0195f256b1 47 board->~XNucleo53L0A1();
harrisonstatham 0:9e0195f256b1 48 board = NULL;
harrisonstatham 0:9e0195f256b1 49 }
harrisonstatham 0:9e0195f256b1 50
harrisonstatham 0:9e0195f256b1 51 /* creates the 53L0A1 expansion board singleton obj */
harrisonstatham 0:9e0195f256b1 52 board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
harrisonstatham 0:9e0195f256b1 53
harrisonstatham 0:9e0195f256b1 54 left_shdn = 0;
harrisonstatham 0:9e0195f256b1 55 right_shdn = 0;
harrisonstatham 0:9e0195f256b1 56 center_shdn = 0;
harrisonstatham 0:9e0195f256b1 57
harrisonstatham 0:9e0195f256b1 58 switch(WhichChannel)
harrisonstatham 0:9e0195f256b1 59 {
harrisonstatham 0:9e0195f256b1 60 // left channel
harrisonstatham 0:9e0195f256b1 61 case 0:
harrisonstatham 0:9e0195f256b1 62
harrisonstatham 0:9e0195f256b1 63 left_shdn = 1;
harrisonstatham 0:9e0195f256b1 64
harrisonstatham 0:9e0195f256b1 65 break;
harrisonstatham 0:9e0195f256b1 66
harrisonstatham 0:9e0195f256b1 67 // right channel
harrisonstatham 0:9e0195f256b1 68 case 2:
harrisonstatham 0:9e0195f256b1 69
harrisonstatham 0:9e0195f256b1 70 right_shdn = 1;
harrisonstatham 0:9e0195f256b1 71
harrisonstatham 0:9e0195f256b1 72 break;
harrisonstatham 0:9e0195f256b1 73
harrisonstatham 0:9e0195f256b1 74 // center channel
harrisonstatham 0:9e0195f256b1 75 case 1:
harrisonstatham 0:9e0195f256b1 76 default:
harrisonstatham 0:9e0195f256b1 77
harrisonstatham 0:9e0195f256b1 78 center_shdn = 1;
harrisonstatham 0:9e0195f256b1 79
harrisonstatham 0:9e0195f256b1 80 break;
harrisonstatham 0:9e0195f256b1 81 }
harrisonstatham 0:9e0195f256b1 82
harrisonstatham 0:9e0195f256b1 83 center_shutx = 0; //must reset sensor for an mbed reset to work
harrisonstatham 0:9e0195f256b1 84 wait(0.001);
harrisonstatham 0:9e0195f256b1 85 center_shutx = 1;
harrisonstatham 0:9e0195f256b1 86 //wait(0.01);
harrisonstatham 0:9e0195f256b1 87
harrisonstatham 0:9e0195f256b1 88 /* init the 53L0A1 board with default values */
harrisonstatham 0:9e0195f256b1 89 status = board->init_board();
harrisonstatham 0:9e0195f256b1 90 while (status) {
harrisonstatham 0:9e0195f256b1 91
harrisonstatham 0:9e0195f256b1 92 if(MY_DEBUG) {
harrisonstatham 0:9e0195f256b1 93 pc.printf("Failed to init board! \r\n");
harrisonstatham 0:9e0195f256b1 94 }
harrisonstatham 0:9e0195f256b1 95
harrisonstatham 0:9e0195f256b1 96 status = board->init_board();
harrisonstatham 0:9e0195f256b1 97 }
harrisonstatham 0:9e0195f256b1 98 }
harrisonstatham 0:9e0195f256b1 99
harrisonstatham 0:9e0195f256b1 100 void TurnLeft()
harrisonstatham 0:9e0195f256b1 101 {
harrisonstatham 0:9e0195f256b1 102 TurnLeft = 1;
harrisonstatham 0:9e0195f256b1 103 wait(0.1);
harrisonstatham 0:9e0195f256b1 104 TurnLeft = 0;
harrisonstatham 0:9e0195f256b1 105 }
harrisonstatham 0:9e0195f256b1 106
harrisonstatham 0:9e0195f256b1 107 void TurnRight()
harrisonstatham 0:9e0195f256b1 108 {
harrisonstatham 0:9e0195f256b1 109 TurnLeft = 0;
harrisonstatham 0:9e0195f256b1 110 TurnRight = 1;
harrisonstatham 0:9e0195f256b1 111 wait(0.1);
harrisonstatham 0:9e0195f256b1 112 TurnRight = 0;
harrisonstatham 0:9e0195f256b1 113 }
harrisonstatham 0:9e0195f256b1 114
harrisonstatham 0:9e0195f256b1 115
harrisonstatham 0:9e0195f256b1 116 void Drive()
harrisonstatham 0:9e0195f256b1 117 {
harrisonstatham 0:9e0195f256b1 118
harrisonstatham 0:9e0195f256b1 119 }
harrisonstatham 0:9e0195f256b1 120
harrisonstatham 0:9e0195f256b1 121 void Reverse()
harrisonstatham 0:9e0195f256b1 122 {
harrisonstatham 0:9e0195f256b1 123
harrisonstatham 0:9e0195f256b1 124 }
harrisonstatham 0:9e0195f256b1 125
harrisonstatham 0:9e0195f256b1 126
harrisonstatham 0:9e0195f256b1 127
harrisonstatham 0:9e0195f256b1 128
harrisonstatham 0:9e0195f256b1 129
harrisonstatham 0:9e0195f256b1 130
harrisonstatham 0:9e0195f256b1 131 uint32_t left_distance;
harrisonstatham 0:9e0195f256b1 132 uint32_t left_distance_prev;
harrisonstatham 0:9e0195f256b1 133
harrisonstatham 0:9e0195f256b1 134 uint32_t center_distance;
harrisonstatham 0:9e0195f256b1 135 uint32_t center_distance_prev;
harrisonstatham 0:9e0195f256b1 136
harrisonstatham 0:9e0195f256b1 137 uint32_t right_distance;
harrisonstatham 0:9e0195f256b1 138 uint32_t right_distance_prev;
harrisonstatham 0:9e0195f256b1 139
harrisonstatham 0:9e0195f256b1 140 // Flashes all 4 leds with right channel selected after n minutes.
harrisonstatham 0:9e0195f256b1 141 // 12:31-12:32 start
harrisonstatham 0:9e0195f256b1 142 //
harrisonstatham 0:9e0195f256b1 143
harrisonstatham 0:9e0195f256b1 144
harrisonstatham 0:9e0195f256b1 145 int main()
harrisonstatham 0:9e0195f256b1 146 {
harrisonstatham 0:9e0195f256b1 147 int status;
harrisonstatham 0:9e0195f256b1 148 uint32_t distance;
harrisonstatham 0:9e0195f256b1 149
harrisonstatham 0:9e0195f256b1 150 TurnLeft = 0;
harrisonstatham 0:9e0195f256b1 151 TurnRight = 0;
harrisonstatham 0:9e0195f256b1 152 Forward = 0;
harrisonstatham 0:9e0195f256b1 153 Reverse = 0;
harrisonstatham 0:9e0195f256b1 154
harrisonstatham 0:9e0195f256b1 155 left_distance = 2500;
harrisonstatham 0:9e0195f256b1 156 left_distance_prev = 2500;
harrisonstatham 0:9e0195f256b1 157
harrisonstatham 0:9e0195f256b1 158 center_distance = 2500;
harrisonstatham 0:9e0195f256b1 159 center_distance_prev = 2500;
harrisonstatham 0:9e0195f256b1 160
harrisonstatham 0:9e0195f256b1 161 right_distance = 2500;
harrisonstatham 0:9e0195f256b1 162 right_distance_prev = 2500;
harrisonstatham 0:9e0195f256b1 163
harrisonstatham 0:9e0195f256b1 164 int state = 0;
harrisonstatham 0:9e0195f256b1 165
harrisonstatham 0:9e0195f256b1 166 DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
harrisonstatham 0:9e0195f256b1 167
harrisonstatham 0:9e0195f256b1 168 device_i2c->frequency(3000000);
harrisonstatham 0:9e0195f256b1 169
harrisonstatham 0:9e0195f256b1 170 // Init the center channel
harrisonstatham 0:9e0195f256b1 171 InitChannel(device_i2c, 1);
harrisonstatham 0:9e0195f256b1 172
harrisonstatham 0:9e0195f256b1 173
harrisonstatham 0:9e0195f256b1 174 //loop taking and printing distance
harrisonstatham 0:9e0195f256b1 175 // bool didSwitchOver = false;
harrisonstatham 0:9e0195f256b1 176 int i = 0;
harrisonstatham 0:9e0195f256b1 177 bool CountUp = true;
harrisonstatham 0:9e0195f256b1 178
harrisonstatham 0:9e0195f256b1 179 uint32_t TheTest = 0;
harrisonstatham 0:9e0195f256b1 180
harrisonstatham 0:9e0195f256b1 181 while (1) {
harrisonstatham 0:9e0195f256b1 182
harrisonstatham 0:9e0195f256b1 183 status = board->sensor_centre->get_distance(&distance);
harrisonstatham 0:9e0195f256b1 184
harrisonstatham 0:9e0195f256b1 185 if (status == VL53L0X_ERROR_NONE) {
harrisonstatham 0:9e0195f256b1 186 pc.printf("D=%ld mm\r\n", distance);
harrisonstatham 0:9e0195f256b1 187
harrisonstatham 0:9e0195f256b1 188
harrisonstatham 0:9e0195f256b1 189 switch(state)
harrisonstatham 0:9e0195f256b1 190 {
harrisonstatham 0:9e0195f256b1 191 // Right
harrisonstatham 0:9e0195f256b1 192 case 2:
harrisonstatham 0:9e0195f256b1 193
harrisonstatham 0:9e0195f256b1 194 TheTest = right_distance_prev;
harrisonstatham 0:9e0195f256b1 195 right_distance_prev = right_distance;
harrisonstatham 0:9e0195f256b1 196 right_distance = distance;
harrisonstatham 0:9e0195f256b1 197
harrisonstatham 0:9e0195f256b1 198 // Handle step functions (hopefully) gracefully.
harrisonstatham 0:9e0195f256b1 199 if(TheTest <= 500 && distance > 500)
harrisonstatham 0:9e0195f256b1 200 {
harrisonstatham 0:9e0195f256b1 201 TurnRight = 0;
harrisonstatham 0:9e0195f256b1 202
harrisonstatham 0:9e0195f256b1 203 }
harrisonstatham 0:9e0195f256b1 204 else
harrisonstatham 0:9e0195f256b1 205 {
harrisonstatham 0:9e0195f256b1 206 TheTest = (uint32_t)((right_distance + right_distance_prev)/2);
harrisonstatham 0:9e0195f256b1 207
harrisonstatham 0:9e0195f256b1 208 if( TheTest <= 500 )
harrisonstatham 0:9e0195f256b1 209 {
harrisonstatham 0:9e0195f256b1 210 TurnRight = 1;
harrisonstatham 0:9e0195f256b1 211 }
harrisonstatham 0:9e0195f256b1 212 else
harrisonstatham 0:9e0195f256b1 213 {
harrisonstatham 0:9e0195f256b1 214 TurnRight = 0;
harrisonstatham 0:9e0195f256b1 215 }
harrisonstatham 0:9e0195f256b1 216
harrisonstatham 0:9e0195f256b1 217 }
harrisonstatham 0:9e0195f256b1 218
harrisonstatham 0:9e0195f256b1 219 break;
harrisonstatham 0:9e0195f256b1 220
harrisonstatham 0:9e0195f256b1 221 // Left
harrisonstatham 0:9e0195f256b1 222 case 0:
harrisonstatham 0:9e0195f256b1 223 TheTest = left_distance_prev;
harrisonstatham 0:9e0195f256b1 224 left_distance_prev = left_distance;
harrisonstatham 0:9e0195f256b1 225 left_distance = distance;
harrisonstatham 0:9e0195f256b1 226
harrisonstatham 0:9e0195f256b1 227 // Handle step functions (hopefully) gracefully.
harrisonstatham 0:9e0195f256b1 228 if(TheTest <= 500 && distance > 500)
harrisonstatham 0:9e0195f256b1 229 {
harrisonstatham 0:9e0195f256b1 230 TurnLeft = 0;
harrisonstatham 0:9e0195f256b1 231 }
harrisonstatham 0:9e0195f256b1 232 else
harrisonstatham 0:9e0195f256b1 233 {
harrisonstatham 0:9e0195f256b1 234 TheTest = (uint32_t)((left_distance + left_distance_prev)/2);
harrisonstatham 0:9e0195f256b1 235
harrisonstatham 0:9e0195f256b1 236
harrisonstatham 0:9e0195f256b1 237
harrisonstatham 0:9e0195f256b1 238 if( TheTest <= 500 )
harrisonstatham 0:9e0195f256b1 239 {
harrisonstatham 0:9e0195f256b1 240 TurnLeft = 1;
harrisonstatham 0:9e0195f256b1 241 }
harrisonstatham 0:9e0195f256b1 242 else
harrisonstatham 0:9e0195f256b1 243 {
harrisonstatham 0:9e0195f256b1 244 TurnLeft = 0;
harrisonstatham 0:9e0195f256b1 245 }
harrisonstatham 0:9e0195f256b1 246
harrisonstatham 0:9e0195f256b1 247 }
harrisonstatham 0:9e0195f256b1 248
harrisonstatham 0:9e0195f256b1 249 break;
harrisonstatham 0:9e0195f256b1 250
harrisonstatham 0:9e0195f256b1 251 // Center
harrisonstatham 0:9e0195f256b1 252 case 1:
harrisonstatham 0:9e0195f256b1 253 default:
harrisonstatham 0:9e0195f256b1 254
harrisonstatham 0:9e0195f256b1 255 TheTest = center_distance_prev;
harrisonstatham 0:9e0195f256b1 256
harrisonstatham 0:9e0195f256b1 257 center_distance_prev = center_distance;
harrisonstatham 0:9e0195f256b1 258 center_distance = distance;
harrisonstatham 0:9e0195f256b1 259
harrisonstatham 0:9e0195f256b1 260 // Handle step functions (hopefully) gracefully.
harrisonstatham 0:9e0195f256b1 261 if(TheTest <= 500 && distance > 500 )
harrisonstatham 0:9e0195f256b1 262 {
harrisonstatham 0:9e0195f256b1 263 Forward = 0;
harrisonstatham 0:9e0195f256b1 264 Reverse = 0;
harrisonstatham 0:9e0195f256b1 265 }
harrisonstatham 0:9e0195f256b1 266 else
harrisonstatham 0:9e0195f256b1 267 {
harrisonstatham 0:9e0195f256b1 268
harrisonstatham 0:9e0195f256b1 269 TheTest = (uint32_t)((center_distance + center_distance_prev)/2);
harrisonstatham 0:9e0195f256b1 270
harrisonstatham 0:9e0195f256b1 271 if( TheTest <= 200)
harrisonstatham 0:9e0195f256b1 272 {
harrisonstatham 0:9e0195f256b1 273 Forward = 1;
harrisonstatham 0:9e0195f256b1 274 Reverse = 0;
harrisonstatham 0:9e0195f256b1 275 }
harrisonstatham 0:9e0195f256b1 276 else if( TheTest >= 200 && TheTest <= 500 )
harrisonstatham 0:9e0195f256b1 277 {
harrisonstatham 0:9e0195f256b1 278 Forward = 0;
harrisonstatham 0:9e0195f256b1 279 Reverse = 1;
harrisonstatham 0:9e0195f256b1 280 }
harrisonstatham 0:9e0195f256b1 281 else
harrisonstatham 0:9e0195f256b1 282 {
harrisonstatham 0:9e0195f256b1 283 Forward = 0;
harrisonstatham 0:9e0195f256b1 284 Reverse = 0;
harrisonstatham 0:9e0195f256b1 285 }
harrisonstatham 0:9e0195f256b1 286
harrisonstatham 0:9e0195f256b1 287 }
harrisonstatham 0:9e0195f256b1 288
harrisonstatham 0:9e0195f256b1 289
harrisonstatham 0:9e0195f256b1 290
harrisonstatham 0:9e0195f256b1 291 break;
harrisonstatham 0:9e0195f256b1 292 }
harrisonstatham 0:9e0195f256b1 293
harrisonstatham 0:9e0195f256b1 294
harrisonstatham 0:9e0195f256b1 295
harrisonstatham 0:9e0195f256b1 296
harrisonstatham 0:9e0195f256b1 297
harrisonstatham 0:9e0195f256b1 298 i++;
harrisonstatham 0:9e0195f256b1 299 }
harrisonstatham 0:9e0195f256b1 300 else
harrisonstatham 0:9e0195f256b1 301 {
harrisonstatham 0:9e0195f256b1 302 if(MY_DEBUG) {
harrisonstatham 0:9e0195f256b1 303 pc.printf("COuld not get distance.\r\n");
harrisonstatham 0:9e0195f256b1 304 }
harrisonstatham 0:9e0195f256b1 305 distance = 2500; // 2500 mm
harrisonstatham 0:9e0195f256b1 306 }
harrisonstatham 0:9e0195f256b1 307
harrisonstatham 0:9e0195f256b1 308 // Take 5 measurements on the channel
harrisonstatham 0:9e0195f256b1 309 if( i > 2) {
harrisonstatham 0:9e0195f256b1 310
harrisonstatham 0:9e0195f256b1 311 if(CountUp)
harrisonstatham 0:9e0195f256b1 312 {
harrisonstatham 0:9e0195f256b1 313
harrisonstatham 0:9e0195f256b1 314 if( state == 2 )
harrisonstatham 0:9e0195f256b1 315 {
harrisonstatham 0:9e0195f256b1 316 state--;
harrisonstatham 0:9e0195f256b1 317 CountUp = false;
harrisonstatham 0:9e0195f256b1 318 }
harrisonstatham 0:9e0195f256b1 319 else
harrisonstatham 0:9e0195f256b1 320 {
harrisonstatham 0:9e0195f256b1 321 state++;
harrisonstatham 0:9e0195f256b1 322 CountUp = true;
harrisonstatham 0:9e0195f256b1 323 }
harrisonstatham 0:9e0195f256b1 324
harrisonstatham 0:9e0195f256b1 325 }
harrisonstatham 0:9e0195f256b1 326 else
harrisonstatham 0:9e0195f256b1 327 {
harrisonstatham 0:9e0195f256b1 328 if( state == 0 )
harrisonstatham 0:9e0195f256b1 329 {
harrisonstatham 0:9e0195f256b1 330 state++;
harrisonstatham 0:9e0195f256b1 331 CountUp = true;
harrisonstatham 0:9e0195f256b1 332 }
harrisonstatham 0:9e0195f256b1 333 else
harrisonstatham 0:9e0195f256b1 334 {
harrisonstatham 0:9e0195f256b1 335 state--;
harrisonstatham 0:9e0195f256b1 336 CountUp = false;
harrisonstatham 0:9e0195f256b1 337 }
harrisonstatham 0:9e0195f256b1 338 }
harrisonstatham 0:9e0195f256b1 339
harrisonstatham 0:9e0195f256b1 340 if(MY_DEBUG) {
harrisonstatham 0:9e0195f256b1 341 pc.printf("Doing switch over\r\n");
harrisonstatham 0:9e0195f256b1 342 }
harrisonstatham 0:9e0195f256b1 343 InitChannel(device_i2c, state);
harrisonstatham 0:9e0195f256b1 344
harrisonstatham 0:9e0195f256b1 345 if(MY_DEBUG) {
harrisonstatham 0:9e0195f256b1 346 pc.printf("Finishing switch over\r\n");
harrisonstatham 0:9e0195f256b1 347 }
harrisonstatham 0:9e0195f256b1 348
harrisonstatham 0:9e0195f256b1 349 i = 0;
harrisonstatham 0:9e0195f256b1 350 }
harrisonstatham 0:9e0195f256b1 351 }
harrisonstatham 0:9e0195f256b1 352 }
harrisonstatham 0:9e0195f256b1 353
harrisonstatham 0:9e0195f256b1 354
harrisonstatham 0:9e0195f256b1 355
harrisonstatham 0:9e0195f256b1 356