File content as of revision 18:6e8eae661cf3:

#include "mbed.h"
#include <string>
using namespace std;
Serial blue(PTC4,PTC3);

//motor select pins
DigitalOut motor_lf(PTE2);
DigitalOut motor_lb(PTE3);
DigitalOut motor_rf(PTE4);
DigitalOut motor_rb(PTE5);


DigitalIn input(PTC1);  //input from sensor array
//DigitalOut Line_right(LED_GREEN);//no line detected
//DigitalOut blue(LED_BLUE);

BusOut sensor(PTD7,PTE1,PTE0);//multiplex sensors
AnalogOut level(PTE30);//set comparator level needs to be tuned for each sensor (could create program)


//Frequency of Pulse Width Modulated signal in Hz
#define PWM_FREQ          1000

//PWM pin (Enable 1 and 2)
PwmOut motor_l (PTC2);
PwmOut motor_r (PTE29);

//LED to test
DigitalOut led(LED_RED);

//integer value of sensors (has to be global)
int val_r=0;
int val_l=0;
int old_val_r=0;
int old_val_l=0;

//Flags for decisions
int l_turn_spot = 0;
int r_turn_spot = 0;
int l_180_spot = 0;
int r_180_spot = 0;
int forwards = 0;
int backwards = 0;
int spin = 0;
//Junction Counter (hard coding deccsions on junctions)
int junction_cnt = 0;

void set_direction( int direction, float duty_l, float duty_r)
{
    switch( direction ) {
        case 0x11: { //forward
            motor_r.write( duty_r);
            //Look up ternary statements -- {condition ? value_if_true : value_if_false} This makes 0<result<1
            motor_l.write( duty_l);

            motor_rf=1;
            motor_rb=0;
            motor_lf=1;
            motor_lb=0;
            //   blue.printf("Going forward, right:%f ; left:%f\n", duty_r, duty_l);
            break;
        }
        case 0x00: { //backward
            motor_r.write( duty_r);
            motor_l.write( duty_l);

            motor_rf=0;
            motor_rb=1;
            motor_lf=0;
            motor_lb=1;
            //   blue.printf("Going backward, right:%f ; left:%f\n", duty_r, duty_l);
            break;
        }
        case 0x01: {  //spin left  --     Right forward, left backward
            motor_r.write( duty_r);
            motor_l.write( duty_l);

            motor_rf=1;
            motor_rb=0;
            motor_lf=0;
            motor_lb=1;
            //     blue.printf("Spinning Left, right:%f ; left:%f\n", duty_r, duty_l);
            break;
        }
        case 0x10: {    //spin right
            motor_r.write( duty_r);
            motor_l.write( duty_l);

            motor_rf=0;
            motor_rb=1;
            motor_lf=1;
            motor_lb=0;
            //   blue.printf("Spinning Right, right:%f ; left:%f\n", duty_r, duty_l);
            break;
        }
    }
}

