File content as of revision 1:f4e3365155e1:

#include "mbed.h"


#define wheelc 0.1759292
#define wheel_spacing 0.128 //add this in
#define pi 3.141592654
#define degreel 0.021988888
//Status LED
DigitalOut led(LED1);

//Motor PWM (speed)
PwmOut PWMA(PA_8);
PwmOut PWMB(PB_4);

//Motor Direction
DigitalOut DIRA(PA_9);
DigitalOut DIRB(PB_10);

//Hall-Effect Sensor Inputs
InterruptIn HEA1(PB_2);
InterruptIn HEA2(PB_1);
InterruptIn HEB1(PB_15);
InterruptIn HEB2(PB_14);

//On board switch
DigitalIn SW1(USER_BUTTON);

//Use the serial object so we can use higher speeds
Serial terminal(USBTX, USBRX);

//Timer used for measuring speeds
Timer timer;

//Enumerated types
enum DIRECTION   {FORWARD=0, REVERSE};
enum PULSE       {NOPULSE=0, PULSE};
enum SWITCHSTATE {PRESSED=0, RELEASED};

//Debug GPIO
DigitalOut probe(D10);

//Duty cycles
float dutyA = 0.000f; //100%
float dutyB = 0.000f; //100%

//distance measurement
float distanceA ;
float distanceB ;
float speedA ;
float speedB ;

//for interrupt speed and distance calcs
int tA1[2];
int tA2[2];
int tB1[2];
int tB2[2];
int pulse_orderA = 1;
int pulse_orderB = 1;

//interupt handler
int HEPulse(int let,int num, int order) //tell us which hall effect HEA1= hall effect, (let, A=1, B=2), (num, 1/2), order (index possition to be filled alternates)
{
    float fA1;
    float fA2;
    float fB1;
    float fB2;
    int time;
    time = timer.read_us(); //use to check for interrupt debounce

    if((tA1[0]-time<5)||(tA2[0]-time<5)||(tA1[1]-time<5)||(tA2[1]-time<5)) {
        return 1;
    } // if less than 5us from last interupt assume debouce and ignore
    //find right thing to do for current hall effect
    switch(order) { //check whether to fill index possition 0/1
        case(1) :
            switch(let) { //check hall effect A/B
                case(1):

                    switch(num) { //check hall effect 1/2
                        case (1):
                            tA1[0] = timer.read_us();
                            distanceA += (wheelc/6); //increment distance
                            pulse_orderA = 2; //toggle index to be filled
                            break;

                        case(2) :
                            tA2[0] = timer.read_us();
                            pulse_orderA = 2;
                            break;
                    }
                    break;


                case(2): //check hall effect A/B

                    switch(num) {
                        case (1):
                            tB1[0] = timer.read_us();
                            distanceB += (wheelc/6);
                            pulse_orderB = 2;
                            break;

                        case(2) :
                            tB2[0] = timer.read_us();
                            pulse_orderB = 2;
                            break;
                    }
                    break;

            }
        case(2) :
            switch(let) {
                case(1):

                    switch(num) {
                        case (1):
                            tA1[1] = timer.read_us();
                            distanceA += (wheelc/6);
                            fA1 = 1.0f/(( fabsf(tA1[1]-tA1[0]) ) * (float)3.0E-6);
                            pulse_orderA = 1;
                            break;

                        case(2) :
                            tA2[1] = timer.read_us();
                            fA2 = 1.0f/(( fabsf(tA2[1]-tA2[0]) ) * (float)3.0E-6);
                            pulse_orderA = 1;
                            break;
                    }
                    break;


                case(2):

                    switch(num) {
                        case (1):
                            tB1[1] = timer.read_us();
                            distanceB += (wheelc/6);
                            fB1 = 1.0f/(( fabsf(tB1[1]-tB1[0]) ) * (float)3.0E-6);
                            pulse_orderB = 1;
                            break;

                        case(2) :
                            tB2[1] = timer.read_us();
                            fB2 = 1.0f/((fabsf( tB2[1]-tB2[0]) ) * (float)3.0E-6);
                            pulse_orderB = 1;
                            break;
                    }
                    break;

            }

            float fA = (fA1 + fA2)*0.5f; //calculate average frequency of hall effect A
            float fB = (fB1 + fB2)*0.5f; //calculate average frequency of hall effect B
            speedA = fA/20.8f; //output speeds of motors by frequency/gear box ratio
            speedB = fB/20.8f;
    }
    return 1;
}

