Diff: main.cpp

Revision:

13:205002e3c176

Parent:

12:ee175985ef09

Child:

14:353e92037bc8

--- a/main.cpp	Sat May 23 15:24:09 2020 +0000
+++ b/main.cpp	Sun May 24 13:23:08 2020 +0000
@@ -27,7 +27,7 @@
 InterruptIn leftSwitch(D2);
 InterruptIn rightSwitch(D3);
 Ticker tick;
-//Ticker speedTicker;
+Ticker speedTicker;
 Ticker swingTicker;
 RawSerial rpc(D1,D0,9600);
 DigitalOut led(D4);
@@ -88,9 +88,9 @@
 
 my_union myUnion;
 
-/*bool isPendulumSwinging() {
+bool isPendulumSwinging() {
     return  state == STATE_SWING_RIGHT || state == STATE_SWING_LEFT;
-}*/
+}
 
 float getPosMM() {
     //return (pos-railLength/2) * 550.0f/railLength;
@@ -167,7 +167,8 @@
     SPEED CALC
 */
 
-/*void calcSpeed() {
+
+void calcSpeed() {
     /*posMap[posCounter] = getPosMM() / 1000;
     if (posCounter == 3) {
         posCounter = 0;
@@ -184,8 +185,16 @@
     } else {
         posCounter += 1;
     }*/
+    
+    xPosNew = (getPosMM() - posOffset) / 1000;
+    speed = xPosNew - xPosOld;
+    if (dir == DIR_LEFT) {
+        speed = -speed;
+    }
+    xPosOld = xPosNew;
+}
 
-/*void calcControl() {
+void calcControl() {
     //float frequency = 1000000 / (period_us / 2);
     //float rates = frequency / 6400;
     //control = rates * PIPI * radius;
@@ -194,7 +203,7 @@
     } else {
         control = period_us;
     }
-}*/
+}
 
 
 
@@ -231,14 +240,14 @@
         state = STATE_GOTO_END_COUNTING;
         pos = 0;
     }
-/*    else if (isPendulumSwinging()) {
+    else if (isPendulumSwinging()) {
         //state = STATE_GOTO_MIDDLE;
         //angleOffset -= 0.006191;
         state = STATE_ERROR;
+    }
+    /*if (state == STATE_GOTO_SWING) {
+        state = STATE_ERROR;
     }*/
-    if (state == STATE_GOTO_SWING) {
-        state = STATE_ERROR;
-    }
 }
 
 void rightEnd() {
@@ -247,14 +256,14 @@
         railLength=pos;
         state = STATE_GOTO_MIDDLE;
     }
-/*    else if (isPendulumSwinging()) {
+    else if (isPendulumSwinging()) {
         //state = STATE_GOTO_MIDDLE;
         //angleOffset += 0.006191;
         state = STATE_ERROR;
+    }
+    /*if (state == STATE_GOTO_SWING) {
+        state = STATE_ERROR;
     }*/
-    if (state == STATE_GOTO_SWING) {
-        state = STATE_ERROR;
-    }
 }
 
 
@@ -289,25 +298,6 @@
         newData = true;
     }
     cmdIndex = !cmdIndex;
-    /*command[0] = rpc.getc();
-    if (command[0] == 50) {
-        canSend = true;
-    }*/
-/*    command[1] = rpc.getc();
-    switch (command[0]) {
-        case 50:
-            canSend = true;
-            break;
-        case 60:
-            swingTicker.attach(changeDir, 2);
-            state = STATE_GOTO_SWING;
-            break;
-        case 70:
-            period_us = command[1];
-            break;
-        default:
-            break;    
-    } */
 }
 
 void proceedCommands() {
@@ -317,28 +307,26 @@
             canSend = true;
             break;
         case 60:
-            /*swingTicker.attach(changeDir, 2);
-            state = STATE_GOTO_SWING;*/
+            led = !led;
+            state = STATE_GOTO_SWING;
+            break;
+        case 65:
+            led = !led;
+            if (data[1] == 1) {
+                dir = DIR_RIGHT;
+            } else if (data[1] == 10) {
+                dir = DIR_LEFT;
+            }
             break;
         case 70:
             period_us = data[1];
-            control = period_us;
+            calcControl();
             break;
         default:
             break;    
     }
 }
 
-/*void govnina() {
-    if (timerCounter * timer_period_us > period_us) {
-        led = !led;
-        timerCounter = 0;
-    } else {
-        timerCounter += 1;
-    }
-}*/
-
-
 
 int main() {
     RCout = 1;
@@ -355,14 +343,15 @@
     }
     angleOffset= 3.14 - angle;
     calibrated = true;
+    led = !led;
     //wait(12);
     //wait(3);
     //updatePeriod();
-    //calcControl();
+    calcControl();
     tick.attach_us(stepperFlip, timer_period_us);
     state=STATE_GOTO_START;
     dir=DIR_LEFT;
-    swingTicker.attach(changeDir, 2);
+    //swingTicker.attach(changeDir, 1.5);
     while(1) {
         getAngularSpeed();
         if (canSend) {
@@ -385,9 +374,9 @@
             case STATE_GOTO_END_COUNTING:
                 break;
             case STATE_GOTO_SWING:
-                //speedTicker.attach(calcSpeed, 1);
+                speedTicker.attach(calcSpeed, 1);
                 //swingTicker.attach(getSwingDirectory, 1);
-                //state = STATE_SWING_LEFT;
+                state = STATE_SWING_LEFT;
                 break;
             case STATE_SWING_LEFT:
                 /*if (swingCounter == 6) {