File content as of revision 1:0d3f572cbc19:

#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
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 Counters
//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;
}

//Wheel Speed Calculations
//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;
}

//Duty Calculations
//Calculate Duty Cycle Correction A
void correctionA(){
    dutyA = dutyA + dA*0.1f;
    dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
    dutyA = (dutyA<0.01f) ? 0.01f : dutyA;
    PWMA.write(dutyA);
    terminal.printf("\n\rDuty A = %6.2f", dutyA);
    }
//Calculate Duty Cycle Correction B
void correctionB(){
    dutyB = dutyB + dB*0.1f;
    dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
    dutyB = (dutyB<0.01f) ? 0.01f : dutyB;
    PWMB.write(dutyB);
    terminal.printf("\n\rDuty B = %6.2f", dutyB);
    }
//Calculate Duty Cycle Correction for turns
void correction90(){
    dutyA = dutyA + dA*0.1f;
    dutyA = (dutyA>2.4f) ? 2.4f : dutyA;
    dutyA = (dutyA<1.0f) ? 1.0f : dutyA;
    PWMA.write(dutyA);
    terminal.printf("\n\rDuty A = %6.2f", dutyA);
}
//Calculate Duty Cycle Correction A (VICTORY)
void cvA(){
    dutyA = dutyA + dA*0.1f;
    dutyA = (dutyA>0.25f) ? 0.25f : dutyA;
    dutyA = (dutyA<0.01f) ? 0.01f : dutyA;
    PWMA.write(dutyA);
    terminal.printf("\n\rDuty A = %6.2f", dutyA);
    }
//Calculate Duty Cycle Correction B (VICTORY)
void cvB(){
    dutyB = dutyB + dB*0.1f;
    dutyB = (dutyB>0.25f) ? 0.25f : dutyB;
    dutyB = (dutyB<0.01f) ? 0.01f : dutyB;
    PWMB.write(dutyB);
    terminal.printf("\n\rDuty B = %6.2f", dutyB);
    }
//Movement Instructions
//Drive function
void drive(){
    PWMA.write(dutyA);
    PWMB.write(dutyB);
}
//Victory dance function
void victoryDance(){
    PWMA.write(dutyA);
    PWMB.write(dutyB);
    DIRA = REVERSE;
    DIRB = FORWARD;
}

//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)-----------------
    correctionA();
    correctionB();
    while(cA1 < 177 && cB1 < 177){
        drive();
        pulsecountA();
        pulsecountB();
        if(cws == 4){
            wsA();
            wsB();
            correctionA();
            correctionB();
            cws = 0;
            }
        terminal.printf("\n\rA Pulses = %d", cA1);
        terminal.printf("\n\rB Pulses = %d", cB1);
        }
        cA1 = 0;
        cB1 = 0;
    
    //Move pulses (turn)---------------------
    correction90();
    terminal.printf("\n\r");
    while (cA1 < 34){
        drive();
        pulsecountA();
        pulsecountB();
        if(cws == 4){
            wsA();
            wsB();
            correction90();
            correctionB();
            cws = 0;
            }
        terminal.printf("\n\rA Pulses = %d", cA1);
        }
        cA1 = 0;
        cB1 = 0;

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