Diff: main.cpp

Revision:

12:ee175985ef09

Parent:

11:6bbe8da4b16a

Child:

13:205002e3c176

--- a/main.cpp	Fri May 22 13:27:01 2020 +0000
+++ b/main.cpp	Sat May 23 15:24:09 2020 +0000
@@ -3,6 +3,7 @@
 #define M_PI 3.14159265358979323846f
 #define smoothingNumber 6
 
+
 #define STATE_ERROR 0
 #define STATE_INIT 1
 #define STATE_GOTO_START 2
@@ -18,26 +19,30 @@
 #define DIR_LEFT 0
 #define DIR_RIGHT 1
 
+#define timer_period_us 10
+
+
 DigitalOut RCout(D10);
 DigitalOut dir(D9);
 InterruptIn leftSwitch(D2);
 InterruptIn rightSwitch(D3);
 Ticker tick;
-Ticker speedTicker;
+//Ticker speedTicker;
 Ticker swingTicker;
-Ticker sendDataTicker;
-DigitalOut  myled(LED2);
 RawSerial rpc(D1,D0,9600);
-
+DigitalOut led(D4);
 
-int period_us = 22;//300 26
+int period_us = 1;//300 26
 int posCounter = 0;
+int timerCounter = 0;
 
 long pos=0;
 
 long railLength = 0;
 
 uint8_t state=STATE_INIT;
+uint8_t data[2] = {0, 0};
+uint8_t cmdIndex = 0;
 
 Timer t;
 SPI spi(D11,D12,D13);// mosi, miso, sclk
@@ -76,11 +81,6 @@
 bool calibrated = false;
 bool dirUpdated = false;
 
