Diff: main.cpp

Revision:

24:105abc3a7c72

Parent:

22:bb9b9d3b45d8

--- a/main.cpp	Thu Apr 23 16:06:03 2020 +0000
+++ b/main.cpp	Thu Apr 23 16:36:53 2020 +0000
@@ -16,6 +16,9 @@
 
 LSM9DS1 imu(p9, p10, 0xD6, 0x3C);
 
+PwmOut leftWheel(p21);
+PwmOut rightWheel(p22);
+
 
 ///////////////////////////////////////////////////////////////////////////////
 ///////////////////////////// Control System Variables/////////////////////////
@@ -121,6 +124,16 @@
     }
 }
 
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////// update motor speeds///////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+void set_wheel_speed(float speed) {
+    if(speed > 1) speed = 1;
+    if speed < -1) speed = -1;
+    leftWheel = speed;
+    rightWheel = speed;
+}
+
 
 ///////////////////////////////////////////////////////////////////////////////
 ///////////////////////////// Control System Updates///////////////////////////
@@ -130,6 +143,10 @@
     while(1){
         get_current_angle();
         //pc.printf("this is running 100");
+        angleMutex.lock();
+        speed = -1*(rp*pAngle + ri*iAngle + rd*dAngle)/150;
+        set_wheel_speed(speed); 
+        angleMutex.unlock();
         Thread::wait(10);
     }
 }
@@ -155,7 +172,10 @@
     //int acc = (-1)*atan2(imu.ax,imu.az)*180/3.142-90-angleBiasAcc;
     //pc.printf("gyro:%f",gyro);
     angleMutex.lock();
-    pAngle += 1*gyro //+ 0.02*acc;
+    dAngle = pAngle;
+    pAngle += 1*gyro; //+ 0.02*acc;
+    dAngle = pAngle - dAngle;
+    iAngle += pAngle * 0.01;
     angleMutex.unlock();
 }
 
@@ -188,6 +208,7 @@
         
         angleMutex.lock();
         pc.printf("pAngle: %f", pAngle);
+        pc.printf("speed: %f", speed);
         angleMutex.unlock();