File content as of revision 0:7118ab174b9c:

#include "mbed.h"

#define wheelc 0.1759292
#define pi 3.141592654
#define arcl 1.979203372
//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);
DigitalIn HEA2(PB_1);
InterruptIn HEB1(PB_15);
DigitalIn 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 = 1.0f; //100%
float dutyB = 1.0f; //100%

//distance measurement
float distanceA ;
float distanceB ;
float speedA ;
float speedB ;
float pretimerA;
float afttimerA;
float pretimerB;
float afttimerB;
float wheel_spacing = 0.128;

//Completed Loop
int loop=0;


void ResetDistanceTimeSpeed()
{
    distanceA=0;
    distanceB=0;
    speedA=0;
    speedB=0;
    pretimerA=0;
    pretimerB=0;
    afttimerA=0;
    afttimerB=0;
}

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

int forward(float distance, float speed, float duty)
{
    //add forward to input with switch for scalability
    int i;
    // Set motor direction forward
    DIRA = FORWARD;
    DIRB = FORWARD;

    // Set motor speed to input speed
    PWMA.write(duty);          //Set duty cycle (%)
    PWMB.write(duty);          //Set duty cycle (%)

    //reset distance
    ResetDistanceTimeSpeed();

    //wait for run to complete
    while (((distanceA+distanceB)/2) < distance) {
        switch {
        case(speedA<speed) {
                    duty += (0.1*duty);
                    PWMA.write(duty);
                }

            case(speedA>speed) {
                    duty+=(0.1*duty);
                    PWMA.write(duty);
                }
                return 1;
        }

        int turn(float distance, float duty, int loop) {
            //Set initial motor direction
            DIRA = FORWARD;
            DIRB = FORWARD;

            //test for loop completion
            if(loop==1) {
                DIRA = REVERSE;
                DIRB = REVERSE;
            }
            /*
            float C1 = (pi * 2 * distance_outside_wheel_turn_point)*(degrees/360);
            float C2 = (pi * 2 * (distance_outside_wheel_turn_point-wheel_spacing))*(degrees/360);
            */
            //Set motor speed
            PWMA.write(duty);          //Set duty cycle (%)
            PWMB.write(0.0f);          //Set duty cycle (%)
            //reset distance time and speed
            resetDistanceTimeSpeed();
            //wait for turn to complete
            while (distanceA < distance) {
            }
            return 1;
        }

        void set_distanceA() {
            float time = 0;
            
            afttimerA = timer;                      //set latest timer to equal timer
            distanceA += (wheelc/6);                  //set distance travelled for this instruction i.e forward/turn etc
            terminal.printf("A =%f\n\r",distanceA); // print for fault finding (delete once working)
            time = afttimer - pretimer;             //
            speedA = (wheelc/6)/time);
            pretimerA = afttimerA;
        }

        void set_distanceB() {
            float time = 0;
            
            afttimerB = timer;
            distanceB += (wheelc/6);
            terminal.printf("A =%f\n\r",distanceB);
            time = afttimer - pretimer;
            speedB = (wheelc/6)/time);
            pretimerB = afttimerB;
        }


        int main() {
            //Configure the terminal to high speed
            terminal.baud(115200);

            HEA1.rise(set_distanceA);
            HEB1.rise(set_distanceB);
            //Set initial motor speed to stop
            `   stopmotors();
            PWMA.period_ms(10);
            PWMB.period_ms(10);
            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(12,50);
                            //terminal.printf("turn\n\r");
                            turn(arcl,50,0);
                            //terminal.printf("forward\n\r");
                            forward(6,50);
                            //terminal.printf("turn\n\r");
                            turn(arcl,50,0);
                        }

                        stopmotors();

                        wait(0.5);

                        //victory spin
                        turn(arcl*4.5,50,1);

                        stopmotors();
                        break;
                    }
                }
            }
        }