File content as of revision 17:a92d96b65cbc:

//Enhancement 2//
#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 timerA1;
Timer timerA2;
Timer timerB1;
Timer timerB2;
Timer timer1;
//Enumerated types
enum DIRECTION   {FORWARD=0, REVERSE};
enum PULSE       {NOPULSE=0, PULSE};
enum SWITCHSTATE {PRESSED=0, RELEASED};
//Duty cycles
float dutyA;
float dutyB;
float dis;
float trav =0;
float speedA, speedB = 1.0f;
float paceA, paceB = 1.0f;
int tmrA1[2];
int tmrA2[2];
int tmrB1[2];
int tmrB2[2];
void time()
{
    terminal.printf("tmrA1(0) = %d\n", tmrA1[0]);
    terminal.printf("tmrA1(1) = %d\n", tmrA1[1]);
    terminal.printf("tmrB1(0) = %d\n", tmrA2[0]);
    terminal.printf("tmrB1(1) = %d\n", tmrA2[1]);
    float TA1 = 2.0f * (tmrA1[1]-tmrA1[0]);
    float TA2 = 2.0f * (tmrA2[1]-tmrA2[0]);
    float TA = (TA1 + TA2) * 0.5f;
    float TB1 = 2.0f * (tmrB1[1]-tmrB1[0]);
    float TB2 = 2.0f * (tmrB2[1]-tmrB2[0]);
    float TB = (TB1 + TB2) * 0.5f;
    dis = timer1.read_us();
    float mm = (((((TA*0.3)*20.8)/175.9)+(((TB*0.3)*20.8)/175.9)))*0.5f;
    trav = dis/mm;
    float fA = 1.0f/ (TA *(float)3.0E-7); // FOR A
    float fB = 1.0f/ (TB *(float)3.0E-7); // FOR B
    terminal.printf("Wheel A: %6.2f \t Wheel B: %6.2f \t Distance travelled: %6.2f\n",fA/20.8f, fB/20.8f, trav);
    speedA = fA/20.8f;
    speedB = fB/20.8f;
}
//Rotational speed towards 1rps
void oneRPS()
{
    time();
    if (speedA <= 1.0f)
    {
        paceA +=0.01f;
        wait_ms(25);
    }
    if (speedA >= 1.0f)
    {
        paceA -=0.01f;
        wait_ms(25);
    }
    if (speedB <= 1.0f)
    {
        paceB +=0.01f;
        wait_ms(25);
    }
    if (speedB >= 1.0f)
    {
        paceB -=0.01f;
        wait_ms(25);
    }
    PWMA.write(paceA);
    PWMB.write(paceA); 
}

int main()
{
    int hallStateA = 0;
    int hallStateB = 0;
    //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 - give time to start running
    wait(1.0);
    PWMA.write(1.0f);           //100% duty cycle
    PWMB.write(1.0f);
    timer1.reset();
    timer1.start();
    while(1)
    {
        int passA1 = HEA1;
        int passA2 = HEA2;
        int passB1 = HEB1;
        int passB2 = HEB2;
        time();
        switch (hallStateA)
        {
            case 0:
                if (passA1 ==1)
                {
                    hallStateA = 1;
                    timerA1.reset();
                    timerA1.start();
                    tmrA1[0] = timerA1.read_us();
                }
                break;
            case 1:
                if (passA2 ==1)
                {
                    hallStateA = 2;
                    timerA2.reset();
                    timerA2.start();
                    tmrA2[0] = timerA2.read_us();
                }
                break;
            case 2:
                if (passA1 ==0)
                {
                    hallStateA = 3;
                    tmrA1[1] = timerA1.read_us();
                    timerA1.stop();
                }
                break;
            case 3:
                if (passA2 ==0)
                {
                    hallStateA = 0;
                    tmrA2[1] = timerA2.read_us();
                    timerA2.stop();
                }
                break;
        }
        
        
        switch (hallStateB)
        {
            case 0:
                if (passB1 ==1)
                {
                    hallStateB = 1;
                    timerB1.reset();
                    timerB1.start();
                    tmrB1[0] = timerB1.read_us();
                }
                break;
            case 1:
                if (passB2 ==1)
                {
                    hallStateB = 2;
                    timerB2.reset();
                    timerB2.start();
                    tmrB2[0] = timerB2.read_us();
                }
                break;
            case 2:
                if (passB1 ==0)
                {
                    hallStateB = 3;
                    tmrB1[1] = timerB1.read_us();
                    timerB1.stop();
                }
                break;
            case 3:
                if (passB2 ==0)
                {
                    hallStateB = 0;
                    tmrB2[1] = timerB2.read_us();
                    timerB2.stop();
                }
                break;
        }
    }
}