Diff: main.cpp

Revision:

14:89bb16d03b15

Parent:

13:c00ddcfea79f

--- a/main.cpp	Sat Apr 18 20:49:31 2020 +0000
+++ b/main.cpp	Sat Apr 18 21:10:35 2020 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include "rtos.h"
+#include "LSM9DS1.h"
 
 
 ///////////////////////////////////////////////////////////////////////////////
@@ -11,6 +12,9 @@
 Mutex parametersmutex;
 Serial blue(p28, p27);
 
+LSM9D1 imu(p9, p10, 0xD6, 0x3C);
+
+
 ///////////////////////////////////////////////////////////////////////////////
 ///////////////////////////// Control System Variables/////////////////////////
 ///////////////////////////////////////////////////////////////////////////////
@@ -19,6 +23,14 @@
 float ri = 50;
 float desired_angle = 0;
 
+float speed = 0;
+
+float pAngle = 0;
+float dAngle = 0;
+float iAngle = 0;
+
+time_segment = 5;                        //Update the speed every 5 milliseconds
+
 
 
 ///////////////////////////////////////////////////////////////////////////////
@@ -122,7 +134,7 @@
 }
 
 void get_current_angle() {
-    return;
+    imu.readGyro();
 }
 
 
@@ -136,14 +148,22 @@
     pc.printf("this is running");
     Thread bluetooth;
     Thread system_update;
+    
+    imu.begin();
+    
     bluetooth.start(bluetooth_update);
     system_update.start(update_system);
+    
     //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);
+//        pc.printf("rp: %f, rd: %f, ri: %f, desired_angle: %f\n", rp, rd, ri, desired_angle);
         parametersmutex.unlock();
+        
+        get_current_angle();
+        pc.printf("gyro: %d %d %d\n\r", imu.gx, imu.gy, imu.gz);
+        
         Thread::wait(100);
     }
 }