Mukund Pandiri / Mbed 2 deprecated segway-balancing

self-balancing-robot

Dependencies:   mbed mbed-rtos Motor

Diff: main.cpp

Revision:
27:d053966e9320
Parent:
26:dcf173d2904f
Child:
28:31ae3dd720e1
diff -r dcf173d2904f -r d053966e9320 main.cpp
--- a/main.cpp	Thu Apr 23 16:46:13 2020 +0000
+++ b/main.cpp	Thu Apr 23 19:31:24 2020 +0000
@@ -144,7 +144,7 @@
         get_current_angle();
         
         angleMutex.lock();
-        speed = -1*(rp*pAngle + ri*iAngle + rd*dAngle)/150;
+        speed = -1*(rp*pAngle + ri*iAngle + rd*dAngle)/(70 * 150 * 3);
         set_wheel_speed(speed); 
         angleMutex.unlock();
         
@@ -171,7 +171,15 @@
     int gyro = -(imu.gy-angleBias) * .01;
     //pc.printf("gyro:%f",gyro);
     angleMutex.lock();
-    pAngle += gyro;
+    dAngle = pAngle;
+    
+    pAngle += 1*gyro; //+ 0.02*acc;
+    pAngle -= desired_angle*70;
+    
+    dAngle = pAngle - dAngle;
+    
+    iAngle += pAngle * 0.01;
+    
     angleMutex.unlock();
 }