![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
A program which uses twin DC motors and a lidar to drive a robot controlled by a gamepad through an mbed.
Test
This is a test of the repository
main.cpp@14:6f3985a23eeb, 2019-11-23 (annotated)
- Committer:
- lballard9
- Date:
- Sat Nov 23 19:45:07 2019 +0000
- Revision:
- 14:6f3985a23eeb
- Parent:
- 13:1b31916de79e
- Child:
- 15:e012856b3a61
current working version
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
lballard9 | 12:4f635df5b368 | 1 | // Sweep the motor speed from full-speed reverse (-1.0) to full speed forwards (1.0) |
lballard9 | 12:4f635df5b368 | 2 | |
johnAlexander | 0:ce8359133ae6 | 3 | #include "mbed.h" |
lballard9 | 12:4f635df5b368 | 4 | #include "Motor.h" |
Davidroid | 10:891e10d3b4a6 | 5 | #include "XNucleo53L0A1.h" |
lballard9 | 12:4f635df5b368 | 6 | |
lballard9 | 12:4f635df5b368 | 7 | #define VL53L0_I2C_SDA p28 // To be changed based on Pins |
lballard9 | 12:4f635df5b368 | 8 | #define VL53L0_I2C_SCL p27 // To be changed based on Pins |
4180_1 | 11:5186cc367be0 | 9 | DigitalOut shdn(p26); |
johnAlexander | 0:ce8359133ae6 | 10 | |
lballard9 | 12:4f635df5b368 | 11 | Motor lm(p21, p7, p8); // pwm, fwd, rev |
lballard9 | 12:4f635df5b368 | 12 | Motor rm(p22, p6, p10); // pwm, fwd, rev |
lballard9 | 12:4f635df5b368 | 13 | //BusOut myled(LED1,LED2,LED3,LED4); |
lballard9 | 12:4f635df5b368 | 14 | Serial BT(p13,p14); |
lballard9 | 12:4f635df5b368 | 15 | Serial pc(USBTX,USBRX); |
lballard9 | 14:6f3985a23eeb | 16 | BusOut led(p15,p16,p19,p18); |
johnAlexander | 0:ce8359133ae6 | 17 | |
Davidroid | 10:891e10d3b4a6 | 18 | static XNucleo53L0A1 *board=NULL; |
lballard9 | 12:4f635df5b368 | 19 | bool stopMotor = false; |
lballard9 | 13:1b31916de79e | 20 | bool turbo = false; |
johnAlexander | 1:3483e701ec59 | 21 | |
lballard9 | 12:4f635df5b368 | 22 | float getSpeed(char& value) { |
lballard9 | 12:4f635df5b368 | 23 | |
lballard9 | 12:4f635df5b368 | 24 | float ones, tens = 0, hundreds, n2, n3, dot; |
lballard9 | 12:4f635df5b368 | 25 | |
lballard9 | 12:4f635df5b368 | 26 | dot = BT.getc(); |
lballard9 | 12:4f635df5b368 | 27 | n2 = BT.getc(); |
lballard9 | 12:4f635df5b368 | 28 | n3 = BT.getc(); |
lballard9 | 12:4f635df5b368 | 29 | |
lballard9 | 12:4f635df5b368 | 30 | ones = (float)(value -'0'); |
lballard9 | 12:4f635df5b368 | 31 | |
lballard9 | 12:4f635df5b368 | 32 | tens = (float)(n2 -'0'); |
lballard9 | 12:4f635df5b368 | 33 | tens = tens / 10.00; |
lballard9 | 12:4f635df5b368 | 34 | |
lballard9 | 12:4f635df5b368 | 35 | hundreds = (float)(n3 - '0'); |
lballard9 | 12:4f635df5b368 | 36 | hundreds = hundreds/100.00; |
lballard9 | 12:4f635df5b368 | 37 | |
lballard9 | 12:4f635df5b368 | 38 | return (ones + tens + hundreds); |
lballard9 | 12:4f635df5b368 | 39 | } |
lballard9 | 12:4f635df5b368 | 40 | |
lballard9 | 12:4f635df5b368 | 41 | void setMotorSpeed(Motor* motor, float mspeed) { |
lballard9 | 12:4f635df5b368 | 42 | if(mspeed <= 1.0){ |
lballard9 | 12:4f635df5b368 | 43 | if(abs(mspeed) > 0.2 && !stopMotor || mspeed > .2){ |
lballard9 | 12:4f635df5b368 | 44 | pc.printf("%0.2f is the motor speed\n\r", mspeed); |
lballard9 | 13:1b31916de79e | 45 | if(turbo) |
lballard9 | 12:4f635df5b368 | 46 | motor->speed(mspeed); |
lballard9 | 13:1b31916de79e | 47 | if(!turbo) |
lballard9 | 13:1b31916de79e | 48 | motor->speed(mspeed/1.75); |
lballard9 | 12:4f635df5b368 | 49 | } |
lballard9 | 12:4f635df5b368 | 50 | else |
lballard9 | 12:4f635df5b368 | 51 | motor->speed(0); |
lballard9 | 12:4f635df5b368 | 52 | } |
lballard9 | 12:4f635df5b368 | 53 | } |
lballard9 | 12:4f635df5b368 | 54 | void btMotorUpdate(Motor* motor) { |
lballard9 | 12:4f635df5b368 | 55 | |
lballard9 | 12:4f635df5b368 | 56 | char PlusMinus = BT.getc(); |
lballard9 | 12:4f635df5b368 | 57 | |
lballard9 | 12:4f635df5b368 | 58 | if(PlusMinus == '-'){ |
lballard9 | 12:4f635df5b368 | 59 | char n1 = BT.getc(); |
lballard9 | 12:4f635df5b368 | 60 | setMotorSpeed(motor, -(getSpeed(n1))); |
lballard9 | 12:4f635df5b368 | 61 | |
lballard9 | 12:4f635df5b368 | 62 | } else { |
lballard9 | 12:4f635df5b368 | 63 | setMotorSpeed(motor, getSpeed(PlusMinus)); |
lballard9 | 12:4f635df5b368 | 64 | |
lballard9 | 12:4f635df5b368 | 65 | } |
lballard9 | 12:4f635df5b368 | 66 | } |
lballard9 | 12:4f635df5b368 | 67 | |
lballard9 | 12:4f635df5b368 | 68 | void btSerialInterrupt() { |
lballard9 | 12:4f635df5b368 | 69 | |
lballard9 | 12:4f635df5b368 | 70 | char LRABYX = BT.getc(); |
lballard9 | 12:4f635df5b368 | 71 | |
lballard9 | 12:4f635df5b368 | 72 | if(LRABYX == 'L'){ |
lballard9 | 12:4f635df5b368 | 73 | btMotorUpdate(&lm); |
lballard9 | 12:4f635df5b368 | 74 | } |
lballard9 | 12:4f635df5b368 | 75 | if(LRABYX == 'R') { |
lballard9 | 12:4f635df5b368 | 76 | btMotorUpdate(&rm); |
lballard9 | 12:4f635df5b368 | 77 | } |
lballard9 | 13:1b31916de79e | 78 | if(LRABYX == 'A') { |
lballard9 | 13:1b31916de79e | 79 | turbo = !turbo; |
lballard9 | 13:1b31916de79e | 80 | } |
lballard9 | 12:4f635df5b368 | 81 | |
lballard9 | 12:4f635df5b368 | 82 | } |
lballard9 | 12:4f635df5b368 | 83 | |
lballard9 | 12:4f635df5b368 | 84 | |
lballard9 | 12:4f635df5b368 | 85 | int main() { |
lballard9 | 12:4f635df5b368 | 86 | |
lballard9 | 12:4f635df5b368 | 87 | DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL); |
lballard9 | 12:4f635df5b368 | 88 | board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2); |
lballard9 | 12:4f635df5b368 | 89 | uint32_t distance; |
lballard9 | 12:4f635df5b368 | 90 | shdn = 0; //must reset sensor for an mbed reset to work |
lballard9 | 12:4f635df5b368 | 91 | wait(0.1); |
lballard9 | 12:4f635df5b368 | 92 | shdn = 1; |
lballard9 | 12:4f635df5b368 | 93 | wait(0.1); |
lballard9 | 12:4f635df5b368 | 94 | |
4180_1 | 11:5186cc367be0 | 95 | /* init the 53L0A1 board with default values */ |
lballard9 | 12:4f635df5b368 | 96 | int status = board->init_board(); |
4180_1 | 11:5186cc367be0 | 97 | while (status) { |
4180_1 | 11:5186cc367be0 | 98 | status = board->init_board(); |
johnAlexander | 7:c8087e7333b8 | 99 | } |
lballard9 | 12:4f635df5b368 | 100 | |
lballard9 | 12:4f635df5b368 | 101 | BT.attach(&btSerialInterrupt); |
lballard9 | 12:4f635df5b368 | 102 | lm.speed(0); |
lballard9 | 12:4f635df5b368 | 103 | rm.speed(0); |
lballard9 | 14:6f3985a23eeb | 104 | led.write(0); |
lballard9 | 12:4f635df5b368 | 105 | |
4180_1 | 11:5186cc367be0 | 106 | while (1) { |
johnAlexander | 7:c8087e7333b8 | 107 | status = board->sensor_centre->get_distance(&distance); |
johnAlexander | 9:9733cfdb0a18 | 108 | if (status == VL53L0X_ERROR_NONE) { |
lballard9 | 12:4f635df5b368 | 109 | pc.printf("%d\n\r",distance); |
lballard9 | 12:4f635df5b368 | 110 | |
lballard9 | 13:1b31916de79e | 111 | if (distance < 300) { // Distance might need to be adjusted |
lballard9 | 12:4f635df5b368 | 112 | stopMotor = true; |
lballard9 | 14:6f3985a23eeb | 113 | led.write(15*!led.read()); |
lballard9 | 13:1b31916de79e | 114 | wait(.1); |
lballard9 | 12:4f635df5b368 | 115 | } |
lballard9 | 12:4f635df5b368 | 116 | else{ |
lballard9 | 12:4f635df5b368 | 117 | stopMotor = false; |
lballard9 | 14:6f3985a23eeb | 118 | led.write(0); |
lballard9 | 12:4f635df5b368 | 119 | } |
lballard9 | 12:4f635df5b368 | 120 | |
lballard9 | 12:4f635df5b368 | 121 | |
lballard9 | 12:4f635df5b368 | 122 | // Any Lighting or Sound Effects for Crash Detection |
lballard9 | 12:4f635df5b368 | 123 | |
johnAlexander | 7:c8087e7333b8 | 124 | } |
4180_1 | 11:5186cc367be0 | 125 | } |
lballard9 | 12:4f635df5b368 | 126 | |
lballard9 | 12:4f635df5b368 | 127 | } |