Diff: main.cpp

Revision:

3:8b5700499eb8

Parent:

2:2c0cd1aaae83

--- a/main.cpp	Mon Oct 05 19:35:04 2015 +0000
+++ b/main.cpp	Tue Oct 06 17:25:53 2015 +0000
@@ -1,23 +1,17 @@
 #include "mbed.h"
-
-//#include "SerialRPCInterface.h"
-
+#include "SerialRPCInterface.h"
 #include "MPU6050.h"
 
-//SerialRPCInterface SerialRPC(USBTX, USBRX, 115200);
+SerialRPCInterface SerialRPC(USBTX, USBRX, 115200);
+
+float accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z;
 
-typedef struct imu_struct_t{
-    float accel_x;
-    float accel_y;
-    float accel_z;
-    float gyro_x;
-    float gyro_y;
-    float gyro_z;
-} imu_struct_t;
-
-imu_struct_t imu;
-
-//RPCVariable<imu_struct_t> rpc_imu(&imu, "imu");
+RPCVariable<float> rpc_accel_x(&accel_x, "accel_x");
+RPCVariable<float> rpc_accel_y(&accel_y, "accel_y");
+RPCVariable<float> rpc_accel_z(&accel_z, "accel_z");
+RPCVariable<float> rpc_gryo_x(&gyro_x, "gyro_x");
+RPCVariable<float> rpc_gryo_y(&gyro_y, "gyro_y");
+RPCVariable<float> rpc_gryo_z(&gyro_z, "gyro_z");
 
 MPU6050 mpu6050;
 
@@ -30,31 +24,12 @@
     imu_good = 0;
     main_loop = 0;
     
-    // setup the classes that can be created dynamically
-    /*Base::add_rpc_class<AnalogIn>();
-    Base::add_rpc_class<AnalogOut>();
-    Base::add_rpc_class<DigitalIn>();
-    Base::add_rpc_class<DigitalOut>();
-    Base::add_rpc_class<DigitalInOut>();
-    Base::add_rpc_class<PwmOut>();
-    Base::add_rpc_class<Timer>();
-    Base::add_rpc_class<SPI>();
-    Base::add_rpc_class<BusOut>();
-    Base::add_rpc_class<BusIn>();
-    Base::add_rpc_class<BusInOut>();
-    Base::add_rpc_class<Serial>();*/
-    
-    imu.accel_x = 0.0;
-    imu.accel_y = 0.0;
-    imu.accel_z = 0.0;
-    imu.gyro_x = 0.0;
-    imu.gyro_y = 0.0;
-    imu.gyro_z = 0.0;
-    
     //Set up I2C
     i2c.frequency(400000);  // use fast (400 kHz) I2C
     
-    bool imu_ready = false;
+    volatile bool imu_ready = false;
+    
+    wait_ms(100);
     
     uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);
     
@@ -73,31 +48,31 @@
     }
     
     init_done = 1;
-    
+    uint8_t loop_count = 10;
     while(1) {
-        wait_ms(100);
+        wait_ms(10);
         
-        main_loop = !main_loop;
+        if (!(--loop_count)) {
+            loop_count = 10;
+            main_loop = !main_loop;
+        }
         
-        if (imu_ready) {    
-            imu.accel_x = imu.accel_x + 0.1;
+        if (imu_ready) {
             
-            /*
             if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
                 mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
                 mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
 
                 // Now we'll calculate the accleration value into actual g's
-                ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
-                ay = (float)accelCount[1]*aRes - accelBias[1];   
-                az = (float)accelCount[2]*aRes - accelBias[2];  
+                accel_x = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
+                accel_y = (float)accelCount[1]*aRes - accelBias[1];   
+                accel_z = (float)accelCount[2]*aRes - accelBias[2];  
                
                 // Calculate the gyro value into actual degrees per second
-                gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
-                gy = (float)gyroCount[1]*gRes - gyroBias[1];  
-                gz = (float)gyroCount[2]*gRes - gyroBias[2];
+                gyro_x = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
+                gyro_y = (float)gyroCount[1]*gRes - gyroBias[1];  
+                gyro_z = (float)gyroCount[2]*gRes - gyroBias[2];
             }
-            */
         }
     }
 }