main.cpp

00001 // Sweep the motor speed from full-speed reverse (-1.0) to full speed forwards (1.0)
00002 
00003 #include "mbed.h"
00004 #include "Motor.h"
00005 #include "XNucleo53L0A1.h"
00006 
00007 #define VL53L0_I2C_SDA   p28 // To be changed based on Pins
00008 #define VL53L0_I2C_SCL   p27 // To be changed based on Pins
00009 DigitalOut shdn(p26);
00010 
00011 Motor lm(p21, p7, p8); // pwm, fwd, rev
00012 Motor rm(p22, p6, p10); // pwm, fwd, rev
00013 //BusOut myled(LED1,LED2,LED3,LED4);
00014 Serial BT(p13,p14);
00015 Serial pc(USBTX,USBRX);
00016 BusOut led(p15,p16,p19,p18);
00017 PwmOut spkr(p23);
00018 DigitalOut spkrenable(p5);
00019 
00020 static XNucleo53L0A1 *board=NULL;
00021 bool stopMotor = false;
00022 bool turbo = false;
00023 
00024 float getSpeed(char& value) {
00025 
00026     float ones, tens = 0, hundreds, n2, n3, dot;
00027 
00028     dot = BT.getc();
00029     n2 = BT.getc();
00030     n3 = BT.getc();
00031 
00032     ones = (float)(value -'0');
00033 
00034     tens = (float)(n2 -'0');
00035     tens = tens / 10.00;
00036 
00037     hundreds = (float)(n3 - '0');
00038     hundreds = hundreds/100.00;
00039 
00040     return (ones + tens + hundreds);
00041 }
00042 
00043 void setMotorSpeed(Motor* motor, float mspeed) {
00044 if(mspeed <= 1.0){
00045     if(abs(mspeed) > 0.2 && !stopMotor || mspeed > .2){
00046         pc.printf("%0.2f is the motor speed\n\r", mspeed);
00047         if(turbo)
00048         motor->speed(mspeed);
00049         if(!turbo)
00050         motor->speed(mspeed/1.25);
00051         }
00052     else
00053         motor->speed(0);
00054 }
00055 }
00056 void btMotorUpdate(Motor* motor) {
00057 
00058     char PlusMinus = BT.getc();
00059 
00060     if(PlusMinus == '-'){
00061         char n1 = BT.getc();
00062         setMotorSpeed(motor, -(getSpeed(n1)));
00063 
00064     } else {
00065         setMotorSpeed(motor, getSpeed(PlusMinus));
00066 
00067     }
00068 }
00069 
00070 void btSerialInterrupt() {
00071 
00072     char LRABYX = BT.getc();
00073 
00074     if(LRABYX == 'L'){
00075         btMotorUpdate(&lm);
00076     }
00077     if(LRABYX == 'R') {
00078         btMotorUpdate(&rm);
00079     }
00080     if(LRABYX == 'A') {
00081         turbo = true;
00082         }  
00083     if(LRABYX == 'B') {
00084         turbo = false;
00085         }  
00086    
00087 }
00088 
00089 
00090 int main() {
00091 
00092      DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
00093      board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
00094      uint32_t distance;
00095      shdn = 0; //must reset sensor for an mbed reset to work
00096      wait(0.1);
00097      shdn = 1;
00098      wait(0.1);
00099 
00100     /* init the 53L0A1 board with default values */
00101     int status = board->init_board();
00102     while (status) {
00103         status = board->init_board();
00104     }
00105 
00106     BT.attach(&btSerialInterrupt);
00107     lm.speed(0);
00108     rm.speed(0);
00109     led.write(0);
00110     spkrenable = 0;
00111     
00112 
00113     while (1) {
00114         status = board->sensor_centre->get_distance(&distance);
00115         if (status == VL53L0X_ERROR_NONE) {
00116             pc.printf("%d\n\r",distance);
00117             
00118             if (distance < 300) { // Distance might need to be adjusted
00119                 stopMotor = true;
00120                 led.write(15*!led.read());
00121                 spkrenable = 1;
00122                 spkr = .5;
00123                 wait(.1); 
00124                 spkr = 0;   
00125                 spkrenable = 0;
00126                 }
00127             else{
00128                 stopMotor = false;
00129                 
00130                 led.write(0);
00131                 }
00132                 
00133             
00134                 // Any Lighting or Sound Effects for Crash Detection
00135             
00136         }
00137     }
00138 
00139 }