File content as of revision 17:54da4359134f:

#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);



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(int val)
{

    led= 0;
    switch(val) {
        case 0: //lost
            //  blue.printf("0000\n");
            set_direction(0x00, 0,0);
            break;
        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 6:
            //  blue.printf("0110\n");
            set_direction(0x11, 0.3,0.3);
            break;
        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;
        case 8:
            //blue.printf("1000\n");
            set_direction(0x11, 0.1,0.3);
            break;
        default:
            //blue.printf("%i\n", val);
            set_direction(0x00, 0,0);
    }


}
int sensor_read()
{
    int val=0;
    int x = 0;
    int sens[4] = {0};
    for (x = 0; x < 4; x++) {
        switch(x) {
            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;
        }

        sens[x] = input;
        //blue = !blue;



    }
    for(x = 3; x >= 0; x--) {
        val = val << 1;
        val = val + sens[x];
    }
    //blue.printf("%i\n",val);

    return 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;
    int val =0;
    int old_val=0;
    while(1) {
        old_val = val;
        val = sensor_read();
        if(!(old_val == val)) {
            string binary = "";
            int mask = 1;
            for(int i = 0; i < 4; i++) {
                if((mask&val) >= 1)         binary = "1"+binary;
                else                        binary = "0"+binary;
                mask = mask << 1;
            }
            blue.printf("%s\n", binary);
            wait(0.2);
            motor_result(val);
        }

        // 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);

    //}
}