File content as of revision 0:d1ef26d38f28:

#include "mbed.h"

#define wheelc 0.1759292
#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);
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 = 0.100f; //100%
float dutyB = 0.100f; //100%

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

//Completed Loop
int loop=0;

int turn();
void ResetDistanceTimeSpeed()
{
    distanceA=0;
    distanceB=0;
    speedA=0;
    speedB=0;
    pretimerA=0;
    pretimerB=0;
    afttimerA=0;
    afttimerB=0;
    timer.reset();
    timer.start();
}

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

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

    //reset distance
    ResetDistanceTimeSpeed();

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


    //wait for run to complete
    while (((distanceA+distanceB)/2) < distance) {

        if (speedA<speed) {

            dutyA += (float)0.0051;
            PWMA.write(dutyA);
        }
        if( speedA>speed) {
            dutyA -= (float)0.0051;
            PWMA.write(dutyA);
        }

        if (speedB<speed) {

            dutyB += (float)0.0051;
            PWMB.write(dutyB);
        }
        if( speedB>speed) {
            dutyB -= (float)0.0051;
            PWMB.write(dutyB);
        }
        wait(0.05);
    }
    return 1;
}

int turn(float degrees, float speed, 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;

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

    // Test for Loop Completion to Enter Victory Spin
    if(direction==REVERSE) {
        DIRA = REVERSE;
        DIRB = FORWARD;
        PWMB.write(0.0f);          //Set duty cycle (%)
    }



    // Reset Distance Travelled
    ResetDistanceTimeSpeed();
    // Wait for Turn to Complete
    while (distanceA < distance) {
        if (speedA<speed) {

            dutyA += (float)0.0051;
            PWMA.write(dutyA);
        }
        if( speedA>speed) {
            dutyA -= (float)0.0051;
            PWMA.write(dutyA);
        }

        if(direction==REVERSE) {
            if (speedB<(speed/2)) {

                dutyB += (float)0.0051;
                PWMB.write(dutyB);
            }
            if( speedB>(speed/2)) {
                dutyB -= (float)0.0051;
                PWMB.write(dutyB);
            }
        }
        wait(0.1);
    }
    return 1;
}



void set_distanceA()
{
    float time = 0;
    float interupttime = timer.read_us();


    if(interupttime-lastinterupttimeA > 5) {

        afttimerA = timer.read();                      //set latest timer to equal timer
        distanceA += (wheelc/6);                //set distance travelled for this instruction i.e forward/turn etc
        time = afttimerA - pretimerA;             //work out time taken for last 6th of a rotation
        speedA = 0.166667f/(time);              //distance/time = average speed for last 6th rotation
        pretimerA = afttimerA;                  //update pretimer for next calculation of time
        terminal.printf("speedA %f : time %f\n\r", speedA, time);
    }
    lastinterupttimeA = interupttime;
}

void set_distanceB()
{
    float time = 0;
    float interupttime = timer.read_us();

    if(interupttime-lastinterupttimeB > 5) {
        afttimerB = timer.read();
        distanceB += (wheelc/6);
        time = afttimerB - pretimerB;
        speedB = 0.166667f/time;
        pretimerB = afttimerB;
        terminal.printf("speedB %f : time %f\n\r", speedB, time);
    }
    lastinterupttimeB = interupttime;
}

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,10);
                    //terminal.printf("turn\n\r");
                    turn(90,10,0);
                    //terminal.printf("forward\n\r");
                    forward(7,10);
                    //terminal.printf("turn\n\r");
                    turn(90,10,0);
                }

                stopmotors();

                wait(0.5);

                //victory spin
                turn(180,40,1);

                stopmotors();
                break;
            }
        }
    }

}