Diff: main.cpp

Revision:

2:2c0cd1aaae83

Parent:

1:de34af25056a

Child:

3:8b5700499eb8

--- a/main.cpp	Tue Aug 24 15:15:44 2010 +0000
+++ b/main.cpp	Mon Oct 05 19:35:04 2015 +0000
@@ -1,9 +1,37 @@
 #include "mbed.h"
-#include "rpc.h"
-Serial pc(USBTX, USBRX);
+
+//#include "SerialRPCInterface.h"
+
+#include "MPU6050.h"
+
+//SerialRPCInterface SerialRPC(USBTX, USBRX, 115200);
+
+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");
+
+MPU6050 mpu6050;
+
+DigitalOut init_done(LED1);
+DigitalOut imu_good(LED2);
+DigitalOut main_loop(LED3);
+
 int main() {
+    init_done = 0;
+    imu_good = 0;
+    main_loop = 0;
+    
     // setup the classes that can be created dynamically
-    Base::add_rpc_class<AnalogIn>();
+    /*Base::add_rpc_class<AnalogIn>();
     Base::add_rpc_class<AnalogOut>();
     Base::add_rpc_class<DigitalIn>();
     Base::add_rpc_class<DigitalOut>();
@@ -14,12 +42,62 @@
     Base::add_rpc_class<BusOut>();
     Base::add_rpc_class<BusIn>();
     Base::add_rpc_class<BusInOut>();
-    Base::add_rpc_class<Serial>();
-    // receive commands, and send back the responses
-    char buf[256], outbuf[256];
+    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;
+    
+    uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);
+    
+    if (whoami == 0x68) // WHO_AM_I should always be 0x68
+    {
+        mpu6050.MPU6050SelfTest(SelfTest);
+        if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
+            mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
+            mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
+            mpu6050.initMPU6050();
+            mpu6050.getAres();
+            mpu6050.getGres();
+            imu_ready = true;
+            imu_good = 1;
+        }
+    }
+    
+    init_done = 1;
+    
     while(1) {
-        pc.gets(buf, 256);
-        rpc(buf, outbuf); 
-        pc.printf("%s\n", outbuf);
+        wait_ms(100);
+        
+        main_loop = !main_loop;
+        
+        if (imu_ready) {    
+            imu.accel_x = imu.accel_x + 0.1;
+            
+            /*
+            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];  
+               
+                // 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];
+            }
+            */
+        }
     }
 }