File content as of revision 32:b7a08eb0408d:

//Enhancement 3//

#include "mbed.h"
//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
DigitalIn HEA1(PB_2);
DigitalIn HEA2(PB_1);
DigitalIn 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 timerA;
Timer timerB;
//Enumerated types
enum DIRECTION   {FORWARD=0, REVERSE};
enum PULSE       {NOPULSE=0, PULSE};
enum SWITCHSTATE {PRESSED=0, RELEASED};
//Duty cycles
float dutyA, dutyB = 0.0f;
//Speed Calculations
float fA, fB = 0.0f;
float speedA, speedB = 0.0f;
float TA[2];
float TB[2];
float TAA, TBB = 0.0f;
//Distance Calculations
float pulseA, pulseB = 0.0f;
float travA, travB = 0.0f;

void timeA() // this funtion calulates the rotation speed and distance of wheel A
{
    static int n=0;            //Number of pulse sets
    static int HallState = 0;   //the hall effect current state
    if (n==0) {
        //Reset timer and Start
        timerA.reset(); //Resets timerA
        timerA.start(); //Starts timerA
        TA[0] = timerA.read_us(); //Reads timer from the beginning of the first pulse
    }
    switch(HallState) {
        case 0:
            if(HEA1 == PULSE) {
                HallState = 1; //Change state
            }
            break;
        case 1:
            if(HEA1 == NOPULSE) {
                HallState = 0; //Change state
                n++; //Add 1 to the number of pulses counted (resets after 4 pulsees)
                pulseA++; //Add 1 to the number of pulses counted for the duration
                travA = ((176/20.8)/3)*pulseA; //Gives the distance travelled by wheel A in mm
            }
            break;
    }
    if (n < 4) return;
    TA[1] = timerA.read_us(); //Reads timer at the end of one shaft rotation
    timerA.stop();
    TAA = (TA[1]-TA[0]); //Time taken to do one shaft rotation

    fA = 1.0f/ (TAA *(float)1.0E-6); //Frequency of shaft rotation
    speedA = fA/20.8; //wheel RPS
    //Reset count
    n=0;
}
void timeB() //This funtion calulates the rotation speed and distance of wheel A
{
    static int nB=0;            //Number of pulse sets
    static int HallStateB = 0;   //The hall effect current state
    if (nB==0) {
        //Reset timer and Start
        timerB.reset(); //Resets timerB
        timerB.start(); //Starts timerB
        TB[0] = timerB.read_us(); //Reads time from the beginning of the first pulse
    }
    switch(HallStateB) {
        case 0:
            if(HEB1 == PULSE) {
                HallStateB = 1; //Change state
            }
            break;
        case 1:
            if(HEB1 == NOPULSE) {
                HallStateB = 0; //Change state
                nB++; //Add 1 to the number of pulses counted (resets after 4 pulsees)
                pulseB++; //Add 1 to the number of pulses counted for the duration
                travB = ((176/20.8)/3)*pulseB; //Gives the distance travelled by wheel B in mm
            }
            break;
    }
    if (nB < 4) return;
    TB[1] = timerB.read_us(); //Reads timer at the end of one shaft rotation
    timerB.stop();
    TBB = (TB[1]-TB[0]); //Time taken to do one shaft rotation
    fB = 1.0f/ (TBB *(float)1.0E-6); //Frequency of shaft rotation
    speedB = fB/20.8;  //Wheel RPS
    //Reset count
    nB=0;
}
void oneRPS()
{
    float deltaA = 1.0f-speedA;      //Error
    float deltaB = 1.0f-speedB;      //Error
    dutyA = dutyA + deltaA*0.01f;    //Increase duty in proportion to the error
    dutyB = dutyB + deltaB*0.01f;    //Increase duty in proportion to the error
    //Lock the max and min values of duty
    dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
    dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
    dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
    dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
    //Update duty cycle to new value
    PWMA.write(dutyA);
    PWMB.write(dutyB);
}
void cornerRPS()
{
    float deltaA = 1.2f-speedA;       //Error
    float deltaB = 0.55f-speedB;      //Error
    dutyA = dutyA + deltaA*0.001f;    //Increase duty in proportion to the error
    dutyB = dutyB + deltaB*0.001f;    //Increase duty in proportion to the error
    //Lock the maximum and minimum values of duty
    dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
    dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
    dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
    dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
    //Update duty cycle to new value
    PWMA.write(dutyA);
    PWMB.write(dutyB);
}
void reset()  //This fuction resets the distnace travelled and pulses to 0 allowing fo a new distance measurement
{
    travA = 0;
    travB = 0;
    pulseA = 0;
    pulseB = 0;
}
void straight()  //This sets the wheel speed to roughly 1rps so the program doesnt have make major adjustments from the beginning
{
    dutyA = 0.463f;
    dutyB = 0.457f;
    PWMA.write(dutyA);          //Set duty cycle (%)
    PWMB.write(dutyB);
}
void turn() //This sets the wheel speed to roughly what is needed to make the turn so that the program doesnt have to make any major adjustments
{
    dutyA = 0.556f;
    dutyB = 0.3f;
    PWMA.write(dutyA);          //Set duty cycle (%)
    PWMB.write(dutyB);
}
int main()
{
    //Configure the terminal to high speed
    terminal.baud(115200);
    //Set initial motor direction
    DIRA = FORWARD;
    DIRB = FORWARD;
    //Set motor period to 100Hz
    PWMA.period_ms(10);
    PWMB.period_ms(10);
    //Set initial motor speed to stop
    PWMA.write(0.0f);           //0% duty cycle
    PWMB.write(0.0f);           //0% duty cycle
    //Wait for USER button (blue pull-down switch) to start
    terminal.puts("Press USER button to start");
    while (SW1 == RELEASED);
    //Wait - so buggy doesn't move immediately
    wait(1.0);
    straight();   //starts the straight function
    oneRPS();     //starts oneRPS function
    timeA();      //TimeA function to get wheel speed and distance
    timeB();      //TimeB function to get wheel speed and distance
    terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
    while(travA <= 1250) {      //starts moving from 0mm to 1250mm (distance of wheel travel)
        timeA();    //Starts time A
        timeB();    //starts time B
        oneRPS();   //starts oneRPS function
        terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
    }
    reset();       //activates reset function
    turn();        //starts turn function
    while(travA <= 597) {   //moves from 0mm to 597mm (distance of outside wheel travel)
        timeA();   //starts time A
        timeB();   //starts time B
        cornerRPS();  //starts CornerRPS function
        terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
    }
    reset();        //activates reset function
    straight();     //starts the straight function
    while(travA <= 1457) { //starts moving from 0mm to 1457mm (distance of wheel travel)
        timeB();    //starts time A
        timeA();    //starts time B
        oneRPS();   //starts oneRPS function
        terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
    }
    reset();    //activates reset function
    turn();        //starts turn function
    while(travA <= 453.8) { //starts moving from 0mm to 453.8mm (distance of wheel travel)
        timeA();        //starts time A
        timeB();        //starts time B
        cornerRPS(); ///starts CornerRPS function
        terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
    }
    reset();        //activates reset function
    straight();     //starts the straight function
    while(travA <= 750) {     //starts moving from 0mm to 750mm (distance of wheel travel)
        timeB();        //starts time A
        timeA();        //starts time B
        oneRPS();       //starts oneRPS function
        terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
    }
    reset();        //activates reset function
    turn();         //starts turn function
    while(travA <= 350.3) {   //starts moving from 0mm to 349mm (distance of wheel travel)
        timeA();        //starts time A
        timeB();        //starts time B
        cornerRPS(); //starts CornerRPS function
        terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
    }
    PWMA.write(0.0f);       //tells wheel A to stop
    PWMB.write(0.0f);       //tells wheel B to stop
}