File content as of revision 25:50d3f80cb763:

//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 = 0.2f; //100%
float dutyB = 0.2f; //100%
//Array of sensor data
int tA1[2];
int tA2[2];
int tB1[2];
int tB2[2];
float speedA[3];
float speedB[3];
float fA, fB = 0.0f;
float sumA, sumB = 0.0f;
int durA, durB = 0;
float TA[2];
float TB[2];
float TAA, TBB = 0.0f;
float pulse = 0.0f;
float trav = 0.0f;
float rpsA, rpsB = 0.0f;
void timeA()
{
    static int n=0;            //Number of pulse sets
    static int HallState = 0;
    //**********************
    //TIME THE FULL SEQUENCE
    //**********************
    if (n==0) {
        //Reset timer and Start
        timerA.reset();
        timerA.start();  
        TA[0] = timerA.read_us();      
    }
    switch(HallState)
            {
                case 0:
                if(HEA1 == PULSE){
                    HallState = 1;
                }break;
                case 1:
                if(HEA1 == NOPULSE){
                    HallState = 0;
                    n++;
                    pulse++;
                    trav = ((176/20.8)/3)*pulse;
                }break;
            }
    if (n < 9) return;
    TA[1] = timerA.read_us(); 
    TAA = (TA[1]-TA[0]);
    // Calculate speeed
    fA = 2.0f/ (TAA *(float)1.0E-6);
    if(durA == 0){
        speedA[durA] = fA/20.8;
        durA++;
        return;
    }
    else if(durA == 1){
        speedA[durA] = fA/20.8;
        durA++;
        return;
    }
    else if(durA == 2){
        speedA[durA] = fA/20.8;
        durA = 0;
    }
    for(int xA=0;xA<3;xA++){
      sumA+=speedA[xA];
    }
    sumA = sumA/3;
    //Reset count
    n=0;
    }

void timeB()
{
    static int nB=0;            //Number of pulse sets
    static int HallStateB = 0;
    //**********************
    //TIME THE FULL SEQUENCE
    //**********************
    if (nB==0) {
        //Reset timer and Start
        timerB.reset();
        timerB.start();  
        TB[0] = timerB.read_us();      
    }
    switch(HallStateB)
            {
                case 0:
                if(HEB1 == PULSE){
                    HallStateB = 1;
                }break;
                case 1:
                if(HEB1 == NOPULSE){
                    HallStateB = 0;
                    nB++;
                }break;
            }
    if (nB < 9) return;
    TB[1] = timerB.read_us();   
    TBB = (TB[1]-TB[0]);
    // Calculate speeed
    fB = 2.0f/ (TBB *(float)1.0E-6);
    if(durB == 0){
        speedB[durB] = fB/20.8;
        durB++;
        return;
    }
    else if(durB == 1){
        speedB[durB] = fB/20.8;
        durB++;
        return;
    }
    else if(durB == 2){
        speedB[durB] = fB/20.8;
        durB = 0;
    }
    for(int xB=0;xB<3;xB++){
      sumB+=speedB[xB];
    }
    sumB = sumB/3;
    //Reset count
    nB=0;
    }

void oneRPS(){
    float deltaA = 1.0f-sumA;         //Error
    float deltaB = 1.0f-sumB;
    dutyA = dutyA + deltaA*0.001f;    //Increase duty in proportion to the error
    dutyB = dutyB + deltaB*0.001f;    //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;
    dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
    dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
    //Update duty cycle to correct in the first direction
    PWMA.write(dutyA);
    PWMB.write(dutyB);
    rpsA = dutyA;
    rpsB = dutyB;
}
void cornerRPS(){
    float deltaA = 1.0f-sumA;         //Error
    float deltaB = 1.0f-sumB;
    dutyA = dutyA + deltaA*0.001f;    //Increase duty in proportion to the error
    dutyB = dutyB + deltaB*0.001f;    //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;
    dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
    dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
    //Update duty cycle to correct in the first direction
    PWMA.write(dutyA+0.2);
    PWMB.write(dutyB-0.35);
}
void reset(){
    trav = 0;
    pulse = 0;
    dutyA = rpsA;
    dutyB = rpsB;
}
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
    PWMA.write(0.2f);          //Set duty cycle (%)
    PWMB.write(0.2f);
    while(1)
    {
        timeA();
        timeB();
        oneRPS();
        terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
        while(trav <= 1400)  
            {  
                timeA();
                timeB();
                oneRPS();  
                terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);      
            } 
        reset();
        while(trav <= 640)  
            {  
                timeA();
                timeB();
                cornerRPS();
                terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
            }
        reset();
        while(trav <= 1550)  
            {  
                timeB();
                timeA();
                oneRPS();
                terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
            }
        reset();
        while(trav <= 455)  
            {  
                timeA();
                timeB();
                cornerRPS();
                terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
            }
        reset();     
        while(trav <= 800)  
            {  
                timeB();
                timeA();
                oneRPS();
                terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
            }
        reset();
        while(trav <= 330)  
            {  
                timeA();
                timeB();
                cornerRPS();
                terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
            }  
        break;
    }
    PWMA.write(0.0f);    
    PWMB.write(0.0f);
        
}