File content as of revision 0:e4566f96ab5c:

#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 Input
DigitalIn HEA1(PB_2);
DigitalIn HEA2(PB_1);
DigitalIn HEB1(PB_15);
DigitalIn HEB2(PB_14);
//Use the serial object so we can use higher speeds
Serial terminal(USBTX, USBRX);
//Enumerated types
enum DIRECTION   {FORWARD=0, REVERSE};
enum PULSE       {NOPULSE=0, PULSE};
//Counter Set
int cA1 = 0;
int cB1 = 0;
int cws = 0;
//Timer Set
Timer timer;
//Set Duty Cycles
float dutyA = 1.0f;
float dutyB = 1.0f;
float dA;
float dB;

//Pulse Counter for wheel A
void pulsecountA(){
    while(HEA1 == PULSE);
    while(HEA1 == NOPULSE);
    cA1 = cA1 + 1;
    cws = cws + 1;
}
//Pulse Counter for wheel B
void pulsecountB(){
    while(HEB1 == PULSE);
    while(HEB1 == NOPULSE);
    cB1 = cB1 + 1;
}

//Speed calculation for wheel A
void wsA(){
    int tA1[2];
    int tA2[2];
    
    while(HEA1 == PULSE);
    
    while(HEA1 == NOPULSE);
    tA1[0] = timer.read_us();
    while(HEA2 == NOPULSE);
    tA2[0] = timer.read_us();
    
    while(HEA1 == PULSE);
    while(HEA2 == PULSE);
    
    while(HEA1 == NOPULSE);
    tA1[1] = timer.read_us();
    while(HEA2 == NOPULSE);
    tA2[1] = timer.read_us();
    
    float fA1 = 1.0f/((tA1[1]-tA1[0])*(float)3.0E-6);
    float fA2 = 1.0f/((tA2[1]-tA2[0])*(float)3.0E-6);
    
    float avA = (fA1 + fA2)/2.0f;
    
    float wsA = avA/20.8f;
    
    terminal.printf("\n\rA Wheel Speed = %6.2f", wsA);
    
    dA = 1.0f - wsA;
}
//Speed calculation for wheel B
void wsB(){
    int tB1[2];
    int tB2[2];
    
    while(HEB1 == PULSE);
    
    while(HEB1 == NOPULSE);
    tB1[0] = timer.read_us();
    while(HEB2 == NOPULSE);
    tB2[0] = timer.read_us();
    
    while(HEB1 == PULSE);
    while(HEB2 == PULSE);
    
    while(HEB1 == NOPULSE);
    tB1[1] = timer.read_us();
    while(HEB2 == NOPULSE);
    tB2[1] = timer.read_us();
    
    float fB1 = 1.0f/((tB1[1]-tB1[0])*(float)3.0E-6);
    float fB2 = 1.0f/((tB2[1]-tB2[0])*(float)3.0E-6);
    
    float avB = (fB1 + fB2)/2.0f;
    
    float wsB = avB/20.8f;
    
    terminal.printf("\n\rB Wheel Speed = %6.2f", wsB);
    
    dB = 1.0f - wsB;
}

void wsC(){
    int tA1[2];
    int tA2[2];
    
    while(HEA1 == PULSE);
    
    while(HEA1 == NOPULSE);
    tA1[0] = timer.read_us();
    while(HEA2 == NOPULSE);
    tA2[0] = timer.read_us();
    
    while(HEA1 == PULSE);
    while(HEA2 == PULSE);
    
    while(HEA1 == NOPULSE);
    tA1[1] = timer.read_us();
    while(HEA2 == NOPULSE);
    tA2[1] = timer.read_us();
    
    float fA1 = 2.4f/((tA1[1]-tA1[0])*(float)3.0E-6);
    float fA2 = 2.4f/((tA2[1]-tA2[0])*(float)3.0E-6);
    
    float avC = (fA1 + fA2)/2.0f;
    
    float wsC = avC/20.8f;
    
    terminal.printf("\n\rA Wheel Speed = %6.2f", wsC);
    
    dA = 1.0f - wsC;
}

//Calculate Duty Cycle Correction
void correction(){
    dutyA = dutyA + dA*0.1f;
    dutyB = dutyB + dB*0.1f;
    dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
    dutyA = (dutyA<0.01f) ? 0.01f : dutyA;
    dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
    dutyB = (dutyB<0.01f) ? 0.01f : dutyB;
    PWMA.write(dutyA);
    PWMB.write(dutyB);
    terminal.printf("\n\rDuty A = %6.2f", dutyA);
    terminal.printf("\n\rDuty B = %6.2f", dutyB);
    }

//Movement Instructions
//Drive function
void drive(){
    PWMA.write(dutyA);
    PWMB.write(dutyB);
}
//Turn function
void turn(){
    PWMA.write(dutyA);
    PWMB.write(dutyB);
}

