Remote Control Car

File content as of revision 2:5c9526ccb055:

#include "mbed.h"
#include "XNucleo53L0A1.h"
#include "Servo.h"
#include "motordriver.h"
#include <stdio.h>
Serial pc(USBTX,USBRX);
Serial blue(p9,p10);
DigitalOut shdn(p26);
DigitalOut myled(LED1);      

#define VL53L0_I2C_SDA   p28
#define VL53L0_I2C_SCL   p27

static XNucleo53L0A1 *board=NULL;
Motor M(p21, p22, p23, 1); // pwm, fwd, rev, can brake 
Servo S(p24);


int main() {
    int status;
    uint32_t distance;
    DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
    /* creates the 53L0A1 expansion board singleton obj */
    board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
    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 */
    status = board->init_board();
    while (status) {
        pc.printf("Failed to init board! \r\n");
        status = board->init_board();
    }
    
    
    //Logic for AdaFruit App
    char bnum=0;
    char bhit=0;
    while(1) {
        if (blue.getc()=='!') {
            if (blue.getc()=='B') { //button data packet
                bnum = blue.getc(); //button number
                bhit = blue.getc(); //1=hit, 0=release
                if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
                    myled = bnum - '0'; //current button number will appear on LEDs
                    switch (bnum) {
                        //case '1': //AutoPilot Mode
//                            if (bhit=='1') {
//                                M.speed(1.0);
//                                //loop taking and printing distance
//                                status = board->sensor_centre->get_distance(&distance);
//                                if (distance <= 50) {
//                                    //turn left or right depending on location of sensor
//                                    M.speed(0);
//                                }
//                            }
//                            break;
                        case '5': //forward
                            if (bhit=='1') {
                                status = board->sensor_centre->get_distance(&distance);
                                if (distance <= 50) {
                                    M.speed(1.0); //drive forward
                                } else {
                                    M.speed(0.0); //stop
                                }
                            } else {
                                M.speed(0.0); //stop
                            }   
                            break;
                        case '6': //reverse
                            if (bhit=='1') {
                                M.speed(-1.0); //reverse
                            } else {
                                M.speed(0.0); //stop
                            }
                            break;
                        case '7': //left
                            if (bhit=='1') {
                                S = S - 0.5; //turn left                                
                            }
                            break;
                        case '8': //right
                            if (bhit=='1') {
                                S = S + 0.5; //turn right
                            }
                            break;
                    }
                }
            }
        }
    }
}