Diff: main.cpp

Revision:

1:0d3f572cbc19

Parent:

0:e4566f96ab5c

--- a/main.cpp	Fri Nov 30 13:41:21 2018 +0000
+++ b/main.cpp	Mon Dec 03 11:30:12 2018 +0000
@@ -11,7 +11,7 @@
 DigitalIn HEA2(PB_1);
 DigitalIn HEB1(PB_15);
 DigitalIn HEB2(PB_14);
-//Use the serial object so we can use higher speeds
+//Use the serial object
 Serial terminal(USBTX, USBRX);
 //Enumerated types
 enum DIRECTION   {FORWARD=0, REVERSE};
@@ -28,6 +28,7 @@
 float dA;
 float dB;
 
+//Pulse Counters
 //Pulse Counter for wheel A
 void pulsecountA(){
     while(HEA1 == PULSE);
@@ -42,6 +43,7 @@
     cB1 = cB1 + 1;
 }
 
+//Wheel Speed Calculations
 //Speed calculation for wheel A
 void wsA(){
     int tA1[2];
@@ -105,82 +107,59 @@
     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(){
+//Duty Calculations
+//Calculate Duty Cycle Correction A
+void correctionA(){
     dutyA = dutyA + dA*0.1f;
-    dutyB = dutyB + dB*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;
-    PWMA.write(dutyA);
     PWMB.write(dutyB);
-    terminal.printf("\n\rDuty A = %6.2f", dutyA);
     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);
 }
-//Turn function
-void turn(){
+//Victory dance function
+void victoryDance(){
     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;
+    DIRA = REVERSE;
+    DIRB = FORWARD;
 }
 
 //Start code------------------------------------
@@ -200,6 +179,8 @@
     drive();
     
     //Move 177 pulses (1 meter)-----------------
+    correctionA();
+    correctionB();
     while(cA1 < 177 && cB1 < 177){
         drive();
         pulsecountA();
@@ -207,7 +188,8 @@
         if(cws == 4){
             wsA();
             wsB();
-            correction();
+            correctionA();
+            correctionB();
             cws = 0;
             }
         terminal.printf("\n\rA Pulses = %d", cA1);
@@ -216,15 +198,18 @@
         cA1 = 0;
         cB1 = 0;
     
-    //Move 24 pulses (turn)---------------------
+    //Move pulses (turn)---------------------
+    correction90();
     terminal.printf("\n\r");
-    while (cA1 < 24){
-        turn();
+    while (cA1 < 34){
+        drive();
         pulsecountA();
+        pulsecountB();
         if(cws == 4){
-            wsC();
+            wsA();
             wsB();
             correction90();
+            correctionB();
             cws = 0;
             }
         terminal.printf("\n\rA Pulses = %d", cA1);
@@ -233,6 +218,8 @@
         cB1 = 0;
 
     //Move 89 pulses (0.5 meters)---------------
+    correctionA();
+    correctionB();
     terminal.printf("\n\r");
     while(cA1 < 89 && cB1 < 89){
         drive();
@@ -241,7 +228,8 @@
         if(cws == 4){
             wsA();
             wsB();
-            correction();
+            correctionA();
+            correctionB();
             cws = 0;
             }
         terminal.printf("\n\rA Pulses = %d", cA1);
@@ -250,15 +238,18 @@
         cA1 = 0;
         cB1 = 0;
     
-    //Move 24 pulses (turn)---------------------
+    //Move pulses (turn)---------------------
+    correction90();
     terminal.printf("\n\r");
-    while (cA1 < 24){
-        turn();
+    while (cA1 < 34){
+        drive();
         pulsecountA();
+        pulsecountB();
         if(cws == 4){
-            wsC();
+            wsA();
             wsB();
             correction90();
+            correctionB();
             cws = 0;
             }
         terminal.printf("\n\rA Pulses = %d", cA1);
@@ -267,6 +258,8 @@
         cB1 = 0;
     
     //Move 177 pulses (1 meter)-----------------
+    correctionA();
+    correctionB();
     terminal.printf("\n\r");    
     while(cA1 < 177 && cB1 < 177){
         drive();
@@ -275,7 +268,8 @@
         if(cws == 4){
             wsA();
             wsB();
-            correction();
+            correctionA();
+            correctionB();
             cws = 0;
             }
         terminal.printf("\n\rA Pulses = %d", cA1);
@@ -284,15 +278,18 @@
         cA1 = 0;
         cB1 = 0;
     
-    //Move 24 pulses (turn)---------------------
+    //Move pulses (turn)---------------------
+    correction90();
     terminal.printf("\n\r");
-    while (cA1 < 24){
-        turn();
+    while (cA1 < 34){
+        drive();
         pulsecountA();
+        pulsecountB();
         if(cws == 4){
-            wsC();
+            wsA();
             wsB();
             correction90();
+            correctionB();
             cws = 0;
             }
         terminal.printf("\n\rA Pulses = %d", cA1);
@@ -301,6 +298,8 @@
         cB1 = 0;
     
     //Move 89 pulses (0.5 meters)---------------
+    correctionA();
+    correctionB();
     terminal.printf("\n\r");
     while(cA1 < 89 && cB1 < 89){
         drive();
@@ -309,7 +308,8 @@
         if(cws == 4){
             wsA();
             wsB();
-            correction();
+            correctionA();
+            correctionB();
             cws = 0;
             }
         terminal.printf("\n\rA Pulses = %d", cA1);
@@ -318,15 +318,19 @@
         cA1 = 0;
         cB1 = 0;
     
-    //Move 24 pulses (turn)---------------------
+    //Move pulses (turn)---------------------
+    correction90();
     terminal.printf("\n\r");
-    while (cA1 < 24){
-        turn();
+    while (cA1 < 34){
+        drive();
         pulsecountA();
+        pulsecountB();
         if(cws == 4){
-            wsC();
+            wsA();
             wsB();
             correction90();
+            correctionB();
+            cws = 0;
             }
         terminal.printf("\n\rA Pulses = %d", cA1);
         }
@@ -334,8 +338,23 @@
         cB1 = 0;
     
     //Vicorty Dance-----------------------------
-    victoryDance();
-    wait(3);
+    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);