Added mpu6050 code

comm.c

@@ -2,13 +2,13 @@
 
 void Comm_Init()
 {
-    putsUSART("AT\r\n"); //Test Connection
+    putsUSART((char*) "AT\r\n"); //Test Connection
     Comm_WaitsOK();
-    putsUSART("AT+NAME=RmGestCtrl\r\n"); // Set device name
+    putsUSART((char*) "AT+NAME=RmGestCtrl\r\n"); // Set device name
     Comm_WaitsOK();
-    putsUSART("AT+ROLE=1\r\n"); // Set to master mode
+    putsUSART((char*) "AT+ROLE=1\r\n"); // Set to master mode
     Comm_WaitsOK();
-    putsUSART("AT+CMOD=1\r\n"); // Set connection mode to any address
+    putsUSART((char*) "AT+CMOD=1\r\n"); // Set connection mode to any address
     Comm_WaitsOK();
 
     //putsUSART("AT+CMOD=0\r\n"); // Set connection mode to fixed address
@@ -19,7 +19,7 @@ void Comm_Init()
 void Comm_Send(char command)
 {
     char c[2];
-    sprintf("%c0\r\n", command);
+    sprintf((char*) "%c0\r\n", (const char*) &command);
     putsUSART(c);
 }
 

main.c

@@ -61,67 +61,25 @@
 //</editor-fold>
 
 #include "system.h"
-#include "accel.h"
-#include "gyros.h"
 #include "comm.h"
+#include "mpu6050.h"
 
-byte App_SendGyrosData(byte lastState, vec3f* accel, rotation* rot);
+void App_RunTasks();
 
 void main() {
     System_Init();
-    Accel_Init();
+    MPU_Init();
     //Comm_Init();
 
-    vec3f accel;
-    rotation rot;
-    byte state = GYROS_UNKNOWN;
-
     while (1) {
-       Accel_Get(&accel);
-       Gyros_CalculateRotation(&accel, &rot);
-       state = App_SendGyrosData(state, &accel, &rot);
-
-       __delay_us(500);
+        App_RunTasks();
     }
 }
 
-byte App_SendGyrosData(byte lastState, vec3f* accel, rotation* rot)
+void App_RunTasks()
 {
-    byte state = Gyros_GetState(lastState, accel, rot);
-
-    if (state == GYROS_UP) {
-        WHITE_LED = 1;
-        GREEN_LED = 0;
-        YELLOW_LED = 0;
-        RED_LED = 0;
-        //Comm_Send(OBJ_GO);
-    } else if (state == GYROS_RIGHT) {
-        WHITE_LED = 0;
-        GREEN_LED = 0;
-        YELLOW_LED = 0;
-        RED_LED = 1; 
-        //Comm_Send(OBJ_TURN_RIGHT);
-    } else if (state == GYROS_LEFT) {
-        WHITE_LED = 0;
-        GREEN_LED = 0;
-        YELLOW_LED = 1;
-        RED_LED = 0;
-        //Comm_Send(OBJ_TURN_LEFT);
-    } else if (state == GYROS_DOWN) {
-        WHITE_LED = 0;
-        GREEN_LED = 1;
-        YELLOW_LED = 0;
-        RED_LED = 0;
-        //Comm_Send(OBJ_GO_BACK);
-    } else {
-        WHITE_LED = 0;
-        GREEN_LED = 0;
-        YELLOW_LED = 0;
-        RED_LED = 0;
-
-        //if (state == GYROS_DROP)
-        //  Comm_Send(OBJ_STOP);
-    }
+    vec3 gyro;
+    MPU_GetGyro(&gyro);
 
-    return state;
+    __delay_us(300);
 }

mpu6050.c

@@ -0,0 +1,102 @@
+#include "mpu6050.h"
+
+int MPU_Init()
+{
+    // Check MPU6050 Who Am I Register
+    if (MPU_ReadRegister(MPU6050_REG_WHO_AM_I) != 0x68)
+        return 0;
+
+    // Set Clock Source
+    MPU_SetClockSource(MPU6050_CLOCK_PLL_XGYRO);
+
+    // Disable Sleep Mode
+    MPU_SetSleepEnabled(0);
+
+    return 1;
+}
+
+void MPU_SetClockSource(mpu6050_clockSource_t source)
+{
+    byte value;
+    value = MPU_ReadRegister(MPU6050_REG_PWR_MGMT_1);
+    value &= 0b11111000;
+    value |= source;
+    MPU_WriteRegister(MPU6050_REG_PWR_MGMT_1, value);
+}
+
+void MPU_SetSleepEnabled(byte state)
+{
+    MPU_WriteRegisterBit(MPU6050_REG_PWR_MGMT_1, 6, state);
+}
+
+void MPU_SetGyroOffsetX(int offset)
+{
+    MPU_WriteRegister16(MPU6050_REG_GYRO_XOFFS_H, offset);
+}
+
+void MPU_SetGyroOffsetY(int offset)
+{
+    MPU_WriteRegister16(MPU6050_REG_GYRO_YOFFS_H, offset);
+}
+
+void MPU_SetGyroOffsetZ(int offset)
+{
+    MPU_WriteRegister16(MPU6050_REG_GYRO_ZOFFS_H, offset);
+}
+
+void MPU_GetGyro(vec3* out)
+{
+    byte values[6];
+	EESequentialRead(MPU6050_ADDRESS, MPU6050_REG_GYRO_XOUT_H, values, 6);
+
+    out->x = values[0] << 8 | values[1];
+    out->y = values[2] << 8 | values[3];
+    out->z = values[4] << 8 | values[5];
+}
+
+//<editor-fold desc="I2C Helper Functions" defaultstate="collapsed">
+byte MPU_ReadRegister(byte reg)
+{
+    return EERandomRead(MPU6050_ADDRESS, reg);
+}
+
+void MPU_WriteRegister(byte reg, byte value)
+{
+    EEByteWrite(MPU6050_ADDRESS, reg, value);
+    EEAckPolling(MPU6050_ADDRESS);
+}
+
+int MPU_ReadRegister16(byte reg)
+{
+    byte values[2];
+    EESequentialRead(MPU6050_ADDRESS, reg, values, 2);
+
+    int result = values[0] << 8 | values[1];
+    return result;
+}
+
+void MPU_WriteRegister16(byte reg, int value)
+{
+    byte values[2] = {(byte) value >> 8, (byte) value};
+    EEPageWrite(MPU6050_ADDRESS, reg, values, 2);
+    EEAckPolling(MPU6050_ADDRESS);
+}
+
+byte MPU_ReadRegisterBit(byte reg, byte pos)
+{
+    byte value = MPU_ReadRegister(reg);
+    return ((value >> pos) & 1);
+}
+
+void MPU_WriteRegisterBit(byte reg, byte pos, byte state)
+{
+    byte value = MPU_ReadRegister(reg);
+    if (state) {
+        value |= (1 << pos);
+    } else {
+        value &= ~(1 << pos);
+    }
+
+    MPU_WriteRegister(reg, value);
+}
+//</editor-fold>