File content as of revision 9:7d74c22ed54e:

#include "mbed.h"


Serial pc(USBTX, USBRX);
//For the solenoid
#define OFF 0
#define ON 1

//For motor control
#define PWM_PERIOD_US 1000 //For setting PWM periods to 1 milliseconds. I made this number up
#define STOP 0
#define FORWARD 1
#define BACKWARD 2
//For line following, use the previous defines and the follwoing
#define LEFT 3
#define RIGHT 4
#define CHOOSEPATH 5

#define LINE_THRESHOLD 0.8

#define RED_CLEAR_VALUE_MAX 20000
#define BLUE_CLEAR_VALUE_MAX 55000

//Debug LED
DigitalOut redled(LED_RED);

//Connections for the Adafruit TCS34725 colour sensor
I2C i2c(PTC9, PTC8); //(SDA, SCL)


//Set PWMs for controlling the H-bridge for the motor speed
PwmOut PWMmotorLeft(PTA4); //Connect to EN1 of L298N
PwmOut PWMmotorRight(PTA5); //Connect to EN2 of L298N

BusOut leftMotorMode(PTC17,PTC16); //Connect to IN1 and IN2 of L298N
BusOut rightMotorMode(PTC13,PTC12); //Connect to IN3 and IN4 of L298N

DigitalOut solenoid(PTC3); //For the gate of the solenoid control MOSFET

//For black line detection
AnalogIn QTR3A_1(PTB3);
AnalogIn QTR3A_2(PTC2);
AnalogIn QTR3A_3(PTC1);
AnalogIn QTR3A_4(PTB0);
AnalogIn QTR3A_5(PTB1);
AnalogIn QTR3A_6(PTB2);


//Remote control novel feature
Serial bluetooth(PTE0,PTE1);


//A lookup table of which direction to turn the robot based on the values of all 6 sensor readings
//e.g. 0 = 000000 meaning no sensors detect a line and directionLookup[0] = STOP
//e.g. 12 = 001100 meaning the middle two sensors detect a line directionLookup[12] = FORWARD

int directionLookup[64] = {STOP, RIGHT, RIGHT, RIGHT, FORWARD, RIGHT, FORWARD, RIGHT, FORWARD, LEFT, //0-9
                            CHOOSEPATH, CHOOSEPATH, FORWARD, CHOOSEPATH, FORWARD, FORWARD, RIGHT, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, //10-19
                            CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, FORWARD, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, FORWARD, CHOOSEPATH, //20-29
                            FORWARD, FORWARD, LEFT, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, //30-39
                            RIGHT, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, LEFT, CHOOSEPATH, //40-49
                            CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, LEFT, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, //50-59
                            FORWARD, CHOOSEPATH, FORWARD, FORWARD
                           }; //60-63

                           

const int sensor_addr = 41 << 1; //this should just be equal to 82, haven't touched it though


class SolenoidController
{
public:
    bool state;


    void initialize();
    void off();
    void on();
    void controlLogic(bool, bool, bool, bool);

private:
    bool paper_detected;
};


void SolenoidController::off()
{
    state = OFF;
    solenoid = OFF;
}

void SolenoidController::on()
{
    state = ON;
    solenoid = ON;
}

void SolenoidController::initialize()
{
    paper_detected = false;
    off();
}

void SolenoidController::controlLogic(bool red_path, bool blue_path, bool red_detected, bool blue_detected)
{
//Logic for the solenoid based on colour detected

    //Detect the first sheet of paper if blue and pick up the disc
    if(blue_detected && !paper_detected && !state) {
        paper_detected = true;
        blue_path = true;
        on();
    }

    //Detect the first sheet of paper if red and pick up the disc
    else if(red_detected && !paper_detected && !state) {
        paper_detected = true;
        red_path = true;
        on();
    }

     //Detect the end of the first sheet of paper
    else if(!blue_detected && !red_detected && paper_detected) {
        paper_detected = false;
    }

    //Drop the disc once the second blue paper is detected
    else if(blue_detected && blue_path && !paper_detected && state) {
        paper_detected = true;
        off();
    }

    //Drop the disc once the second red paper is detected
    else if(red_detected && red_path && !paper_detected && state) {
        paper_detected = true;
        off();
    }
}

class MotorController
{
public:
    int state;
    int speed;
    int lastTurn;


    void initialize();
    void setSpeed(int pulsewidth_us);
    void stopMotors();
    void goForward();
    void goBackward();
    void turnLeft();
    void turnRight();
    /*
    void turnLeftHard();
    void turnRightHard();
    */
    void changeDirection(int);

private:
    void setLeftMotorMode(int);
    void setRightMotorMode(int);
    void setLeftMotorSpeed(int);
    void setRightMotorSpeed(int);
};

void MotorController::initialize()
{
    state = STOP;
    speed = 0;
    PWMmotorLeft.period_us(PWM_PERIOD_US);
    PWMmotorRight.period_us(PWM_PERIOD_US);

}

void MotorController::setSpeed(int pulsewidth_us)
{
    speed = pulsewidth_us;
}

