Diff: main.cpp

Revision:

7:ca62dda005d5

Parent:

6:999e8ae7d969

Child:

8:b3ce040fdebc

--- a/main.cpp	Mon May 18 15:18:22 2020 +0000
+++ b/main.cpp	Tue May 19 14:31:15 2020 +0000
@@ -31,6 +31,7 @@
 RawSerial rpc(D1,D0,9600);
 
 int period_us = 26;//300 26
+int posCounter = 0;
 
 long pos=0;
 
@@ -54,6 +55,7 @@
 float xPosNew = 0;
 float xPosOld = 0;
 float speed = 0;
+float posMap[4] = {0.0f, 0.0f, 0.0f, 0.0f}; 
 
 double dAngle = 0.0f;
 double anSpd = 0.0f;
@@ -167,8 +169,25 @@
 }*/
 
 void calcSpeed() {
+    /*posMap[posCounter] = getPosMM() / 1000;
+    if (posCounter == 3) {
+        posCounter = 0;
+        float pathSum = 0;
+        for (int i = 0; i < 3; i++) {
+            float pathDiff = posMap[i + 1] - posMap[i]; 
+            if ((pathDiff) < 0) {
+                pathSum += -pathDiff;
+            } else {
+                pathSum += pathDiff;
+            }
+        }
+        speed = pathSum;  
+    } else {
+        posCounter += 1;
+    }*/
     xPosNew = getPosMM() / 1000;
     speed = xPosNew - xPosOld;
+    speed = -speed;
     xPosOld = xPosNew;
 }
 
@@ -176,7 +195,7 @@
     //float frequency = 1000000 / (period_us / 2);
     //float rates = frequency / 6400;
     //control = rates * PIPI * radius;
-    control = period_us;
+    control = -period_us;
 }
 
 
@@ -229,7 +248,7 @@
 }
 
 void getSwingDirectory() {
-        //control = -control;
+        control = -control;
         if (dir == DIR_RIGHT) {
             state = STATE_SWING_RIGHT;
             dir = DIR_LEFT;
@@ -277,7 +296,7 @@
         wait_ms(500);
     }
     angleOffset= 3.14 - angle;
-    wait(15);
+    wait(12);
     updatePeriod();
     calcControl();
     state=STATE_GOTO_START;