Sam Stansfield
/
ProjectLasthope1
ok
main.cpp
- Committer:
- sas638
- Date:
- 2022-05-08
- Revision:
- 0:e26f2e97182d
File content as of revision 0:e26f2e97182d:
/* mbed Microcontroller Library * Copyright (c) 2019 ARM Limited * SPDX-License-Identifier: Apache-2.0 */ #include "mbed.h" #include "platform/mbed_thread.h" #include "tb6612.h" #include "VL53L0X.h" I2C i2c(D14,D15); VL53L0X vl_sensors[3] = {(&i2c),(&i2c),(&i2c)} BusOut vl_shutdown(D0,D1,D3); Serial usb(USBTX. USBRX, 115200); TB6612 motorL(D2,D4,D5); TB6612 motorR(D9,D10,D11); float DR; //distance measured if need to turn 45º right - manual measured value float DL; //distance measured if need to turn 45º left - manual measured value float D; // distance measured to go straight - given value float TM; //Time needed to turn 45º in either direction float DI; // distance measured float R; //Ratio between D1 and D float T; //Calculated turn time for current offset float Dlefts; //left sensor reading float Drights; //right sensor reading float Dfronts; // front sensor reading int main() { usb.printf("Multiple VL53L0X\n\r"); uint8_t expander_shutdown_mask = 1; for(uint8_t i = 0; i < 3; i++) { vl_shutdown = expander_shutdown_mask; expander_shutdown_mask = (expander_shutdown_mask << 1) + 1; vl_sensors[i].init(); vl_sensors[i].setDeviceAddress(0x40 + i); vl_sensors[i].setModeContinuous(); vl_sensors[i].startContinuous(); } uint16_t results[3]; while(1) { for(uint8_t i = 0; i < 3; i++) { results[i] = vl_sensors[i].getRangeMillimeters(); } Dfronts = results[0]; Dlefts = results[1]; Drigts = results [2]; DI = Dfronts; if(DI>D) { R = DR/DI; T = TM/R; motorR.setSpeed(-1); motorL.setSpeed(1); usb.printf("right turn\n\r"); wait(T); } if (DI<D) { R = DL/DI; T = TM/R; motorR.setSpeed(1); motorL.setSpeed(-1); usb.printf("left turn\n\r"); wait(T); } } }