File content as of revision 18:11937e78239c:

/*
Our version
*/
#include "mbed.h"
 
//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
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;
Timer timer1;
 
//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%
//Array of sensor data
int tA1[2];
int tA2[2];
int tB1[2];
int tB2[2];
float dis;
float trav =0;

void HallA()
        {
            //Reset timer and Start
        timerA.reset();
        timerA.start();
        bool all = true;
        //**********************
        //TIME THE FULL SEQUENCE
        //**********************
        int HallStateA = 0;
        while(all)
        {
            switch(HallStateA)
            {
                case 0:
                if(HEA1 == NOPULSE){
                    HallStateA = 1;
                    tA1[0] = timerA.read_us();
                }break;
                case 1:
                if(HEA2 == NOPULSE){
                    HallStateA = 2;
                    tA2[0] = timerA.read_us();
                }break;
                case 2:
                if(HEA1 == PULSE){
                    HallStateA = 3;
                    tA1[1] = timerA.read_us();
                }break;
                case 3:
                if(HEA2 == PULSE){
                    HallStateA = 0;
                    all = false;
                    tA2[1] = timerA.read_us();
                }break;
            }
        }
 
 
        terminal.printf("tA1(0) = %d\n", tA1[0]);
        terminal.printf("tA1(1) = %d\n", tA1[1]);
        terminal.printf("tA2(0) = %d\n", tA2[0]);
        terminal.printf("tA2(1) = %d\n", tA2[1]);
 
        //Calculate the frequency of rotation
        float TA1 = 2.0f * (tA1[1]-tA1[0]);
        float TA2 = 2.0f * (tA2[1]-tA2[0]);
        float TA = (TA1 + TA2) * 0.5f;

        dis = timer1.read_us();
        float mm = ((TA*3)*20.8)/175.9;
        trav = dis/mm;
        float fA = 1.0f/ (TA *(float)3.0E-6);
        terminal.printf("Average A2 Shaft: %6.2fHz \t Wheel: %6.2f \t trav: %6.2f\n", fA, fA/20.8f, trav);
        }

void HallB()
        {
            //Reset timer and Start
        timerB.reset();
        timerB.start();
        bool allB = true;
        //**********************
        //TIME THE FULL SEQUENCE
        //**********************
        int HallStateB = 0;
        while(allB)
        {
            switch(HallStateB)
            {
                case 0:
                if(HEB1 == NOPULSE){
                    HallStateB = 1;
                    tB1[0] = timerB.read_us();
                }break;
                case 1:
                if(HEB2 == NOPULSE){
                    HallStateB = 2;
                    tB2[0] = timerB.read_us();
                }break;
                case 2:
                if(HEB1 == PULSE){
                    HallStateB = 3;
                    tB1[1] = timerB.read_us();
                }break;
                case 3:
                if(HEB2 == PULSE){
                    HallStateB = 0;
                    allB = false;
                    tB2[1] = timerB.read_us();
                }break;
            }
        }
 
 
        terminal.printf("tB1(0) = %d\n", tB1[0]);
        terminal.printf("tB1(1) = %d\n", tB1[1]);
        terminal.printf("tB2(0) = %d\n", tB2[0]);
        terminal.printf("tB2(1) = %d\n", tB2[1]);
 
        //Calculate the frequency of rotation
        float TB1 = 2.0f * (tB1[1]-tB1[0]);
        float TB2 = 2.0f * (tB2[1]-tB2[0]);
        float TB = (TB1 + TB2) * 0.5f;
        float fB = 1.0f/ (TB *(float)3.0E-6);
        }
         
void reset()
{
    timer1.reset();
    HallA();
}
         
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");
    led = 0;
    while (SW1 == RELEASED);
    led = 1;
 
    //Set initial motor speed to stop
    PWMA.write(0.0f);          //Set duty cycle (%)
    PWMB.write(0.0f);          //Set duty cycle (%)
 
    //Wait - give time to start running
    wait(1.0);
    timer1.reset();
    timer1.start();
    //Main polling loop
    while(1)
    {
        while(trav <= 1250)  
            {  
               PWMA.write(dutyA);          //Set duty cycle y  
               PWMB.write(dutyB);
               HallA();
               HallB();
            } 
        reset();
        while(trav <= 330)  
            {  
               PWMA.write(dutyA);           
               PWMB.write(0.0f);
               HallA();
               HallB();
            }
        reset();
        while(trav <= 1457)  
            {  
               PWMA.write(dutyA);           
               PWMB.write(dutyB);
               HallA();
               HallB();
            }
        reset();
        while(trav <= 268)  
            {  
               PWMA.write(dutyA);           
               PWMB.write(0.0f);
               HallA();
               HallB();
            }
        reset();
        while(trav <= 750)  
            {  
               PWMA.write(dutyA);           
               PWMB.write(dutyB);
               HallA();
               HallB();
            }
        reset();
        while(trav <= 200)  
            {  
               PWMA.write(dutyA);           
               PWMB.write(0.0f);
               HallA();
               HallB();
            }  
        timerA.stop();   
        timerB.stop();
        break;
    }
    PWMA.write(0.0f);    
    PWMB.write(0.0f);
}