Revision 2:405f8e1a01d3, committed 2018-02-03

Comitter:

rsmits

Date:

Sat Feb 03 15:21:25 2018 +0000

Parent:

1:0e63617e1965

Child:

3:78cf56b8eb21

Commit message:

test angle working

Changed in this revision

--- a/main.cpp	Wed Jan 17 20:43:09 2018 +0000
+++ b/main.cpp	Sat Feb 03 15:21:25 2018 +0000
@@ -6,20 +6,23 @@
 // refresh time. set to 500 for part 2 and 50 for part 4
 #define REFRESH_TIME_MS 100
 //Gravity at Earth's surface in m/s/s
-#define g0 9.812865328
-//Convert from degrees to radians.
-#define toRadians(x) (x * 0.01745329252)
+#define g0 9.812865328f
+//Convert from degrees to radians. radians = (degrees*pi)/180
+#define toRadians(x) (x * 0.01745329252f)
+//Convert from radians to degrees.
+#define toDegrees(x) (x * 57.2957795)
 
 const int   LSM6DS3_ADDRESS = 0xD4;
-int PERIOD = 20;
+float       DEGREES[3];
+float       ACCELERATIONS[3];
+float       ANGLES[3];
+//const int   LSM6DS3_ADDRESS = 0x69;
 
-//float       MOTOR_PRINT; 
 Serial      pc(SERIAL_TX, SERIAL_RX); 
-ESC         MOTOR(PA_8, PERIOD);
 AnalogIn    PRESSURE_SENSOR(PA_0);
-//Ticker      UpdateScreen;
-LSM6DS3     imu(PF_0, PF_1, LSM6DS3_ADDRESS);
-IMUfilter imuFilter(REFRESH_TIME_MS/1000, 10);
+LSM6DS3     imu(PB_9, PB_8, LSM6DS3_ADDRESS);
+IMUfilter   imuFilter(0.1, 10);
+Ticker      ScreenTicker; 
 
 //Init Serial port and LSM6DS3 chip
 void setup()
@@ -43,58 +46,64 @@
     pc.printf("LSM6DS3 WHO_AM_I's returned: 0x%X\r\n", status);
     pc.printf("Should be 0x69\r\n");
 }
+
+void screenUpdate(){
+    pc.printf("Accel (m/s^2):\r\n");
+    pc.printf("AX: %2f\r\n", ACCELERATIONS[0]);
+    pc.printf("AY: %2f\r\n", ACCELERATIONS[1]);
+    pc.printf("AZ: %2f\r\n", ACCELERATIONS[2]);
+
+    pc.printf("Gyro (degrees/s):\r\n");
+    pc.printf("GX: %f\r\n", DEGREES[0]);
+    pc.printf("GY: %f\r\n", DEGREES[1]);
+    pc.printf("GZ: %2f\r\n", DEGREES[2]);
+    
+    pc.printf("Angle (degrees):\r\n");
+    pc.printf("X: %f\r\n", ANGLES[0]);
+    pc.printf("Y: %f\r\n", ANGLES[1]);
+    pc.printf("Z: %f\r\n\r\n", ANGLES[2]); 
+}
  
 int main()
 {
-    //MOTOR.setThrottle(0.0); 
     bool started = false;
-    
     while (true){
         
-        if(PRESSURE_SENSOR > 0.9){
+        if(PRESSURE_SENSOR > 0.9f){
             
             if (not started){
                 setup();  //Setup sensor and Serial
                 pc.printf("\r\n------ LSM6DS3 Demo -----------\r\n");
                 started=true;
+                ScreenTicker.attach(&screenUpdate, 1);
             }
- 
-//            imu.readTemp();
-//            pc.printf("Temp:\r\n");
-//            pc.printf("TC: %2f\r\n", imu.temperature_c);
-//            pc.printf("TF: %2f\r\n", imu.temperature_f);
-//            
+            
+            wait_ms(REFRESH_TIME_MS);     
             imu.readAccel();
-//            pc.printf("Accel:\r\n");
-//            pc.printf("AX: %2f\r\n", imu.ax);
-//            pc.printf("AY: %2f\r\n", imu.ay);
-//            pc.printf("AZ: %2f\r\n", imu.az);
-//     
             imu.readGyro();
-//            pc.printf("Gyro:\r\n");
-//            pc.printf("GX: %2f\r\n", imu.gx);
-//            pc.printf("GY: %2f\r\n", imu.gy);
-//            pc.printf("GZ: %2f\r\n\r\n", imu.gz);
+            
             
-            // radians = (degrees*pi)/180
-            double rx = (imu.gx*PI)/180; 
-            double ry = (imu.gy*PI)/180;
-            double rz = (imu.gz*PI)/180;
+            DEGREES[0] = imu.gx;
+            DEGREES[1] = imu.gy;
+            DEGREES[2] = imu.gz;
             
             // m/(s^2) = g0*g
-            double mx = g0*imu.ax;
-            double my = g0*imu.ay;
-            double mz = g0*imu.az;
+            ACCELERATIONS[0] = g0*imu.ax;
+            ACCELERATIONS[1] = g0*imu.ay;
+            ACCELERATIONS[2] = g0*imu.az;
+                        
+            imuFilter.updateFilter(toRadians(imu.gx), toRadians(imu.gy), toRadians(imu.gz), 
+                                   g0*imu.ax, g0*imu.ay, g0*imu.az);
+            imuFilter.computeEuler();
             
-            imuFilter.updateFilter(rx, ry, rz, mx, my, mz);
-            imuFilter.computeEuler();
-            pc.printf("%f, %f, %f\r\n\r\n", imuFilter.getRoll(), imuFilter.getPitch(), imuFilter.getYaw());
-           
-            wait_ms(REFRESH_TIME_MS);
+            ANGLES[0] = toDegrees(imuFilter.getRoll());
+            ANGLES[1] = toDegrees(imuFilter.getPitch());
+            ANGLES[2] = toDegrees(imuFilter.getYaw());            
         }
         else {
             started=false;
             imuFilter.reset();
+            ScreenTicker.detach();
         }
     }
 }