File content as of revision 1:2a6b6d2112ba:

/*  
 Version 3 – 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 = 0.99f; //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-0.011);          //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);  
    }  
}