File content as of revision 14:e66cf781f5b9:

//Enhancement 2//

#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;
Timer timer1;
 
//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;
void time()
    {
        //Reset timer and Start
    timer.reset();
    timer.start();

    //**********************
    //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();
    //Wait for rising edge of B1 and log time
    while (HEB1 == NOPULSE);
    tB1[0] = timer.read_us();
    //Wait for rising edge of B2 and log time (30 degrees?)
    while (HEB2 == NOPULSE);
    tB2[0] = timer.read_us();
    //Wait for falling edge of B1
    while (HEB1 == PULSE);
    tB1[1] = timer.read_us();
    //Wait for falling edge of B2
    while (HEB2 == PULSE);
    tB2[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 for A
    float TA1 = 2.0f * (tA1[1]-tA1[0]);
    float TA2 = 2.0f * (tA2[1]-tA2[0]);
    float TA = (TA1 + TA2) * 0.5f;

    //Calculate the frequency of rotation for B
    float TB1 = 2.0f * (tB1[1]-tB1[0]);
    float TB2 = 2.0f * (tB2[1]-tB2[0]);
    float TB = (TB1 + TB2) * 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); // FOR A
    float fB = 1.0f/ (TB *(float)3.0E-6); // 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;
    }
         
void reset()
{
    timer1.reset();
    time();
}
         
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;
    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);
    timer1.reset();
    timer1.start();
    //Main polling loop
    
    float paceA = 1.0f;
    float paceB = 1.0f;
    PWMA.write(paceA);          //Set duty cycle (%)
    PWMB.write(paceB);
    while(1)
    {
        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);
    }
}