Diff: main.cpp

Revision:

9:e8c6a414e226

Parent:

8:b3ce040fdebc

Child:

10:ca38e4d775c8

--- a/main.cpp	Wed May 20 15:07:31 2020 +0000
+++ b/main.cpp	Thu May 21 15:35:04 2020 +0000
@@ -18,7 +18,6 @@
 #define DIR_LEFT 0
 #define DIR_RIGHT 1
 
-//Serial pc(D1,D0,4800);
 DigitalOut RCout(D10);
 DigitalOut dir(D9);
 InterruptIn leftSwitch(D2);
@@ -74,6 +73,7 @@
 bool canSend = false;
 bool periodUpdated = false;
 bool calibrated = false;
+bool dirUpdated = false;
 
 typedef union {
     float number[6];
@@ -116,8 +116,7 @@
         if (angle < -3.14) {
             angle += PIPI;
         }
-    }
-    */
+    }*/
     /*if (calibrated) {
         float test = fmodf(angle, M_PI);
         if (test >= 1) {
@@ -162,27 +161,6 @@
     SPEED CALC
 */
 
-/*void getDeltaPos()
-{
-    float delta = 0;
-    xPosNew = getPosMM();
-    delta = xPosNew - xPosOld;
-    dx = delta;
-    xPosOld = xPosNew;
-}*/
-
-/*float getSpeed(){
-    float deltaTime;
-    float speed;
-    getDeltaPos();
-    timeStartPos = float(t.read());
-    deltaTime = (timeStartPos - timeOldPos);
-    speed = (dx) * deltaTime;
-    timeOldPos = timeStartPos;
-    //взятие по модулю, спросить
-    return speed;
-}*/
-
 void calcSpeed() {
     /*posMap[posCounter] = getPosMM() / 1000;
     if (posCounter == 3) {
@@ -205,6 +183,7 @@
     if (dir == DIR_LEFT) {
         speed = -speed;    
     }
+    //speed = -speed;
     
     xPosOld = xPosNew;
 }
@@ -241,6 +220,7 @@
 void updatePeriod(){
     tick.detach();
     tick.attach_us (&stepperFlip, period_us / 2.0f);
+    wait_ms(20);
 }
 
 void leftEnd() {
@@ -250,11 +230,13 @@
         pos = 0;
     }
     else if (isPendulumSwinging()) {
-        state = STATE_GOTO_MIDDLE;
+        //state = STATE_GOTO_MIDDLE;
         //angleOffset -= 0.006191;
-        //state = STATE_ERROR;
+        state = STATE_ERROR;
     }
-    //при втыкании в концевик меняем смещение (offset)
+    if (state == STATE_GOTO_SWING) {
+        state = STATE_ERROR;
+    }
 }
 
 void rightEnd() {
@@ -264,13 +246,18 @@
         state = STATE_GOTO_MIDDLE;
     }
     else if (isPendulumSwinging()) {
-        state = STATE_GOTO_MIDDLE;
+        //state = STATE_GOTO_MIDDLE;
         //angleOffset += 0.006191;
-        //state = STATE_ERROR;
+        state = STATE_ERROR;
+    }
+    if (state == STATE_GOTO_SWING) {
+        state = STATE_ERROR;
     }
 }
 
+int swingCounter = 0;
 void getSwingDirectory() {
+        swingCounter += 1;
         control = -control;
         if (dir == DIR_RIGHT) {
             state = STATE_SWING_RIGHT;
@@ -310,16 +297,20 @@
         case 65:
             int direction = rpc.getc();
             if (direction <= 0) {
+                if (dir != DIR_RIGHT) {
                 dir = DIR_RIGHT;
+                }
             } else if (direction > 0) {
-                dir = DIR_LEFT;    
+                if (dir != DIR_LEFT) {
+                    dir = DIR_LEFT;    
+                }
             }
             break;
         case 70:
             int newPeriod = rpc.getc();
             if (newPeriod < 26) {
                 tick.detach();
-            } else {
+            } else if (period_us != newPeriod) {
                 period_us = newPeriod;
                 periodUpdated = true;
             }
@@ -356,11 +347,11 @@
         if (canSend) {
             sendData();
         }
-        if (periodUpdated) {
+        /*if (periodUpdated) {
             calcControl();
             updatePeriod();
             periodUpdated = false;
-        }
+        }*/
         switch(state) {
             case STATE_WAITING:
                 //state = STATE_GOTO_SWING;
@@ -370,16 +361,27 @@
             case STATE_GOTO_END_COUNTING:
                 break;
             case STATE_GOTO_SWING:
+                //speedTicker.attach(calcSpeed, 1);
                 //swingTicker.attach(getSwingDirectory, 1);
                 //state = STATE_SWING_LEFT;
                 break;
             case STATE_SWING_LEFT:
+                /*if (swingCounter == 6) {
+                    swingCounter = 0;
+                    period_us += 4;
+                    periodUpdated = true;
+                }*/
                 break;
             case STATE_SWING_RIGHT:
+                /*if (swingCounter == 6) {
+                    swingCounter = 0;
+                    period_us += 4;
+                    periodUpdated = true;
+                }*/
                 break;
             default:
                 break;
         }
-        //wait_ms(5);
+        wait_ms(100);
     }
 }
\ No newline at end of file