Diff: main.cpp

Revision:

14:353e92037bc8

Parent:

13:205002e3c176

--- a/main.cpp	Sun May 24 13:23:08 2020 +0000
+++ b/main.cpp	Sun May 24 13:53:26 2020 +0000
@@ -59,7 +59,7 @@
 float dx = 0;
 float xPosNew = 0;
 float xPosOld = 0;
-float speed = -1;
+float speed = 0;
 float posOffset = 0.0f;
 float posMap[4] = {0.0f, 0.0f, 0.0f, 0.0f}; 
 
@@ -114,15 +114,15 @@
     angle = getPendulumPos();
     angle = angle * 6.28f / 1024.0f;
     angle += angleOffset;
-    if (angle > 3.14) {
+    /*if (angle > 3.14) {
         angle = angle - PIPI;    
-    }
-    /*if (angle > PIPI + 0.01) {
+    }*/
+    if (angle > PIPI + 0.01) {
         angle = fmod((angle + 3.14), PIPI) - 3.14;
         if (angle < -3.14) {
             angle += PIPI;
         }
-    }*/
+    }
     /*if (calibrated) {
         float test = fmodf(angle, M_PI);
         if (test >= 1) {
@@ -186,11 +186,13 @@
         posCounter += 1;
     }*/
     
-    xPosNew = (getPosMM() - posOffset) / 1000;
+   // xPosNew = (getPosMM() - posOffset) / 1000;
+    xPosNew = (getPosMM()) / 1000;
     speed = xPosNew - xPosOld;
-    if (dir == DIR_LEFT) {
+    /*if (dir == DIR_LEFT) {
         speed = -speed;
-    }
+    }*/
+    speed = (dir.read() * 2 - 1) * speed;
     xPosOld = xPosNew;
 }
 
@@ -198,11 +200,12 @@
     //float frequency = 1000000 / (period_us / 2);
     //float rates = frequency / 6400;
     //control = rates * PIPI * radius;
-    if (dir = DIR_RIGHT) {
+    /*if (dir = DIR_RIGHT) {
         control = -period_us;
     } else {
         control = period_us;
-    }
+    }*/
+    control = -timer_period_us;
 }
 
 
@@ -269,7 +272,8 @@
 
 void sendData() {
     myUnion.number[0] = t.read();
-    myUnion.number[1] = (getPosMM()- posOffset) / 1000 ;
+    //myUnion.number[1] = (getPosMM()- posOffset) / 1000 ;
+    myUnion.number[1] = getPosMM() / 1000 ;
     myUnion.number[2] = speed;
     //myUnion.number[3] = dAngle;
     myUnion.number[3] = angle;
@@ -282,6 +286,16 @@
 
 
 void changeDir() {
+    led = !led;
+    xPosNew = (getPosMM()) / 1000;
+    speed = xPosNew - xPosOld;
+    /*if (dir == DIR_LEFT) {
+        speed = -speed;
+    }*/
+    speed = -speed;
+    control = -control;
+    //speed = (dir.read() * 2 - 1) * speed;
+    xPosOld = xPosNew;
     if (dir == DIR_LEFT) {
         dir = DIR_RIGHT;    
     } else {
@@ -367,15 +381,15 @@
         }*/
         switch(state) {
             case STATE_WAITING:
-                //state = STATE_GOTO_SWING;
+                state = STATE_GOTO_SWING;
                 break;
             case STATE_GOTO_START:
                 break;
             case STATE_GOTO_END_COUNTING:
                 break;
             case STATE_GOTO_SWING:
-                speedTicker.attach(calcSpeed, 1);
-                //swingTicker.attach(getSwingDirectory, 1);
+                //speedTicker.attach(calcSpeed, 1);
+                swingTicker.attach(changeDir, 1);
                 state = STATE_SWING_LEFT;
                 break;
             case STATE_SWING_LEFT: