File content as of revision 13:c22e150048ae:

#include "mbed.h"

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

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

//Hall Effect Sensors
InterruptIn hall_l (PTA1); //Hall Effect Sensor Output RobotLeft
InterruptIn hall_r (PTA2); //Hall Effect Sensor Output RobotRight
float time1_l, time2_l, time3_l, time1_r, time2_r, time3_r;
//Bluetooth Module to debug
Serial blue(PTC4,PTC3);

void set_direction( int direction, float duty_l, float duty_r)
{
    desired_duty_r = duty_r; //USED FOR PD_CONTROL()
    desired_duty_l = duty_l;

    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;
        }
    }
}

//HALL EFFECT INITIALISATIONS
Timer h_r, h_l;
float hall_rt1, hall_lt1, hall_rt2, hall_lt2 = 0; //hall right time & left time
float hall_rdt, hall_ldt;
int h_f_r, h_f_l; //hallflagright and left
void hallr() //this just reading the various times.
{
    if (h_f_r==2) {
        h_r.start();
        hall_rt1=0;
        hall_rt2 =0;
        blue.printf("Timer R Starts\n");
        h_f_r =1;
    } else if(h_f_r == 1) {
        hall_rt1 = h_r.read();
        //blue.printf("rt1 : %f\n", hall_rt1);
        h_f_r = 0;
        hall_rdt = hall_rt1-hall_rt2;
    } else if (h_f_r == 0) {
        hall_rt2 = h_r.read();
        //blue.printf("rt2 : %f\n", hall_rt2);

        h_f_r = 1;
        hall_rdt = hall_rt2-hall_rt1;
    }
    blue.printf("Hall_R dt : %f\n", hall_rdt);

}
void halll()
{
    blue.printf("Left been triggered\n");
    if (h_f_l==2) {
        h_l.start();
        hall_lt1=0;
        hall_lt2 =0;
        blue.printf("Timer L starts\n");
        h_f_l = 1;
    } else if(h_f_l == 1) {
        hall_lt1 = h_l.read();
        //blue.printf("lt1 : %f\n", hall_lt1);

        h_f_l = 0;
        hall_ldt = hall_lt1-hall_lt2;
    } else if (h_f_l == 0) {
        hall_lt2 = h_l.read();
        //    blue.printf("lt1 : %f\n", hall_lt1);

        h_f_l = 1;
        hall_ldt = hall_lt2-hall_lt1;
    }
    blue.printf("Hall_L dt : %f\n", hall_ldt);

}
void PD_control ()
{
    //PSUEDO
    a_ratio = hall_ldt/hall_rdt; //actual_ratio
    if(a_ratio < (desired_ratio - threshold)) { //if right is too big (bottom heavy denomotator)
        if (desired_duty_l < (duty_l + up_limit)) { //If you've not already set the duty too high.  up_limit to be decided from testing
            duty_l =+ inc_value; //Make Left go faster.  Incremental value requires testing.
            motor_l.write( duty_l);//actually changing the duty here
        } else {
            if (desired_duty_r > (duty_r + low_limit)) { // if right isn't too small already
                duty_r =- inc_value; //make right go slower
                motor_r.write(duty_r);
            }
            //What to do when right is low limt AND left is up limit AND we've still not met ratio??
        }
        else {
            if(a_ratio > (desired_ratio + threshold)) {
                if (desired_duty_r < (duty_r + up_limit)) { //If you've not already set the duty too high.  up_limit to be decided from testing
                    duty_r =+ inc_value; //Make Left go faster.  Incremental value requires testing.
                    motor_r.write( duty_r);//actually changing the duty here
                } else {
                    if (desired_duty_l > (duty_l + low_limit)) { // if right isn't too small already
                        duty_l =- inc_value; //make right go slower
                        motor_l.write(duty_l);
                    }
                    //What to do when left is low limt AND right is up limit AND we've still not met ratio??
                }
            }

        }



    }
    int main() {
        h_f_r = 2;
        h_f_l = 2;
        //attaching hall_r and hall_r so that it calls hall() on a rising and falling edge
        hall_r.rise(&hallr);

        hall_l.rise(&halll);
//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;
        wait(1);
        set_direction(0x11, 0.2,0.2);
        while(1) {
        }
    }