Diff: main.cpp

Revision:

0:e4566f96ab5c

Child:

1:0d3f572cbc19

diff -r 000000000000 -r e4566f96ab5c main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Nov 30 13:41:21 2018 +0000
@@ -0,0 +1,343 @@
+#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);
+}