Diff: main.cpp

Revision:

24:b05cb3dd943e

Parent:

22:568a6d19b98a

Child:

25:50d3f80cb763

--- a/main.cpp	Wed Dec 11 12:39:31 2019 +0000
+++ b/main.cpp	Wed Dec 11 12:44:31 2019 +0000
@@ -170,6 +170,20 @@
     PWMA.write(dutyA);
     PWMB.write(dutyB);
 }
+void cornerRPS(){
+    float deltaA = 1.0f-sumA;         //Error
+    float deltaB = 1.0f-sumB;
+    dutyA = dutyA + deltaA*0.001f;    //Increase duty in proportion to the error
+    dutyB = dutyB + deltaB*0.001f;    //Increase duty in proportion to the error
+    //Clamp the max and min values of duty and 0.0 and 1.0 respectively
+    dutyA = (dutyA>1.5f) ? 1.5f : dutyA;
+    dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
+    dutyB = (dutyB>0.5f) ? 0.5f : dutyB;
+    dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
+    //Update duty cycle to correct in the first direction
+    PWMA.write(dutyA);
+    PWMB.write(dutyB);
+}
 void reset(){
     trav = 0;
     pulse = 0;
@@ -247,7 +261,8 @@
         while(trav <= 330)  
             {  
                 timeA();
-                PWMB.write(0.0f);
+                timeB();
+                cornerRPS();
                 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
             }
         reset();
@@ -262,7 +277,8 @@
         while(trav <= 278.5)  
             {  
                 timeA();
-                PWMB.write(0.0f);
+                timeB();
+                cornerRPS();
                 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
             }
         reset();
@@ -277,7 +293,8 @@
         while(trav <= 200)  
             {  
                 timeA();
-                PWMB.write(0.0f);
+                timeB();
+                cornerRPS();
                 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
             }  
         break;