Diff: main.cpp

Revision:

6:999e8ae7d969

Parent:

5:9aae12408a54

Child:

7:ca62dda005d5

--- a/main.cpp	Sun May 17 16:15:16 2020 +0000
+++ b/main.cpp	Mon May 18 15:18:22 2020 +0000
@@ -26,6 +26,7 @@
 Ticker tick;
 Ticker speedTicker;
 Ticker swingTicker;
+Ticker sendDataTicker;
 DigitalOut  myled(LED2);
 RawSerial rpc(D1,D0,9600);
 
@@ -172,9 +173,10 @@
 }
 
 void calcControl() {
-    float frequency = 1000000 / (period_us / 2);
-    float rates = frequency / 6400;
-    control = rates * PIPI * radius;
+    //float frequency = 1000000 / (period_us / 2);
+    //float rates = frequency / 6400;
+    //control = rates * PIPI * radius;
+    control = period_us;
 }
 
 
@@ -227,7 +229,7 @@
 }
 
 void getSwingDirectory() {
-        control = -control;
+        //control = -control;
         if (dir == DIR_RIGHT) {
             state = STATE_SWING_RIGHT;
             dir = DIR_LEFT;
@@ -242,7 +244,7 @@
 
 void sendData() {
     myUnion.number[0] = t.read();
-    myUnion.number[1] = dx;
+    myUnion.number[1] = getPosMM() / 1000;
     myUnion.number[2] = speed;
     //myUnion.number[3] = dAngle;
     myUnion.number[3] = angle;
@@ -269,14 +271,15 @@
     t.start();
     leftSwitch.rise(&leftEnd);
     rightSwitch.rise(&rightEnd);
-    calcControl();
     rpc.attach(&Rx_interrupt, Serial::RxIrq);
     for (int i=5; i>0; i--) {
         getPendulumAngle();
         wait_ms(500);
     }
     angleOffset= 3.14 - angle;
+    wait(15);
     updatePeriod();
+    calcControl();
     state=STATE_GOTO_START;
     dir=DIR_LEFT;
     while(1) {