-
-DigitalOut led(LED1);
-Timeout flipper;
-
-
 typedef union {
     float number[6];
     uint8_t numberCh[24];
@@ -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,7 @@
     SPEED CALC
 */
 
-void calcSpeed() {
+/*void calcSpeed() {
     /*posMap[posCounter] = getPosMM() / 1000;
     if (posCounter == 3) {
         posCounter = 0;
@@ -184,17 +184,8 @@
     } else {
         posCounter += 1;
     }*/
-    xPosNew = (getPosMM() - posOffset) / 1000;
-    speed = xPosNew - xPosOld;
-    if (dir == DIR_LEFT) {
-        speed = -speed;    
-    }
-    //speed = -speed;
-    
-    xPosOld = xPosNew;
-}
 
-void calcControl() {
+/*void calcControl() {
     //float frequency = 1000000 / (period_us / 2);
     //float rates = frequency / 6400;
     //control = rates * PIPI * radius;
@@ -203,31 +194,36 @@
     } else {
         control = period_us;
     }
-}
+}*/
 
 
 
 void stepperFlip() {
-    if (state != STATE_WAITING && state != STATE_ERROR && state != STATE_INIT && state){
-        RCout = !RCout;
-        pos += (dir.read() * 2 - 1);
+    if (timerCounter * timer_period_us > period_us) {
+        timerCounter = 0;
+        if (state != STATE_WAITING && state != STATE_ERROR && state != STATE_INIT && state){
+            RCout = !RCout;
+            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) {
+            pos += (dir.read() * 2 - 1);
+            RCout = !RCout;
+        }
+    } else {
+        timerCounter += 1;
     }
-    if (state == STATE_GOTO_MIDDLE && pos == railLength / 2) {
-        posOffset = 40;
-        state = STATE_WAITING;
-    }
-   if (state == STATE_SWING_LEFT && state==STATE_SWING_RIGHT) {
-        pos += (dir.read() * 2 - 1);
-        RCout = !RCout;
-    }
-    flipper.attach_us(stepperFlip, period_us/2);    
 }
 
 
-void updatePeriod(){
-    //tick.detach();
+/*void updatePeriod(){
+    tick.detach();
     tick.attach_us (&stepperFlip, period_us / 2.0f);
-}
+    wait_ms(20);
+}*/
 
 void leftEnd() {
     dir = DIR_RIGHT;
@@ -235,11 +231,11 @@
         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;
     }
@@ -251,31 +247,16 @@
         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;
     }
 }
 
-int swingCounter = 0;
-void getSwingDirectory() {
-        swingCounter += 1;
-        control = -control;
-        if (dir == DIR_RIGHT) {
-            state = STATE_SWING_RIGHT;
-            dir = DIR_LEFT;
-            return;
-        }
-        if (dir == DIR_LEFT) {
-            state = STATE_SWING_LEFT;
-            dir = DIR_RIGHT;
-            return;
-        }
-}
 
 void sendData() {
     myUnion.number[0] = t.read();
@@ -290,52 +271,75 @@
     }
 }
 
-int dirCounter = 0;
-int perCounter = 0;
 
+void changeDir() {
+    if (dir == DIR_LEFT) {
+        dir = DIR_RIGHT;    
+    } else {
+        dir = DIR_LEFT;
+    }    
+}
+
+int command[2] = {0, 0};
+bool newData = false;
 
 void Rx_interrupt() {
-    int command = rpc.getc();
-    switch (command) {
+    data[cmdIndex] = rpc.getc();
+    if (cmdIndex == 1) {
+        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;
-            speedTicker.attach(calcSpeed, 1);
-            break;
-        case 65:
-            int direction = rpc.getc();
-            if (direction <= 0) {
-                if (dir != DIR_RIGHT) {
-                    dir = DIR_RIGHT;
-                }
-            } else if (direction > 0) {
-                if (dir != DIR_LEFT) {
-                    dir = DIR_LEFT;    
-                }
-            }
             break;
         case 70:
-            /*int newPeriod = rpc.getc();
-            if (newPeriod < 22) {
-                flipper.detach();
-            } else if (period_us != newPeriod) {
-                period_us = newPeriod;
-                periodUpdated = true;
-            }*/
+            period_us = command[1];
             break;
         default:
-            break;
-    }
-    return;
+            break;    
+    } */
 }
 
-/*void flipLed()
-{
-    flipper.attach_us(stepperFlip, period_us);    
+void proceedCommands() {
+    newData = false;
+    switch (data[0]) {
+        case 50:
+            canSend = true;
+            break;
+        case 60:
+            /*swingTicker.attach(changeDir, 2);
+            state = STATE_GOTO_SWING;*/
+            break;
+        case 70:
+            period_us = data[1];
+            control = period_us;
+            break;
+        default:
+            break;    
+    }
+}
+
+/*void govnina() {
+    if (timerCounter * timer_period_us > period_us) {
+        led = !led;
+        timerCounter = 0;
+    } else {
+        timerCounter += 1;
+    }
 }*/
 
+
+
 int main() {
     RCout = 1;
     wait_ms(500);
@@ -352,23 +356,26 @@
     angleOffset= 3.14 - angle;
     calibrated = true;
     //wait(12);
-    wait(7);
+    //wait(3);
     //updatePeriod();
-    calcControl();
+    //calcControl();
+    tick.attach_us(stepperFlip, timer_period_us);
     state=STATE_GOTO_START;
     dir=DIR_LEFT;
-    flipper.attach_us(stepperFlip, period_us/2);
+    swingTicker.attach(changeDir, 2);
     while(1) {
         getAngularSpeed();
         if (canSend) {
             sendData();
         }
-        if (periodUpdated) {
+        if (newData) {
+            proceedCommands();    
+        }
+        /*if (periodUpdated) {
             calcControl();
-           // updatePeriod();
-            //flipper.attach_us(stepperFlip, period_us/2);
+            updatePeriod();
             periodUpdated = false;
-        }
+        }*/
         switch(state) {
             case STATE_WAITING:
                 //state = STATE_GOTO_SWING;
@@ -399,6 +406,5 @@
             default:
                 break;
         }
-        //wait_ms(100);
     }
 }
\ No newline at end of file