void motor_result() //val_r and val_l are global
{
    //This needed to be completely changed. We need sequential actions, a case statement is too basic (unforetuneately)
    led= 0;
    switch(val_r) {
        case 6: //Normal Mode
            //  blue.printf("0110\n");
            if (!forwards){ //if you're not already going forwards
                set_direction(0x11, 0.3,0.3);
                forwards = 1;
                }
            break;
        case 7: // 0111                             Either misaligned or getting ready for a turn
            r_turn_spot = 1;
            //No need to change direction, if misaligned, it will soon go to a double line alignment protocol
            //if the next state is 0000; then we were right and there is a right turn.
            break;
        case 0: //lost or turn
            //  blue.printf("0000\n");
            if ((r_turn_spot & l_turn_spot) & !spin) {        //if we're expecting a junction and we're not spinning
                set_direction(0x10, 0.3,0.3);       //spin right because there's only one junction
                spin = 1;
                //no stop conditions yet

            } else if (r_turn_spot & !spin) {               //if we're expecting a right turn
                set_direction(0x10, 0.3,0.3);       //spin right
                spin = 1;
                //no stop conditions yet

            } else if(val_l == 6) {                 //0110 0000 could mean a right 180 turn
                //keep going forward,               no need to change dir
                r_180_spot = 1;                     //raise 180 flag in prep for next time

            } else if(r_180_spot) {
                //code for 180???
                blue.printf("I want to 180 Right, but i don't know how yet");
                set_direction(0x00, 0,0);

            } else {                                //probably lost
                set_direction(0x00, 0,0);
            }
            break;


            //***************************IF THERE IS A SINGLE BIT ONLY; WE ARE MISALIGNED*********************************
        case 1:
            //  blue.printf("0001\n");
            set_direction(0x11, 0.3,0.1);
            break;
        case 2:
            // blue.printf("0010\n");
            set_direction(0x11, 0.3,0.2);
            break;
        case 4:
            // blue.printf("0100\n");
            set_direction(0x11, 0.2,0.3);
            break;
        case 8:
            //blue.printf("1000\n");
            set_direction(0x11, 0.1,0.3);
            break;
            //******************************END OF SINGLE BIT ALIGNMENT********************************************

            //*************IF TWO BITS ARE ON, AND NOT IN THE MIDDLE, WE'RE PROBABLY VERY MISALIGNED*******************
        case 12:
            //   blue.printf("1100\n");
            set_direction(0x11, 0.2,0.3);
            break;
        case 3:
            // blue.printf("0011\n");
            set_direction(0x11, 0.3,0.2);
            break;
            //********************************END OF DOUBLE BIT ALIGNMENT********************************************



        default:
            //blue.printf("%i\n", val);
            set_direction(0x00, 0,0);
    }
//***************************************LEFT SHOULD BE PRETTY MUCH MIRRORED***********************************************************
    switch(val_l) {
        case 6: //Normal Mode
            //  blue.printf("0110\n");
            set_direction(0x11, 0.3,0.3);
            break;
        case 7: // 0111                             Either misaligned or getting ready for a turn
            l_turn_spot = 1;
            //No need to change direction, if misaligned, it will soon go to a double line alignment protocol
            //if the next state is 0000; then we were right and there is a left turn.
            break;
        case 0: //lost or turn
            //  blue.printf("0000\n");
            if (r_turn_spot & l_turn_spot) {        //if we're expecting a junction
                set_direction(0x10, 0.3,0.3);       //spin right because there's only one junction
                //no stop conditions yet

            } else if (l_turn_spot) {               //if we're expecting a left turn
                set_direction(0x01, 0.3,0.3);       //spin left
                //no stop conditions yet

            } else if(val_r == 6) {                 //0000 0110 could mean a left 180 turn
                //keep going forward,               no need to change dir
                l_180_spot = 1;                     //raise 180 flag in prep for next time

            } else if(l_180_spot) {
                //code for 180???
                blue.printf("I want to 180 left, but i don't know how yet");
                set_direction(0x00, 0,0);

            } else {                                //probably lost
                set_direction(0x00, 0,0);
            }
            break;

            //***************************IF THERE IS A SINGLE BIT ONLY; WE ARE MISALIGNED*********************************
        case 1:
            //  blue.printf("0001\n");
            set_direction(0x11, 0.1,0.3);
            break;
        case 2:
            // blue.printf("0010\n");
            set_direction(0x11, 0.2,0.3);
            break;
        case 4:
            // blue.printf("0100\n");
            set_direction(0x11, 0.3,0.2);
            break;
        case 8:
            //blue.printf("1000\n");
            set_direction(0x11, 0.3,0.1);
            break;
            //******************************END OF SINGLE BIT ALIGNMENT********************************************

            //*************IF TWO BITS ARE ON, AND NOT IN THE MIDDLE, WE'RE PROBABLY VERY MISALIGNED*******************
        case 12:
            //   blue.printf("1100\n");
            set_direction(0x11, 0.3,0.2);
            break;
        case 3:
            // blue.printf("0011\n");
            set_direction(0x11, 0.2,0.3);
            break;
            //********************************END OF DOUBLE BIT ALIGNMENT********************************************



        default:
            //blue.printf("%i\n", val);
            set_direction(0x00, 0,0);
    }

}
void sensor_read()
{

    int x = 0;
    int sens_r[4] = {0};
    int sens_l[4] = {0};

    for (x = 0; x < 8; x++) {
        switch(x) {
                //0 is the robot's rightest sensor and 7 is the robot's leftmost sensor
                //Addresses are inputs to multiplexer, can ignore them mainly
            case 0:
                sensor = 0x3;
                break;
            case 1:
                sensor = 0x0;
                break;
            case 2:
                sensor = 0x1;
                break;
            case 3:
                sensor = 0x2;
                break;
            case 4:
                sensor = 0x5;
                break;
            case 5:
                sensor = 0x7;
                break;
            case 6:
                sensor = 0x6;
                break;
            case 7:
                sensor = 0x4;
                break;
        }

        if(x<4)sens_r[x] = input;
        else sens_l[x%4] = input;//array of sensor output values for left and right
        //blue = !blue;



    }
    for(x = 7; x >= 0; x--) {
        if(x>3) {
            val_l = val_l << 1;
            val_l = val_l + sens_l[x%4];
        } else {
            val_r = val_r << 1;
            val_r = val_r + sens_r[x];
        }
    }
    //blue.printf("%i\n",val);

}
int main()
{
    level = 0.58; //Analogout Level for black line into comparator
//Set PWM frequency to 1000Hz
    motor_l.period( 1.0f / (float) PWM_FREQ);
    motor_r.period( 1.0f / (float) PWM_FREQ);
//Initialise direction to nothing.
    motor_rf=0;
    motor_rb=0;
    motor_lf=0;
    motor_lb=0;
    //Now Global*****
    //int val =0;
    //int old_val=0;*********

    while(1) { //PRINTS A STRING 0110    0110 FOR LEFT AND RIGHT RESPECTIVELY
        old_val_r = val_r;
        old_val_l = val_l;
        sensor_read();
        if((!(old_val_l == val_l)) | (!(old_val_r == val_r))) {
            string binary_r = "";
            string binary_l = "";
            int mask = 1;
            for(int i = 0; i < 4; i++) {
                if(!(mask&val_r))             binary_r = "1"+binary_r;  //(mask&val) >= 1
                else                        binary_r = "0"+binary_r;
                if(!(mask&val_l))             binary_l = "1"+binary_l;  //(mask&val) >= 1
                else                        binary_l = "0"+binary_l;
                mask = mask << 1;
            }
            blue.printf("%s\t%s\n", binary_l, binary_r);
            wait(0.2);
            motor_result();
        }

        // if(!(old_val == val)) {
        //     motor_result(val);
        // }
    }
    // while(1) {
    //Working
    /*
            led = !led;
            wait(4);
            blue.printf("Forward, full\n");
            set_direction(0x11, 1.0,1.0);//forward full speed
            wait(4);
            blue.printf("Forward, left half\n");
            led = !led;
            set_direction(0x11, 0.5 ,0); //forward left half
            wait(4);
            blue.printf("Forward, right half\n");
            led = !led;
            set_direction(0x11, 0 ,0.5); //forward right half
            wait(4);
            led = !led;
            blue.printf("stop\n");
            set_direction(0x00, 0 ,0); //stop
            wait(4);
            led = !led;
            blue.printf("Back, full\n");
            set_direction(0x00, 1.0 ,1.0); //backward full speed
            wait(4);
            led = !led;
            blue.printf("Back, left\n");
            set_direction(0x00, 0.5 ,0);
            wait(4);
            blue.printf("Back, right\n");
            led = !led;
            set_direction(0x00, 0 ,0.5);

    */

    //Sensor Code + Motor Test
    //int values = 0;
    // values = sensor_read();
    // change_direction(values);

    //}
}