Diff: main.cpp

Revision:

8:b3ce040fdebc

Parent:

7:ca62dda005d5

Child:

9:e8c6a414e226

--- a/main.cpp	Tue May 19 14:31:15 2020 +0000
+++ b/main.cpp	Wed May 20 15:07:31 2020 +0000
@@ -54,7 +54,8 @@
 float dx = 0;
 float xPosNew = 0;
 float xPosOld = 0;
-float speed = 0;
+float speed = -1;
+float posOffset = 0.0f;
 float posMap[4] = {0.0f, 0.0f, 0.0f, 0.0f}; 
 
 double dAngle = 0.0f;
@@ -71,6 +72,8 @@
 double PIPI = 6.28;
 
 bool canSend = false;
+bool periodUpdated = false;
+bool calibrated = false;
 
 typedef union {
     float number[6];
@@ -105,12 +108,24 @@
     angle = getPendulumPos();
     angle = angle * 6.28f / 1024.0f;
     angle += angleOffset;
-    if (angle > PIPI + 0.01) {
+    if (angle > 3.14) {
+        angle = angle - PIPI;    
+    }
+    /*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) {
+            angle = -test;    
+        } else {
+            angle = test;
+        }
+    }*/
     return angle;
 }
 
@@ -147,14 +162,14 @@
     SPEED CALC
 */
 
-void getDeltaPos()
+/*void getDeltaPos()
 {
     float delta = 0;
     xPosNew = getPosMM();
     delta = xPosNew - xPosOld;
     dx = delta;
     xPosOld = xPosNew;
-}
+}*/
 
 /*float getSpeed(){
     float deltaTime;
@@ -185,9 +200,12 @@
     } else {
         posCounter += 1;
     }*/
-    xPosNew = getPosMM() / 1000;
+    xPosNew = (getPosMM() - posOffset) / 1000;
     speed = xPosNew - xPosOld;
-    speed = -speed;
+    if (dir == DIR_LEFT) {
+        speed = -speed;    
+    }
+    
     xPosOld = xPosNew;
 }
 
@@ -195,7 +213,11 @@
     //float frequency = 1000000 / (period_us / 2);
     //float rates = frequency / 6400;
     //control = rates * PIPI * radius;
-    control = -period_us;
+    if (dir = DIR_RIGHT) {
+        control = -period_us;
+    } else {
+        control = period_us;
+    }
 }
 
 
@@ -206,6 +228,7 @@
         pos += (dir.read() * 2 - 1);
     }
     if (state == STATE_GOTO_MIDDLE && pos == railLength / 2) {
+        posOffset = 40;
         state = STATE_WAITING;
     }
    if (state == STATE_SWING_LEFT && state==STATE_SWING_RIGHT) {
@@ -263,7 +286,7 @@
 
 void sendData() {
     myUnion.number[0] = t.read();
-    myUnion.number[1] = getPosMM() / 1000;
+    myUnion.number[1] = (getPosMM()- posOffset) / 1000 ;
     myUnion.number[2] = speed;
     //myUnion.number[3] = dAngle;
     myUnion.number[3] = angle;
@@ -276,8 +299,33 @@
 
 void Rx_interrupt() {
     int command = rpc.getc();
-    if (command == 50) {
-        canSend = true;
+    switch (command) {
+        case 50:
+            canSend = true;
+            break;
+        case 60:
+            state = STATE_GOTO_SWING;
+            speedTicker.attach(calcSpeed, 1);
+            break;
+        case 65:
+            int direction = rpc.getc();
+            if (direction <= 0) {
+                dir = DIR_RIGHT;
+            } else if (direction > 0) {
+                dir = DIR_LEFT;    
+            }
+            break;
+        case 70:
+            int newPeriod = rpc.getc();
+            if (newPeriod < 26) {
+                tick.detach();
+            } else {
+                period_us = newPeriod;
+                periodUpdated = true;
+            }
+            break;
+        default:
+            break;
     }
     return;
 }
@@ -296,28 +344,34 @@
         wait_ms(500);
     }
     angleOffset= 3.14 - angle;
-    wait(12);
+    calibrated = true;
+    //wait(12);
+    wait(7);
     updatePeriod();
     calcControl();
     state=STATE_GOTO_START;
     dir=DIR_LEFT;
     while(1) {
         getAngularSpeed();
-        if(canSend) {
+        if (canSend) {
             sendData();
         }
+        if (periodUpdated) {
+            calcControl();
+            updatePeriod();
+            periodUpdated = false;
+        }
         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);
-                state = STATE_SWING_LEFT;
+                //swingTicker.attach(getSwingDirectory, 1);
+                //state = STATE_SWING_LEFT;
                 break;
             case STATE_SWING_LEFT:
                 break;