File content as of revision 28:b650e7f6c269:

//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 = 0.2f; //100%
float dutyB = 0.2f; //100%
float speedA[3];
float speedB[3];
float fA, fB = 0.0f;
float sumA, sumB = 0.0f;
int durA, durB = 0;
float TA[2];
float TB[2];
float TAA, TBB = 0.0f;
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 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 9 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
    sumA = fA/20.8;
    //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 timerA
        timerB.start(); // starts timerA
        TB[0] = timerB.read_us(); //reads timer from the beginning of 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 9 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
    sumB = fB/20.8;
    //Reset count
    nB=0; 
}
void oneRPS()
{
    float deltaA = 1.0f-sumA;         //Error
    float deltaB = 1.0f-sumB;
    dutyA = dutyA + deltaA*0.01f;    //Increase duty in proportion to the error
    dutyB = dutyB + deltaB*0.01f;    //Increase duty in proportion to the error
    //Clamp the max and min values of duty and 0.0 and 1.0 respectively
    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 correct in the first direction
    PWMA.write(dutyA);
    PWMB.write(dutyB);
}
void cornerRPS()
{
    float deltaA = 1.2f-sumA;         //Error
    float deltaB = 0.55f-sumB;
    dutyA = dutyA + deltaA*0.001f;    //Increase duty in proportion to the error
    dutyB = dutyB + deltaB*0.001f;    //Increase duty in proportion to the error
    //Clamp the max and min values of duty and 0.0 and 1.0 respectively
    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 correct in the first direction
    PWMA.write(dutyA);
    PWMB.write(dutyB);
}
void reset()  // this fuction restes 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 weheel 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.27f;
    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(0.5);
    //Wait - give time to start running
    wait(1.0);
    //Main polling loop

    while(1) {
        dutyA = 0.463f;
        dutyB = 0.457f;
        PWMA.write(dutyA);          //Set duty cycle (%)
        PWMB.write(dutyB);
        oneRPS();
        timeA();
        timeB();
        terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", sumA, sumB, travA, travB);
        while(travA <= 1250) {
            timeA();
            timeB();
            oneRPS();
            terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", sumA, sumB, travA, travB);
        }
        reset();
        turn();
        while(travA <= 597) {
            timeA();
            timeB();
            cornerRPS();
            terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", sumA, sumB, travA, travB);
        }
        reset();
        straight();
        while(travA <= 1457) {
            timeB();
            timeA();
            oneRPS();
            terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", sumA, sumB, travA, travB);
        }
        reset();
        turn();
        while(travA <= 453.8) {
            timeA();
            timeB();
            cornerRPS();
            terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", sumA, sumB, travA, travB);
        }
        reset();
        straight();
        while(travA <= 750) {
            timeB();
            timeA();
            oneRPS();
            terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", sumA, sumB, travA, travB);
        }
        reset();
        turn();
        while(travA <= 349) {
            timeA();
            timeB();
            cornerRPS();
            terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", sumA, sumB, travA, travB);
        }
        break;
    }
    PWMA.write(0.0f);
    PWMB.write(0.0f);

}