the coding of the bots / Mbed 2 deprecated buggyboi_expansion2

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
Pabs44
Date:
Fri Nov 30 13:34:17 2018 +0000
Commit message:
Test code for class

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r b6d1ab1ace48 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Nov 30 13:34:17 2018 +0000
@@ -0,0 +1,295 @@
+#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);
+    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);
+    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;
+}
+
+//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(0.0f);
+}
+//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){
+            wsA();
+            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){
+            wsA();
+            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){
+            wsA();
+            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){
+            wsA();
+            cws = 0;
+            }
+        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);
+}
diff -r 000000000000 -r b6d1ab1ace48 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Nov 30 13:34:17 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/3a7713b1edbc
\ No newline at end of file