File content as of revision 0:735ec5eb58d0:

#include "mbed.h"

#define wheelc 0.1759292 // Circumference of Wheel
#define pi 3.141592654
#define degreel 0.021988888 // Distance Travelled in 1 Degree


// 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 wheel_spacing = 0.128;

// Completed Loop
int loop = 0;


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

void set_distanceA()
{
    // Hall Effect Measuring Distance for Motor A
    distanceA += (wheelc/6);
    terminal.printf("A =%f\n\r",distanceA); //Print to terminal
}

void set_distanceB()
{
    // Hall Effect Measuring Distance for Motor B
    distanceB += (wheelc/6);
    terminal.printf("B =%f\n\r",distanceB); // Print to Terminal Distance Moved so Far
}



int forward(float distance, float duty) // Move Forwards (distance,speed)
{
    // 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 Total Distance Travelled
    distanceA = 0;
    distanceB = 0;
    timer.reset();
    timer.start();

    // Wait for Set Distance to be Reached
    while (((distanceA+distanceB)/2) < distance) {
    } // Using Average Distance for More Accurate Result
    return 1;
}

int turn(float degrees, float duty, int direction) // (Degrees of Turn, Speed, (Anti/Clock)wise)

{
    // Calculate Turn Distance
    float distance=0;
    distance=((float)degreel*degrees);
    //Set Initial Motor Direction
    DIRA = FORWARD;
    DIRB = FORWARD;

    // Test for Loop Completion to Enter Victory Spin
    if(direction==REVERSE) {
        DIRA = REVERSE;
        DIRB = REVERSE;
    }

    // Set Motor Speed for Outside Wheel
    PWMA.write(duty);          //Set duty cycle (%)
    PWMB.write(0.0f);          //Set duty cycle (%)

    // Reset Distance Travelled
    distanceA = 0;
    distanceB = 0;
    timer.reset();
    timer.start();
    // Wait for Turn to Complete
    while (distanceA < distance) {
    }
    return 1;
}

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); // Move Forward 1.2m (extra .2 to avoid cones)
                    turn(90,50,1); // Turn 90°
                    forward(6,50); // Move Forward .6m (extra .1 to avoid cones)
                    turn(90,50,1); // Turn 90°
                }

                stopmotors();
                wait(0.5);

                //Victory Spin
                turn(360,50,0); //Turn 360 degrees in Reverse

                stopmotors();
                break; // End Program, Prevents Looping
            }
        }
    }
}