Sam Stansfield / Mbed OS ProjectLasthope1

ok

Dependencies:   TB6612 VL53L0X

main.cpp@0:e26f2e97182d, 2022-05-08 (annotated)

Committer:
sas638
Date:
Sun May 08 20:14:38 2022 +0000
Revision:
0:e26f2e97182d
ok;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sas638 0:e26f2e97182d 1 /* mbed Microcontroller Library
sas638 0:e26f2e97182d 2 * Copyright (c) 2019 ARM Limited
sas638 0:e26f2e97182d 3 * SPDX-License-Identifier: Apache-2.0
sas638 0:e26f2e97182d 4 */
sas638 0:e26f2e97182d 5
sas638 0:e26f2e97182d 6 #include "mbed.h"
sas638 0:e26f2e97182d 7 #include "platform/mbed_thread.h"
sas638 0:e26f2e97182d 8 #include "tb6612.h"
sas638 0:e26f2e97182d 9 #include "VL53L0X.h"
sas638 0:e26f2e97182d 10
sas638 0:e26f2e97182d 11 I2C i2c(D14,D15);
sas638 0:e26f2e97182d 12 VL53L0X vl_sensors[3] = {(&i2c),(&i2c),(&i2c)}
sas638 0:e26f2e97182d 13 BusOut vl_shutdown(D0,D1,D3);
sas638 0:e26f2e97182d 14 Serial usb(USBTX. USBRX, 115200);
sas638 0:e26f2e97182d 15 TB6612 motorL(D2,D4,D5);
sas638 0:e26f2e97182d 16 TB6612 motorR(D9,D10,D11);
sas638 0:e26f2e97182d 17
sas638 0:e26f2e97182d 18 float DR; //distance measured if need to turn 45º right - manual measured value
sas638 0:e26f2e97182d 19 float DL; //distance measured if need to turn 45º left - manual measured value
sas638 0:e26f2e97182d 20 float D; // distance measured to go straight - given value
sas638 0:e26f2e97182d 21 float TM; //Time needed to turn 45º in either direction
sas638 0:e26f2e97182d 22 float DI; // distance measured
sas638 0:e26f2e97182d 23 float R; //Ratio between D1 and D
sas638 0:e26f2e97182d 24 float T; //Calculated turn time for current offset
sas638 0:e26f2e97182d 25 float Dlefts; //left sensor reading
sas638 0:e26f2e97182d 26 float Drights; //right sensor reading
sas638 0:e26f2e97182d 27 float Dfronts; // front sensor reading
sas638 0:e26f2e97182d 28
sas638 0:e26f2e97182d 29 int main()
sas638 0:e26f2e97182d 30 {
sas638 0:e26f2e97182d 31 usb.printf("Multiple VL53L0X\n\r");
sas638 0:e26f2e97182d 32
sas638 0:e26f2e97182d 33 uint8_t expander_shutdown_mask = 1;
sas638 0:e26f2e97182d 34 for(uint8_t i = 0; i < 3; i++) {
sas638 0:e26f2e97182d 35 vl_shutdown = expander_shutdown_mask;
sas638 0:e26f2e97182d 36 expander_shutdown_mask = (expander_shutdown_mask << 1) + 1;
sas638 0:e26f2e97182d 37 vl_sensors[i].init();
sas638 0:e26f2e97182d 38 vl_sensors[i].setDeviceAddress(0x40 + i);
sas638 0:e26f2e97182d 39 vl_sensors[i].setModeContinuous();
sas638 0:e26f2e97182d 40 vl_sensors[i].startContinuous();
sas638 0:e26f2e97182d 41 }
sas638 0:e26f2e97182d 42 uint16_t results[3];
sas638 0:e26f2e97182d 43
sas638 0:e26f2e97182d 44 while(1) {
sas638 0:e26f2e97182d 45 for(uint8_t i = 0; i < 3; i++) {
sas638 0:e26f2e97182d 46 results[i] = vl_sensors[i].getRangeMillimeters();
sas638 0:e26f2e97182d 47 }
sas638 0:e26f2e97182d 48 Dfronts = results[0];
sas638 0:e26f2e97182d 49 Dlefts = results[1];
sas638 0:e26f2e97182d 50 Drigts = results [2];
sas638 0:e26f2e97182d 51
sas638 0:e26f2e97182d 52 DI = Dfronts;
sas638 0:e26f2e97182d 53
sas638 0:e26f2e97182d 54 if(DI>D) {
sas638 0:e26f2e97182d 55 R = DR/DI;
sas638 0:e26f2e97182d 56 T = TM/R;
sas638 0:e26f2e97182d 57 motorR.setSpeed(-1);
sas638 0:e26f2e97182d 58 motorL.setSpeed(1);
sas638 0:e26f2e97182d 59 usb.printf("right turn\n\r");
sas638 0:e26f2e97182d 60 wait(T);
sas638 0:e26f2e97182d 61 }
sas638 0:e26f2e97182d 62 if (DI<D) {
sas638 0:e26f2e97182d 63 R = DL/DI;
sas638 0:e26f2e97182d 64 T = TM/R;
sas638 0:e26f2e97182d 65 motorR.setSpeed(1);
sas638 0:e26f2e97182d 66 motorL.setSpeed(-1);
sas638 0:e26f2e97182d 67 usb.printf("left turn\n\r");
sas638 0:e26f2e97182d 68 wait(T);
sas638 0:e26f2e97182d 69 }
sas638 0:e26f2e97182d 70 }
sas638 0:e26f2e97182d 71
sas638 0:e26f2e97182d 72 }