void MotorController::setLeftMotorSpeed(int pulsewidth_us)
{
    PWMmotorLeft.pulsewidth_us(pulsewidth_us);
}


void MotorController::setRightMotorSpeed(int pulsewidth_us)
{
    PWMmotorRight.pulsewidth_us(pulsewidth_us);
}


void MotorController::setLeftMotorMode(int mode)
{
    leftMotorMode = mode;
}

void MotorController::setRightMotorMode(int mode)
{
    rightMotorMode = mode;
}


void MotorController::stopMotors()
{
    setLeftMotorMode(STOP);
    setRightMotorMode(STOP);
}

void MotorController::goForward()
{
    state = FORWARD;

    setLeftMotorMode(FORWARD);
    setRightMotorMode(FORWARD);


    setLeftMotorSpeed(speed);
    setRightMotorSpeed(speed);

    //wait(0.2);
    //stopMotors();

}

void MotorController::goBackward()
{
    state =  BACKWARD;

    setLeftMotorMode(BACKWARD);
    setRightMotorMode(BACKWARD);

    setLeftMotorSpeed(speed);
    setRightMotorSpeed(speed);

    //wait(0.2);
    //stopMotors();
}

void MotorController::turnLeft()
{
    state = LEFT;
    lastTurn = LEFT;

    setLeftMotorMode(BACKWARD);
    setRightMotorMode(FORWARD);

    setLeftMotorSpeed(speed);
    setRightMotorSpeed(speed);

    /* wait(0.05);
     stopMotors();
     */
}



void MotorController::turnRight()
{
    state = RIGHT;
    lastTurn = RIGHT;
    setLeftMotorMode(FORWARD);
    setRightMotorMode(BACKWARD);

    setLeftMotorSpeed(speed);
    setRightMotorSpeed(speed);
}

void MotorController::changeDirection(int direction)
{

    switch(direction) {

        case STOP:
            stopMotors();
            break;

        case FORWARD:
            goForward();
            setSpeed(PWM_PERIOD_US * 0.5);
            break;

        case BACKWARD:
            goBackward();
            setSpeed(PWM_PERIOD_US * 0.5);
            break;

        case LEFT:
            turnLeft();
            setSpeed(PWM_PERIOD_US * 0.7);
            break;

        case RIGHT:
            turnRight();
            setSpeed(PWM_PERIOD_US *0.7);
            break;
    }
}

class ColourSensor
{
public:

    bool blue_detected;
    bool red_detected;

    void initialize();
    void read();
};


void ColourSensor::initialize()
{

    i2c.frequency(200000);

    blue_detected = false;
    red_detected = false;

    char id_regval[1] = {146};
    char data[1] = {0};
    i2c.write(sensor_addr,id_regval,1, true);
    i2c.read(sensor_addr,data,1,false);

    char timing_register[2] = {129,0};
    i2c.write(sensor_addr,timing_register,2,false);

    char control_register[2] = {143,0};
    i2c.write(sensor_addr,control_register,2,false);

    char enable_register[2] = {128,3};
    i2c.write(sensor_addr,enable_register,2,false);
}

void ColourSensor::read()
{

    char clear_reg[1] = {148};
    char clear_data[2] = {0,0};
    i2c.write(sensor_addr,clear_reg,1, true);
    i2c.read(sensor_addr,clear_data,2, false);

    int clear_value = ((int)clear_data[1] << 8) | clear_data[0];

    char red_reg[1] = {150};
    char red_data[2] = {0,0};
    i2c.write(sensor_addr,red_reg,1, true);
    i2c.read(sensor_addr,red_data,2, false);

    int red_value = ((int)red_data[1] << 8) | red_data[0];

    char green_reg[1] = {152};
    char green_data[2] = {0,0};
    i2c.write(sensor_addr,green_reg,1, true);
    i2c.read(sensor_addr,green_data,2, false);

    int green_value = ((int)green_data[1] << 8) | green_data[0];

    char blue_reg[1] = {154};
    char blue_data[2] = {0,0};
    i2c.write(sensor_addr,blue_reg,1, true);
    i2c.read(sensor_addr,blue_data,2, false);

    int blue_value = ((int)blue_data[1] << 8) | blue_data[0];


    //Detect the colour of the paper

    //Red is detected if their is the unfiltered light is below a threshold
    //and there is at least twice as much red light copmared to blue light
    if(clear_value < RED_CLEAR_VALUE_MAX && (red_value > (blue_value*2))) {
        red_detected = true;
    }

    else {
        red_detected = false;
    }


    //Similar to detection for red, but with a different threshold
    if(clear_value < BLUE_CLEAR_VALUE_MAX && blue_value > (red_value*2)) {
        blue_detected = true;
    }

    else {
        blue_detected = false;
    }
}

class LineFollower
{

public:
    bool lineDetected[6];
    bool red_path;
    bool blue_path;


    void initialize();
    void readSensors();
    int chooseDirection();
};

void LineFollower::initialize()
{

    for(int i = 0; i < 6; i++) {
        lineDetected[i] = false;
    }

    red_path = false;
    blue_path = false;


}

