File content as of revision 21:bef848ac1894:

#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_f = 0;
int r_turn_f = 0;
int l_180_f = 0;
int r_180_f = 0;
int forward = 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)
{
    blue.printf("Change in direction; %f, %f \n", duty_l, 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;
        }
    }
}
int change =0;

void r_normal()
{
    //  blue.printf("0110\n");
    if (val_l == 0) {
        //check flags
        if( l_180_f  ) {
            set_direction(0x11, 0.2, 0.6); //Needs to test
            spin =1; //new state is spin
            forward = 0;
            //start_timer?

        } else if (l_turn_f) { //
            set_direction(0x01, 0.3,0.3); //CHANGE DUTIES AFTER TESTING
            spin =1 ; //reached the end, nothign else matters
            forward =0;
        }
    } else if(val_l == 7) {
        l_turn_f =1;
        if(!forward) {
            forward =1;
            set_direction(0x11, 0.3,0.3);
        } else if(!forward) {
            forward =1;

            set_direction(0x11, 0.3,0.3);
        }
    }


}
void motor_result()  //val_r and val_l are global
{
    switch (val_r) {
        case 6: //Normal Mode
            r_normal();
            break;
        case 7: // 0111
            r_turn_f =1;                        //Either misaligned or getting ready for a turn
            if (val_l == 14) { //1110 0111
                l_turn_f =1;
            } else if(val_l ==0) { //0000 0111
                if (l_turn_f) { //special case ****************************** WON'T HAPPEN**********************
                    blue.printf("Double Turn?");
                    set_direction(0x01, 0.3,0.3);
                    forward =0;
                    spin =1;
                } else if(l_180_f) {
                    blue.printf("You've got a junction\n");
                } else {
                    l_180_f =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(val_l == 14) {
                //This probably won't happen, but if it does , 180
                r_180_f =1;
                l_turn_f =1;
            } else if(val_l == 0) {
                switch (junction_cnt) {
                    default:
                        set_direction(0x11, 0,0);
                        forward = 0;
                }
            } else if(r_turn_f) {
                set_direction(0x10, 0.3 ,0.3);
                forward = 0;
                spin = 1;
            } else if (r_180_f) {
                set_direction(0x11, 0.6, 0.2);
                forward = 0;
                spin =1;
            } else {
                r_180_f =1;
            }
            break;


        case 2:
            // blue.printf("0010\n");
            r_normal();
            break;
        case 4:
            // blue.printf("0100\n");
            r_normal();
            break;

            //***************************IF THERE IS A SINGLE BIT ONLY; WE ARE very MISALIGNED*********************************
        case 1:
            //  blue.printf("0001\n");
            set_direction(0x11, 0,0);
            forward=0;
            break;

        case 8:
            //blue.printf("1000\n");
            set_direction(0x11, 0,0);
            forward = 0;
            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);
            forward = 0;
            break;
        case 3:
            // blue.printf("0011\n");
            set_direction(0x11, 0.3,0.2);
            forward=0;
            break;
            //********************************END OF DOUBLE BIT ALIGNMENT********************************************

        case 15:
            set_direction(0x11, 0,0);
            forward =0;
            break;

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


/*
       //This needed to be completely changed. We need sequential actions, a case statement is too basic (unforetuneately)
       led= 0;
       switch(val_r) {
           case 4: //Normal mode
               if (!forward) {
                   blue.printf("i've got %i on the right\n", val_r);
                   set_direction(0x11, 0.3,0.3);
                   forward =1;
                   change =1;
                   break;
               }
           case 12:
               blue.printf("i've got %i on the right\n", val_r);

               set_direction(0x11, 0.3, 0.2);
               forward =0;
               break;
               change =1;

           case 8:
               blue.printf("i've got %i on the right\n", val_r);

               set_direction(0x11, 0.2, 0.3);
               forward =0;
               change =1;

               break;
           default:
               if (!forward) {
                   blue.printf("i've got %i on the right\n", val_r);

                   set_direction(0x11, 0.3,0.3);
                   forward =1;
                   break;
               }

       }
       if(!change) {
           switch(val_l) {
               case 2:
                   if (!forward) {
                       blue.printf("i've got %i on the left\n", val_l);

                       set_direction(0x11, 0.3,0.3);
                       forward =1;
                       break;
                   }
               case 3:
                   blue.printf("i've got %i on the left\n", val_l);

                   set_direction(0x11, 0.3, 0.2);
                   forward =0;
                   break;
               case 1:
                   blue.printf("i've got %i on the left\n", val_l);

                   set_direction(0x11, 0.3, 0.2);
                   forward =0;
                   break;



               default:
                   blue.printf("i've got %i on the left\n", val_l);
                   if (!forward) {
                       // blue.printf("i've got %i on the left\n", val_l);

                       set_direction(0x11, 0.3,0.3);
                       forward =1;
                       break;
                   }
           }
       }*/
//********************************************************START******************************************************


//***************************************LEFT SHOULD BE PRETTY MUCH MIRRORED***********************************************************
/*   switch(val_l) {
       case 7: //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_f = 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_f & l_turn_f) {        //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_f) {               //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_f = 1;                     //raise 180 flag in prep for next time

           } else if(l_180_f) {
               //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};
    val_r =0;
    val_l=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("ValL %i \t      ValR %i\n",val_l , val_r);

}
int main()
{
    wait(0.5);
    led = !led;
    blue.printf("Starting Code!!!!!!\n");
    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
        led=!led;
        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("L  %s\t      R  %s\n", binary_l, binary_r);
            motor_result();
        }
        wait(0.2);
        // 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);

    //}
}