Sam Stansfield
/
ProjectLasthope1
ok
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?
User | Revision | Line number | New 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 | } |