Diff: main.cpp

Revision:

1:85ab39fea863

Parent:

0:1cc40073c858

--- a/main.cpp	Sat May 16 12:36:40 2020 +0000
+++ b/main.cpp	Sat May 16 15:08:46 2020 +0000
@@ -21,6 +21,8 @@
 
 float timeOld = 0.0f;
 float timeStart = 0.0f;
+float proceeded = 0.0f;
+float test= 0.0f;
 
 void Rx_interrupt();
 
@@ -54,12 +56,30 @@
     angle = getPendulumPos();
     angle = angle * 6.28f / 1024.0f;
     beforeOffset = angle;
-    if (angle >= angleOffset) {
-        angle -= angleOffset;
+    angle += angleOffset;
+    /*if (angle >= angleOffset) {
+        angle += angleOffset;
     } else {
         //angle += (angleOffset + 0.244346);
-        angle += angleOffset + 0.23970866018;
+        //angle += angleOffset + 0.23970866018;
+        //angle += angleOffset;
+        angle -= angleOffset;
+    }*/
+    if (angle > PIPI + 0.01) {
+        proceeded = fmod((angle + 3.14), PIPI) - 3.14;
+        if (proceeded < -3.14) {
+            proceeded += PIPI;
+        }
+/*        if (proceeded < 0) {
+            proceeded = -proceeded;
+        }*/
+        angle = proceeded;
     }
+    test = fmodf(angle,M_PI);
+    if (test > 1) {
+        test = -test;    
+    }
+
     return angle;
 }
 
@@ -132,7 +152,7 @@
     myUnion.number[2] = beforeOffset;
     myUnion.number[3] = 228.0;
     myUnion.number[4] = angularPosNew;
-    myUnion.number[5] = anSpd;
+    myUnion.number[5] = test;
     for(int i = 0 ; i < 24; i++) {
         rpc.putc(myUnion.numberCh[i]);    
     }
@@ -148,7 +168,7 @@
         getPendulumAngle();
         wait_ms(500);
     }
-    angleOffset=angle;
+    angleOffset= 3.14 - angle;
     rpc.attach(&Rx_interrupt, Serial::RxIrq);
     while(1) {
     wait_us(5);