Diff: main.cpp

Revision:

4:fd49edfabfb2

Parent:

3:8708e61475fe

Child:

5:9aae12408a54

--- a/main.cpp	Sat May 16 12:31:09 2020 +0000
+++ b/main.cpp	Sun May 17 16:13:43 2020 +0000
@@ -24,14 +24,13 @@
 InterruptIn leftSwitch(D2);
 InterruptIn rightSwitch(D3);
 Ticker tick;
-Ticker ledTicker;
+Ticker speedTicker;
+Ticker swingTicker;
 DigitalOut  myled(LED2);
 RawSerial rpc(D1,D0,9600);
 
 int period_us = 26;//300 26
 
-bool isSwinged = false;
-
 long pos=0;
 
 long railLength = 0;
@@ -42,7 +41,7 @@
 SPI spi(D11,D12,D13);// mosi, miso, sclk
 DigitalOut cs(D5);
 
-int driveSpeed = 0;
+float radius = 0.0025;
 
 float angularPosNew = 0;
 float angularPosOld = 0;
@@ -53,6 +52,7 @@
 float dx = 0;
 float xPosNew = 0;
 float xPosOld = 0;
+float speed = 0;
 
 double dAngle = 0.0f;
 double anSpd = 0.0f;
@@ -60,16 +60,18 @@
 float timeOldPos = 0.0f;
 float timeStartPos = 0.0f;
 
-float angle=0.f;
+float angle=0.0f;
 float angleOffset = 0;
 
+float control = 0.0f;
+
 double PIPI = 6.28;
 
 bool canSend = false;
 
 typedef union {
-    float number[5];
-    uint8_t numberCh[20];
+    float number[6];
+    uint8_t numberCh[24];
 } my_union;
 
 my_union myUnion;
@@ -99,6 +101,13 @@
 float getPendulumAngle(){
     angle = getPendulumPos();
     angle = angle * 6.28f / 1024.0f;
+    angle += angleOffset;
+    if (angle > PIPI + 0.01) {
+        angle = fmod((angle + 3.14), PIPI) - 3.14;
+        if (angle < -3.14) {
+            angle += PIPI;
+        }
+    }
     return angle;
 }
 
@@ -144,7 +153,7 @@
     xPosOld = xPosNew;
 }
 
-float getSpeed(){
+/*float getSpeed(){
     float deltaTime;
     float speed;
     getDeltaPos();
@@ -154,6 +163,18 @@
     timeOldPos = timeStartPos;
     //взятие по модулю, спросить
     return speed;
+}*/
+
+void calcSpeed() {
+    xPosNew = getPosMM() / 1000;
+    speed = xPosNew - xPosOld;
+    xPosOld = xPosNew;
+}
+
+void calcControl() {
+    float frequency = 1000000 / (period_us / 2);
+    float rates = frequency / 6400;
+    control = rates * PIPI * radius;
 }
 
 
@@ -173,16 +194,6 @@
 }
 
 
-
-void led() {
-    myled = !myled;
-}
-
-void updateBlink(uint64_t t){
-    ledTicker.detach();
-    ledTicker.attach_us (&led, t / 2);
-}
-
 void updatePeriod(){
     tick.detach();
     tick.attach_us (&stepperFlip, period_us / 2.0f);
@@ -216,36 +227,35 @@
 }
 
 void getSwingDirectory() {
-        int currentSpd = anSpd;
-        if (currentSpd > 0 ) {
+        control = -control;
+        if (dir == DIR_RIGHT) {
             state = STATE_SWING_RIGHT;
             dir = DIR_LEFT;
-            //dir = DIR_LEFT;
             return;
         }
-        if (currentSpd < 0) {
+        if (dir == DIR_LEFT) {
             state = STATE_SWING_LEFT;
             dir = DIR_RIGHT;
-            //dir = DIR_RIGHT;
             return;
         }
 }
 
 void sendData() {
-    getAngularSpeed();
     myUnion.number[0] = t.read();
     myUnion.number[1] = dx;
-    myUnion.number[2] = getSpeed();
-    myUnion.number[3] = dAngle;
+    myUnion.number[2] = speed;
+    //myUnion.number[3] = dAngle;
+    myUnion.number[3] = angle;
     myUnion.number[4] = anSpd;
-    for(int i = 0; i < 20; i++) {
+    myUnion.number[5] = control;
+    for(int i = 0; i < 24; i++) {
         rpc.putc(myUnion.numberCh[i]);
     }
 }
 
 void Rx_interrupt() {
-    int password = rpc.getc();
-    if (password == 228) {
+    int command = rpc.getc();
+    if (command == 50) {
         canSend = true;
     }
     return;
@@ -259,22 +269,19 @@
     t.start();
     leftSwitch.rise(&leftEnd);
     rightSwitch.rise(&rightEnd);
+    rpc.attach(&Rx_interrupt, Serial::RxIrq);
     for (int i=5; i>0; i--) {
         getPendulumAngle();
-        myled = !myled;
         wait_ms(500);
     }
-    angleOffset=angle;
+    angleOffset= 3.14 - angle;
     updatePeriod();
     state=STATE_GOTO_START;
     dir=DIR_LEFT;
-    rpc.attach(&Rx_interrupt, Serial::RxIrq);
     while(1) {
+        getAngularSpeed();
         if(canSend) {
             sendData();
-        } else {
-            getSpeed();
-            getAngularSpeed();
         }
         switch(state) {
             case STATE_WAITING:
@@ -285,6 +292,9 @@
             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:
                 break;