Test

File content as of revision 17:54dec877b815:

// Sweep the motor speed from full-speed reverse (-1.0) to full speed forwards (1.0)

#include "mbed.h"
#include "Motor.h"
#include "XNucleo53L0A1.h"

#define VL53L0_I2C_SDA   p28 // To be changed based on Pins
#define VL53L0_I2C_SCL   p27 // To be changed based on Pins
DigitalOut shdn(p26);

Motor lm(p21, p7, p8); // pwm, fwd, rev
Motor rm(p22, p6, p10); // pwm, fwd, rev
//BusOut myled(LED1,LED2,LED3,LED4);
Serial BT(p13,p14);
Serial pc(USBTX,USBRX);
BusOut led(p15,p16,p19,p18);
PwmOut spkr(p23);
DigitalOut spkrenable(p5);

static XNucleo53L0A1 *board=NULL;
bool stopMotor = false;
bool turbo = false;

float getSpeed(char& value) {

    float ones, tens = 0, hundreds, n2, n3, dot;

    dot = BT.getc();
    n2 = BT.getc();
    n3 = BT.getc();

    ones = (float)(value -'0');

    tens = (float)(n2 -'0');
    tens = tens / 10.00;

    hundreds = (float)(n3 - '0');
    hundreds = hundreds/100.00;

    return (ones + tens + hundreds);
}

void setMotorSpeed(Motor* motor, float mspeed) {
if(mspeed <= 1.0){
    if(abs(mspeed) > 0.2 && !stopMotor || mspeed > .2){
        pc.printf("%0.2f is the motor speed\n\r", mspeed);
        if(turbo)
        motor->speed(mspeed);
        if(!turbo)
        motor->speed(mspeed/1.25);
        }
    else
        motor->speed(0);
}
}
void btMotorUpdate(Motor* motor) {

    char PlusMinus = BT.getc();

    if(PlusMinus == '-'){
        char n1 = BT.getc();
        setMotorSpeed(motor, -(getSpeed(n1)));

    } else {
        setMotorSpeed(motor, getSpeed(PlusMinus));

    }
}

void btSerialInterrupt() {

    char LRABYX = BT.getc();

    if(LRABYX == 'L'){
        btMotorUpdate(&lm);
    }
    if(LRABYX == 'R') {
        btMotorUpdate(&rm);
    }
    if(LRABYX == 'A') {
        turbo = true;
        }  
    if(LRABYX == 'B') {
        turbo = false;
        }  
   
}


int main() {

     DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
     board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
     uint32_t distance;
     shdn = 0; //must reset sensor for an mbed reset to work
     wait(0.1);
     shdn = 1;
     wait(0.1);

    /* init the 53L0A1 board with default values */
    int status = board->init_board();
    while (status) {
        status = board->init_board();
    }

    BT.attach(&btSerialInterrupt);
    lm.speed(0);
    rm.speed(0);
    led.write(0);
    spkrenable = 0;
    

    while (1) {
        status = board->sensor_centre->get_distance(&distance);
        if (status == VL53L0X_ERROR_NONE) {
            pc.printf("%d\n\r",distance);
            
            if (distance < 300) { // Distance might need to be adjusted
                stopMotor = true;
                led.write(15*!led.read());
                spkrenable = 1;
                spkr = .5;
                wait(.1); 
                spkr = 0;   
                spkrenable = 0;
                }
            else{
                stopMotor = false;
                
                led.write(0);
                }
                
            
                // Any Lighting or Sound Effects for Crash Detection
            
        }
    }

}