File content as of revision 14:0e6b26c1a7c5:

#include "RobotController.h"

RobotController::RobotController(PinName leftWheelPwm, PinName leftWheelFwd,
    PinName leftWheelRev, PinName rightWheelPwm, PinName rightWheelFwd,
    PinName rightWheelRev, PinName leftEncoder, PinName rightEncoder,
    PinName lidarShdn, PinName sda, PinName scl) :
    leftWheel(leftWheelPwm, leftWheelFwd, leftWheelRev),
    rightWheel(rightWheelPwm, rightWheelFwd, rightWheelRev),
    _leftEncoder(leftEncoder), _rightEncoder(rightEncoder), _lidarShdn(lidarShdn),
    imu(sda, scl, 0xD6, 0x3C) {
        lidarDevice = new DevI2C(sda, scl);
        lidarBoard = XNucleo53L0A1::instance(lidarDevice, A2, D8, D2);
        _lidarShdn = 0;
        wait(0.1);
        _lidarShdn = 1;
        wait(0.1);
        int status = lidarBoard->init_board();
        while(status) {
            status = lidarBoard->init_board();
        }
        lidarDistance = 0;
        imu.begin();
        imu.calibrate();
        w1 = 0.0;
        w2 = 0.0;
        t1 = 0.0;
        t2 = 0.0;
}

RobotController::~RobotController() {
    delete &leftWheel;
    delete &rightWheel;
    delete &leftEncoder;
    delete &rightEncoder;
    delete &imu;
    delete &t;
}

void RobotController::detectObstacles() {
    led = 0b0001;
    for (int i = 0; i < 360; i++) {
        _leftEncoder.reset();
        _rightEncoder.reset();
        leftWheel.speed(1.8);
        rightWheel.speed(-1.8);
        while((_leftEncoder.read() < 1) && (_rightEncoder.read() < 1));
        leftWheel.speed(0);
        rightWheel.speed(0);
        lidarBoard->sensor_centre->get_distance(&lidarDistance);
        obstacles[i] = (int)(lidarDistance*MMTOIN);
    }
    led = 0;
}

void RobotController::followTrajectory() {
    led = 0b1000;
    while(!pb);
    for (int i = 0; i < trajectoryLength; i = i + 2) {
        t.reset();
        yaw = 0.0;
        w1 = 0.0;
        w2 = 0.0;
        t1 = 0.0;
        t2 = 0.0;
        int angle = trajectory[i];
        if ((trajectory[i] >= 0) && (trajectory[i] < 90)) {
            angle = angle*ROTERRI;
        } else if (trajectory[i] < 180) {
            angle = angle*ROTERRII;
        } else if (trajectory[i] < 360) {
            angle = angle*ROTERRIII;
        }
        t.start();
        leftWheel.speed(0.2);
        rightWheel.speed(-0.2);
        while(yaw < angle) {
            yaw = yaw + (((w2+w1)/2.0)*(t2-t1));
            while(!imu.gyroAvailable());
            imu.readGyro();
            w1 = w2;
            w2 = imu.calcGyro(imu.gz);
            t1 = t2;
            t2 = t.read();
        }
        leftWheel.speed(0);
        rightWheel.speed(0);
        t.stop();
        leftEncoder.reset();
        rightEncoder.reset();
        int distance = (int)(trajectory[i + 1]*COUNTPERIN);
        leftWheel.speed(0.2);
        rightWheel.speed(0.2);
        while((leftEncoder.read() < distance) && (rightEncoder.read() < distance));
        leftWheel.speed(0);
        rightWheel.speed(0);
    }
    delete []trajectory;
    led = 0;
}