Remote Control Car

File content as of revision 3:4956cc0efdf3:

#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

#define AUTOPILOT 10
#define FORWARD 1
#define REVERSE -1
#define STOP 0

static XNucleo53L0A1 *board=NULL;
Motor M(p21, p22, p23, 1); // pwm, fwd, rev, can brake 
Servo S(p24);
int state = 0; //global variable stop state 
int status;
uint32_t distanceCenter;
uint32_t distanceLeft;
uint32_t distanceRight;

void getNewState() {
    //Logic for AdaFruit App
    char bnum=0;
    char bhit=0;
    if (blue.readable()) {
    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') {
                            state = 10; //autopilot state
                        }
                        break;
                    case '5': //forward
                        if (bhit=='1') {
                            state = 1; //forward state
                        } else {
                            state = 0; //stop state
                        }
                        break;
                    case '6': //reverse
                        if (bhit=='1') {
                            state = -1; //reverse state
                        } else {
                            state = 0; //stop state
                        }
                        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;
                    }
                }
            }
        }
    }
}

void autoPilot() {
    if ((distanceCenter >= 250 || distanceCenter == 0)) {
        M.speed(1.0);
        S = 0.5;
    }
    else if (distanceRight > distanceLeft) { //More space on right so turn right
        S = 1.0;
    }
    else { //or turn left
        S = 0;
    }
}


int main() {
    //LIDAR Initialization
    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();
    }
    
    while(1) { //main loop for motor control
        status = board->sensor_centre->get_distance(&distanceCenter);
        pc.printf("D=%ld mm\r\n", distanceCenter);
        getNewState();
        if ((distanceCenter >= 250 || distanceCenter == 0) && state == FORWARD) {
            M.speed(1.0);
        }
        else if (state == REVERSE) {
            M.speed(-1.0);
        }
        else if (state == AUTOPILOT) {
            autoPilot();
        }
        else {
            M.speed(0);
        }
    }
    
}