File content as of revision 21:d553c43a5a26:

//Enhancement 2//
//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 timerA;
Timer timerB;

//Enumerated types
enum DIRECTION   {FORWARD=0, REVERSE};
enum PULSE       {NOPULSE=0, PULSE};
enum SWITCHSTATE {PRESSED=0, RELEASED};

//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;
float speedA, speedB = 1.0f;

int pulse = 0;

void timeA()
{
    static int n=0;            //Number of pulse sets
    static int HallState = 0;
    static float T = 0.0f;
    //**********************
    //TIME THE FULL SEQUENCE
    //**********************
    if (n==0) {
        //Reset timer and Start
        timerA.reset();
        timerA.start();  
        T = timerA.read_us();      
    }
    switch(HallState)
            {
                case 0:
                if(HEA1 == PULSE){
                    HallState = 1;
                }break;
                case 1:
                if(HEA2 == PULSE){
                    HallState = 2;
                }break;
                case 2:
                if(HEA1 == NOPULSE){
                    HallState = 3;
                }break;
                case 3:
                if(HEA2 == NOPULSE){
                    HallState = 0;
                    n++;
                }break;
            }
    if (n < 9) return;
    T = timerA.read_us();
    T = T;    
    // Calculate speeed and adapt
    speedA = ((175.9/20.8)/3) / T;
    //Reset count
    n=0;
    }

void timeB()
{
    static int n=0;            //Number of pulse sets
    static int HallState = 0;
    static float TB = 0.0f;
    //**********************
    //TIME THE FULL SEQUENCE
    //**********************
    if (n==0) {
        //Reset timer and Start
        timerB.reset();
        timerB.start();  
        TB = timerB.read_us();      
    }
    switch(HallState)
            {
                case 0:
                if(HEB1 == PULSE){
                    HallState = 1;
                }break;
                case 1:
                if(HEB2 == PULSE){
                    HallState = 2;
                }break;
                case 2:
                if(HEB1 == NOPULSE){
                    HallState = 3;
                }break;
                case 3:
                if(HEB2 == NOPULSE){
                    HallState = 0;
                    n++;
                }break;
            }
    if (n < 9) return;
    TB = timerB.read_us();   
    TB = TB; 
    // Calculate speeed and adapt
    speedB = ((175.9/20.8)/3) / TB;
    //Reset count
    n=0;
    }


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);
     //Set align wheels
    PWMA.write(1.0f);          //Set duty cycle (%)
    PWMB.write(1.0f);          //Set duty cycle (%)
    //*********************************************************************
    //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);
    //Wait for rising edge of B1 and log time
    while (HEB1 == NOPULSE);
    //Wait for rising edge of B2 and log time (30 degrees?)
    while (HEB2 == NOPULSE);
    //Wait for falling edge of B1
    while (HEB1 == PULSE);
    //Wait for falling edge of B2
    while (HEB2 == PULSE);

    //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);
    //Main polling loop

    float paceA = 1.0f;
    float paceB = 1.0f;
    PWMA.write(paceA);          //Set duty cycle (%)
    PWMB.write(paceB);
    while(1)
    {
        timeA();
        timeB();
        terminal.printf("wheelA: %4.2f \t wheelB: %4.2f \t \n\r", speedA, speedB);
        if (speedA <= 1.0f)
        {
            paceA +=0.005f;
        }
        if (speedA >= 1.0f)
        {
            paceA -=0.005f;
        }
        if (speedB <= 1.0f)
        {
            paceB +=0.005f;
        }
        if (speedB >= 1.0f)
        {
            paceB -=0.005f;
        }
        PWMA.write(paceA);
        PWMB.write(paceB);
    }
}