Diff: main.cpp

Revision:

25:50d3f80cb763

Parent:

24:b05cb3dd943e

Child:

26:072ab2309eec

--- a/main.cpp	Wed Dec 11 12:44:31 2019 +0000
+++ b/main.cpp	Wed Dec 11 16:32:28 2019 +0000
@@ -50,6 +50,7 @@
 float TAA, TBB = 0.0f;
 float pulse = 0.0f;
 float trav = 0.0f;
+float rpsA, rpsB = 0.0f;
 void timeA()
 {
     static int n=0;            //Number of pulse sets
@@ -169,6 +170,8 @@
     //Update duty cycle to correct in the first direction
     PWMA.write(dutyA);
     PWMB.write(dutyB);
+    rpsA = dutyA;
+    rpsB = dutyB;
 }
 void cornerRPS(){
     float deltaA = 1.0f-sumA;         //Error
@@ -176,17 +179,19 @@
     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>1.0f) ? 1.0f : dutyA;
     dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
-    dutyB = (dutyB>0.5f) ? 0.5f : dutyB;
+    dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
     dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
     //Update duty cycle to correct in the first direction
-    PWMA.write(dutyA);
-    PWMB.write(dutyB);
+    PWMA.write(dutyA+0.2);
+    PWMB.write(dutyB-0.35);
 }
 void reset(){
     trav = 0;
     pulse = 0;
+    dutyA = rpsA;
+    dutyB = rpsB;
 }
 int main()
 {
@@ -250,7 +255,7 @@
         timeB();
         oneRPS();
         terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
-        while(trav <= 1250)  
+        while(trav <= 1400)  
             {  
                 timeA();
                 timeB();
@@ -258,7 +263,7 @@
                 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);      
             } 
         reset();
-        while(trav <= 330)  
+        while(trav <= 640)  
             {  
                 timeA();
                 timeB();
@@ -266,7 +271,7 @@
                 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
             }
         reset();
-        while(trav <= 1457)  
+        while(trav <= 1550)  
             {  
                 timeB();
                 timeA();
@@ -274,15 +279,15 @@
                 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
             }
         reset();
-        while(trav <= 278.5)  
+        while(trav <= 455)  
             {  
                 timeA();
                 timeB();
                 cornerRPS();
                 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
             }
-        reset();
-        while(trav <= 750)  
+        reset();     
+        while(trav <= 800)  
             {  
                 timeB();
                 timeA();
@@ -290,7 +295,7 @@
                 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
             }
         reset();
-        while(trav <= 200)  
+        while(trav <= 330)  
             {  
                 timeA();
                 timeB();