void ResetDistanceTimeSpeed() //reset vaviables for new straight or turn
{
    distanceA=0;
    distanceB=0;
    speedA=0;
    speedB=0;
    timer.reset();
    timer.start();
}

void stopmotors() //stop motors dead
{
    PWMA.write(0.0f);       //0% duty cycle
    PWMB.write(0.0f);       //0% duty cycle
}

void HuntSpeeds(float target_speedA, float target_speedB) //alter speeds to desired speed by hunting for corresponding duty (input desired speeds)
{

    float deltaA = target_speedA -speedA; //find difference between actual speed and desired speed
    dutyA = dutyA + deltaA*0.1f; //modify duty dependant upon size of error
    PWMA.write(dutyA); //write new duty to motor

    float deltaB = target_speedB -speedB; //repeat for B motor
    dutyB = dutyB + deltaB*0.1f;
    PWMB.write(dutyB);
}


int forward(float distance, float speed) //forward motion, input distance and speed (rev/s)
{
    // Set motor direction forward
    DIRA = FORWARD;
    DIRB = FORWARD;

    //reset distance
    ResetDistanceTimeSpeed();

    // Set initial motor to be altered by speed code
    PWMA.write(0.1f*speed);          //Set duty cycle (%)
    PWMB.write(0.1f*speed);          //Set duty cycle (%)


    //wait for run to complete byy checking if distance has been covered yet
    while (((distanceA+distanceB)/2) < distance) {
        //maintain desired speed
        HuntSpeeds(speed, speed);
    }
    return 1;
}

int turn(float degrees, float speed, int direction,float turn_radius) // (Degrees of Turn, Speed, (Anti/Clock)wise, turn radius (m))

{
    // Calculate Turn Distance
    float C1 = (pi * 2 * (float)turn_radius)*(degrees/360); // outside arc (piD)/fraction turn
    float C2 = (pi * 2 * ((float)turn_radius-(float)wheel_spacing))*(degrees/360); //inside arc (piD)/fraction turn
    //Set Initial Motor Direction
    DIRA = FORWARD;
    DIRB = FORWARD;

    // Test for direction to Enter Victory Spin
    if(direction==REVERSE) {
        DIRA = REVERSE;
        DIRB = FORWARD;//victory spin opposite wheels
    }
    // Reset Distance Travelled
    ResetDistanceTimeSpeed();

    // Wait for Turn to Complete by checking whether outer wheel completed spin yet
    while (distanceA < C1) {
        if(direction==FORWARD) {
            HuntSpeeds(speed, speed*(float(C2/C1))); //maitain speeds proportional to difference in turn length for each wheel
        }

        if(direction==REVERSE) {
            HuntSpeeds(speed,speed); //set speeds same for reverse spin
        }
    }
    return 1;
}
//interupt handlers
void HEA1I(void)
{
    HEPulse(1,1,pulse_orderA);
}
void HEA2I(void)
{
    HEPulse(1,2,pulse_orderA);
}
void HEB1I(void)
{
    HEPulse(2,1,pulse_orderB);
}
 void HEB2I(void)
{
    HEPulse(2,2,pulse_orderB);
}

int main()
{
    //Configure the terminal to high speed
    terminal.baud(115200);
    // initiate interupts to call interupt code
    HEA1.rise(&HEA1I);
    HEA2.rise(&HEA2I);
    HEB1.rise(&HEB1I);
    HEB2.rise(&HEB2I);
    //Set initial motor speed to stop
    stopmotors();
    //set motor frequency
    PWMA.period_ms(10);
    PWMB.period_ms(10);
    ResetDistanceTimeSpeed();
    while(1) {
    //wait for switch press
    while (SW1 == PRESSED) {
            wait(0.01);
            while(SW1 == RELEASED) {

                //navigate course
                for (int i = 0; i<2; i++) {
                    //forward 1m
                    forward(12,1); //distance, speed in rotations/s
                    //turn 90
                    turn(90,0.5,0,0.2);//degrees, speed (rev/s), direction (0 clockwise),radius of turn)
                    //forward 1/2m
                    forward(7,1);//distance, speed in rotations/s
                    //turn 90
                    turn(90,0.5,0,0.2);//degrees, speed (rev/s), direction (0 clockwise),radius of turn)
                }
                //repeat twice to end up back in starting possition

                //victory spin
                turn(365,0.5,1,0.001);//degrees, speed (rev/s), direction (0 clockwise),radius of turn)

                stopmotors();
                break;
            }
        }
    }

}