File content as of revision 4:4aa46b70064a:

/*  
 Version 5 – Mike's edit 
*/  

#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 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 = 1.0f; //100%  
float dutyB = 1.0f; //100%  
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    {  
    for(float ramp = 0.0f; ramp <= 1.0f ; ramp += 0.1)  
        {  
            PWMA.write(ramp);          //Set duty cycle y  
            PWMB.write(ramp);          //Set duty cycle y  
            wait(1);  
        }  
    PWMA.write(dutyA);          //Set duty cycle hyp  
    PWMB.write(dutyB);          //Set duty cycle hyp  
    wait(0.6);
    PWMB.write(0.2f);          //turn 31deg 
    wait(1.6);  
    PWMA.write(dutyA);          //Set duty cycle hyp  
    PWMB.write(dutyB);          //Set duty cycle hyp  
    wait(4.4);  
    PWMB.write(0.2f);          //turn 59deg 
    wait(1.1);  
    PWMA.write(dutyA);          //Set duty cycle x  
    PWMB.write(dutyB);          //Set duty cycle x  
    wait(2.3);  
    PWMB.write(0.2f);          //turn 45deg 
    wait(0.7);  
    //Array of sensor data  
    int tA1[2];  
    int tA2[2];  
    PWMA.write(0.0f);    
    PWMB.write(0.0f);   
    //Instructions to user  
    terminal.puts("Press USER button repeatedly to adapt duty (to convernge on 1 rotation/s)");  
    //Main polling loop  
    while(1) {  
    //Reset timer and Start  
    timer.reset();  
    timer.start();    

    //*********************************************************************  

    //FIRST TIME - SYNCHRONISE (YOU SHOULD NOT NEED THIS ONCE IT's RUNNING)  

    //*********************************************************************  

    //Wait for rising edge of A1 and log time  
    while (HEA1 == NOPULSE);  
    //Wait for rising edge of A2 and log time (30 degrees?)  
    while (HEA2 == NOPULSE);  
    //Wait for falling edge of A1  
    while (HEA1 == PULSE);  
    //Wait for falling edge of A2  
    while (HEA2 == PULSE);  
    //*********************** 
    //TIME THE FULL SEQUENCE  
    //**********************  
    //Wait for rising edge of A1 and log time  
    while (HEA1 == NOPULSE);  
    tA1[0] = timer.read_us();  
    //Wait for rising edge of A2 and log time (30 degrees?)  
    while (HEA2 == NOPULSE);  
    tA2[0] = timer.read_us();  
    //Wait for falling edge of A1  
    while (HEA1 == PULSE);  
    tA1[1] = timer.read_us();  
    //Wait for falling edge of A2  
    while (HEA2 == PULSE);  
    tA2[1] = timer.read_us();  
    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;  
    float fA = 1.0f/ (TA *(float)3.0E-6);  
    terminal.printf("Average A2 Shaft: %6.2fHz \t Wheel: %6.2f\n", fA, fA/20.2f);  
    //Reset timers  
    timer.stop();  
    //Wait for button press  
    while (SW1 == 1);  
    wait(0.2);  
    while (SW1 == 0);  
    wait(0.1);  
    //******************************************  

    //Adapt duty to meet 1 revolution per second  

    //******************************************  
    float wA = fA/20.2f;            //Wheel speed  
    float deltaA = 1.0f-wA;         //Error  
    dutyA = dutyA + deltaA*0.1f;    //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;  
    //Update duty cycle to correct in the first direction  
    PWMA.write(dutyA);                    
    //Echo to the terminal  
     terminal.printf("Adapting duty cycle to %6.2f\n", dutyA);  
    }  
}