Diff: main.cpp

Revision:

16:f9e3df933304

Parent:

15:f0f19572c4a5

Child:

17:afde478daa01

--- a/main.cpp	Thu Apr 23 15:12:58 2020 +0000
+++ b/main.cpp	Thu Apr 23 15:27:21 2020 +0000
@@ -33,6 +33,7 @@
 
 int time_segment = 5;                        //Update the speed every 5 milliseconds
 
+void get_current_angle();
 
 
 ///////////////////////////////////////////////////////////////////////////////
@@ -123,8 +124,11 @@
 ///////////////////////////////////////////////////////////////////////////////
 //make the calls to IMU here and this should be another thread
 void update_system() {
-    get_current_angle();
-    Thread::wait(10);
+    while(1){
+        get_current_angle();
+        //pc.printf("this is running 100");
+        Thread::wait(10);
+    }
 }
 
 
@@ -139,6 +143,7 @@
 //    return;
     imu.readGyro();
     int gyro = -imu.gy * .01;
+    //pc.printf("gyro:%f",gyro);
     angleMutex.lock();
     pAngle += gyro;
     angleMutex.unlock();
@@ -163,17 +168,18 @@
     
     //bluetooth.attach(&bluetooth_update, 0.1);
     while (1) {
+        
         //bluetooth_update();
-        parametersmutex.lock();
-        pc.printf("rp: %f, rd: %f, ri: %f, desired_angle: %f\n", rp, rd, ri, desired_angle);
-        parametersmutex.unlock();
+        //parametersmutex.lock();
+//        pc.printf("rp: %f, rd: %f, ri: %f, desired_angle: %f\n", rp, rd, ri, desired_angle);
+//        parametersmutex.unlock();
         
         angleMutex.lock();
         pc.printf("pAngle: %f", pAngle);
         angleMutex.unlock();
         
         
-        get_current_angle();
+        //get_current_angle();
         
         
         Thread::wait(100);