Diff: main.cpp

Revision:

26:dcf173d2904f

Parent:

25:154c74800ade

Child:

27:d053966e9320

--- a/main.cpp	Thu Apr 23 16:42:44 2020 +0000
+++ b/main.cpp	Thu Apr 23 16:46:13 2020 +0000
@@ -37,6 +37,9 @@
 
 float angleBias = 0;
 
+PwmOut leftWheel(p21);
+PwmOut rightWheel(p22);
+
 
 ///////////////////////////////////////////////////////////////////////////////
 ///////////////////////////// Bluetooth Section ///////////////////////////////
@@ -140,6 +143,11 @@
     while(1){
         get_current_angle();
         
+        angleMutex.lock();
+        speed = -1*(rp*pAngle + ri*iAngle + rd*dAngle)/150;
+        set_wheel_speed(speed); 
+        angleMutex.unlock();
+        
         //pc.printf("this is running 100");
         Thread::wait(10);
     }
@@ -195,6 +203,7 @@
         
         angleMutex.lock();
         pc.printf("pAngle: %f", pAngle);
+        pc.printf("speed: %f", speed);
         angleMutex.unlock();