4180 Team / Mbed 2 deprecated MbedSpyBot

A program which uses twin DC motors and a lidar to drive a robot controlled by a gamepad through an mbed.

Dependencies:   mbed Motor

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?

UserRevisionLine numberNew 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 }