void correction90(){
    dutyA = dutyA + dA*0.1f;
    dutyB = dutyB + dB*0.1f;
    dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
    dutyA = (dutyA<0.01f) ? 0.01f : dutyA;
    dutyB = (dutyB>2.4f) ? 2.4f : dutyB;
    dutyB = (dutyB<0.01f) ? 0.01f : dutyB;
    PWMA.write(dutyA);
    PWMB.write(dutyB);
    terminal.printf("\n\rDuty A = %6.2f", dutyA);
    terminal.printf("\n\rDuty B = %6.2f", dutyB);
}

//Victory dance function
void victoryDance(){
    PWMA.write(dutyA/4);
    PWMB.write(dutyB/4);
    DIRA = FORWARD;
    DIRB = REVERSE;
}

//Start code------------------------------------
int main() {
    //Setup
    terminal.baud(115200);
    PWMA.period_ms(10);
    PWMB.period_ms(10);
    DIRA = FORWARD;
    DIRB = FORWARD;
    
    //Start
    wait(2);
    terminal.printf("\n\rNew Test\n\r");
    timer.reset();
    timer.start();
    drive();
    
    //Move 177 pulses (1 meter)-----------------
    while(cA1 < 177 && cB1 < 177){
        drive();
        pulsecountA();
        pulsecountB();
        if(cws == 4){
            wsA();
            wsB();
            correction();
            cws = 0;
            }
        terminal.printf("\n\rA Pulses = %d", cA1);
        terminal.printf("\n\rB Pulses = %d", cB1);
        }
        cA1 = 0;
        cB1 = 0;
    
    //Move 24 pulses (turn)---------------------
    terminal.printf("\n\r");
    while (cA1 < 24){
        turn();
        pulsecountA();
        if(cws == 4){
            wsC();
            wsB();
            correction90();
            cws = 0;
            }
        terminal.printf("\n\rA Pulses = %d", cA1);
        }
        cA1 = 0;
        cB1 = 0;

    //Move 89 pulses (0.5 meters)---------------
    terminal.printf("\n\r");
    while(cA1 < 89 && cB1 < 89){
        drive();
        pulsecountA();
        pulsecountB();
        if(cws == 4){
            wsA();
            wsB();
            correction();
            cws = 0;
            }
        terminal.printf("\n\rA Pulses = %d", cA1);
        terminal.printf("\n\rB Pulses = %d", cB1);
        }
        cA1 = 0;
        cB1 = 0;
    
    //Move 24 pulses (turn)---------------------
    terminal.printf("\n\r");
    while (cA1 < 24){
        turn();
        pulsecountA();
        if(cws == 4){
            wsC();
            wsB();
            correction90();
            cws = 0;
            }
        terminal.printf("\n\rA Pulses = %d", cA1);
        }
        cA1 = 0;
        cB1 = 0;
    
    //Move 177 pulses (1 meter)-----------------
    terminal.printf("\n\r");    
    while(cA1 < 177 && cB1 < 177){
        drive();
        pulsecountA();
        pulsecountB();
        if(cws == 4){
            wsA();
            wsB();
            correction();
            cws = 0;
            }
        terminal.printf("\n\rA Pulses = %d", cA1);
        terminal.printf("\n\rB Pulses = %d", cB1);
        }
        cA1 = 0;
        cB1 = 0;
    
    //Move 24 pulses (turn)---------------------
    terminal.printf("\n\r");
    while (cA1 < 24){
        turn();
        pulsecountA();
        if(cws == 4){
            wsC();
            wsB();
            correction90();
            cws = 0;
            }
        terminal.printf("\n\rA Pulses = %d", cA1);
        }
        cA1 = 0;
        cB1 = 0;
    
    //Move 89 pulses (0.5 meters)---------------
    terminal.printf("\n\r");
    while(cA1 < 89 && cB1 < 89){
        drive();
        pulsecountA();
        pulsecountB();
        if(cws == 4){
            wsA();
            wsB();
            correction();
            cws = 0;
            }
        terminal.printf("\n\rA Pulses = %d", cA1);
        terminal.printf("\n\rB Pulses = %d", cB1);
        }
        cA1 = 0;
        cB1 = 0;
    
    //Move 24 pulses (turn)---------------------
    terminal.printf("\n\r");
    while (cA1 < 24){
        turn();
        pulsecountA();
        if(cws == 4){
            wsC();
            wsB();
            correction90();
            }
        terminal.printf("\n\rA Pulses = %d", cA1);
        }
        cA1 = 0;
        cB1 = 0;
    
    //Vicorty Dance-----------------------------
    victoryDance();
    wait(3);
    
    //Finish------------------------------------
    PWMA.write(0.0f);
    PWMB.write(0.0f);
}