File content as of revision 19:14b4b90bdf46:

#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);
//********************************************************
float desired_duty_r; //USED FOR PD_CONTROL() SET IN set_dir()
float desired_duty_l;

float   pres_duty_r, pres_duty_l; 
float   a_ratio;
float   desired_ratio; 
float   perc_r;
float   perc_l;
float   perc_tot;
float   threshold; 
float   limit;
float   inc_value;

//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 init_PD ()
{
    //all these values have to be global; this function gets called each time there's a new direction/desired speed
    a_ratio = hall_ldt/hall_rdt; //actual_ratio
    desired_ratio = desired_duty_l/desired_duty_r; //desired_duty comes from set_direction()
    threshold = desired_ratio*0.1*((1/desired_duty_r) + (1/desired_duty_l)); //dynamic threshold that will change according to duties
    limit = 0.05*((desired_duty_r/desired_duty_l) + (desired_duty_l/desired_duty_r) + 1); //5% of (ratio of duties and inverse ratio of duties + offset(1) )
    inc_value = 0.25*limit; 
}
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;
        }
    }
    init_PD ();
}



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

    if(a_ratio < (desired_ratio - threshold)) { //if right is too big (bottom heavy denomotator)
        if (pres_duty_l < (desired_duty_l + limit)) { //If you've not already set the duty too high.  up_limit to be decided from testing
            pres_duty_l =+ inc_value; //Make Left go faster.  Incremental value requires testing.
            motor_l.write( pres_duty_l);//actually changing the duty here
        } else {
            if (pres_duty_r > (desired_duty_r - limit)) { // if right isn't too small already
                pres_duty_r =- inc_value; //make right go slower
                motor_r.write(pres_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 (pres_duty_r < (desired_duty_r + limit)) { //If you've not already set the duty too high.  up_limit to be decided from testing
                pres_duty_r =+ inc_value; //Make Left go faster.  Incremental value requires testing.
                motor_r.write( pres_duty_r);//actually changing the duty here
            } else {
                if (pres_duty_l > (desired_duty_l + limit)) { // if right isn't too small already
                    pres_duty_l =- inc_value; //make right go slower
                    motor_l.write(pres_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) {
    }
}