void LineFollower::readSensors()
{
    if(QTR3A_1.read() > LINE_THRESHOLD) {
        lineDetected[0] = true;
    }

    else {
        lineDetected[0] = false;
    }

    if(QTR3A_2.read() > LINE_THRESHOLD) {
        lineDetected[1] = true;
    }

    else {
        lineDetected[1] = false;
    }
    if(QTR3A_3.read() > LINE_THRESHOLD) {
        lineDetected[2] = true;
    }

    else {
        lineDetected[2] = false;
    }
    if(QTR3A_4.read() > LINE_THRESHOLD) {
        lineDetected[3] = true;
    }

    else {
        lineDetected[3] = false;
    }
    if(QTR3A_5.read() > LINE_THRESHOLD) {
        lineDetected[4] = true;
    }

    else {
        lineDetected[4] = false;
    }
    if(QTR3A_6.read() > LINE_THRESHOLD) {
        lineDetected[5] = true;
    }

    else {
        lineDetected[5] = false;
    }
}


int LineFollower::chooseDirection()
{
    

    int direction = STOP;

    int sensorData = 0x3F & ((lineDetected[5] << 5) + (lineDetected[4] << 4) + (lineDetected[3]<< 3) +
                              (lineDetected[2] << 2) + (lineDetected[1] << 1) + (lineDetected[0]));
                              
    pc.printf("\n\rSensor data = %d", sensorData);

    direction = directionLookup[sensorData];

    pc.printf("\n\rTable result = %d", direction);
    
    if(direction == CHOOSEPATH) {

        if(red_path) {
            direction = LEFT;
        }

        else if(blue_path) {
            direction = RIGHT;
        } else {
            direction = FORWARD;

        }
    }
    
    pc.printf("\n\rChosen direction = %d", direction);
    return direction;
}
void bluetoothControl(MotorController motorController, SolenoidController solenoidController)
{
    bluetooth.baud(9600);

    char c = '0';
    char state = 'F';
    int speed = 0;

    while(bluetooth.readable()) {

        c = bluetooth.getc();

        switch(c) {

            case 'F':
                if(state != 'F') {
                    state = 'F';
                    speed = PWM_PERIOD_US * 0.4;
                    motorController.setSpeed(speed);
                    motorController.goForward();
                }

                else {
                    speed += PWM_PERIOD_US * 0.1;
                    motorController.setSpeed(speed);
                    motorController.goForward();
                }
                break;

            case 'B':
                if(state != 'B') {
                    state = 'B';
                    speed = PWM_PERIOD_US * 0.4;
                    motorController.setSpeed(speed);
                    motorController.goBackward();
                }

                else {
                    speed += PWM_PERIOD_US * 0.1;
                    motorController.setSpeed(speed);
                    motorController.goBackward();
                }
                break;

            case 'L':
                if(state != 'L') {
                    state = 'L';
                    speed = PWM_PERIOD_US * 0.4;;
                    motorController.setSpeed(speed);
                    motorController.turnLeft();
                }

                else {
                    speed += PWM_PERIOD_US * 0.1;
                    motorController.setSpeed(speed);
                    motorController.turnLeft();
                }
                break;

            case 'R':
                if(state != 'R') {
                    state = 'R';
                    speed = PWM_PERIOD_US * 0.4;
                    motorController.setSpeed(speed);
                    motorController.turnRight();
                }

                else {
                    speed += PWM_PERIOD_US * 0.1;
                    motorController.setSpeed(speed);
                    motorController.turnRight();
                }
                break;

            case 'X':
                state = 'X';
                speed = 0;
                motorController.setSpeed(speed);
                motorController.stopMotors();
                break;

            case 'S':
                if(solenoidController.state) {
                    solenoidController.off();
                } else if(!solenoidController.state) {
                    solenoidController.on();
                }
                break;

        }

        c='0';

    }
}




int main()
{


    bool path_found = false;

    MotorController motorController;
    SolenoidController solenoidController;
    LineFollower lineFollower;
    ColourSensor colourSensor;


    motorController.initialize();
    lineFollower.initialize();
    colourSensor.initialize();
    solenoidController.initialize();

    //Blink LED after reset
    redled = 1;
    wait(0.5);
    redled = 0;
    wait(1);
    redled = 1;

    //Start off going straight
    motorController.setSpeed(PWM_PERIOD_US * 0.4);
    motorController.goForward();




    while(true) {



        if(bluetooth.readable()) {
            bluetoothControl(motorController, solenoidController);
        }



        lineFollower.readSensors();
        motorController.changeDirection(lineFollower.chooseDirection());


        if(colourSensor.red_detected and !path_found) {
            path_found = true;
            lineFollower.red_path = true;
        }

        else if(colourSensor.blue_detected and !path_found) {
            path_found = true;
            lineFollower.blue_path = true;
        }

        solenoidController.controlLogic(lineFollower.red_path, lineFollower.blue_path, colourSensor.red_detected, colourSensor.blue_detected);

        //Blink LED every loop to ensure program isn't stuck
        redled = !redled;
        pc.printf("\n\rRound the loop");
    }

}