update Imu

Author: gbr1

src/Arduino_Robot_Firmware.cpp

@@ -67,7 +67,7 @@ int Arduino_Robot_Firmware::begin(){
     connectExternalI2C();
     ext_wire->begin(ARDUINO_ROBOT_ADDRESS);
 
-    //beginImu();
+
 
     if (beginAPDS()!=0){
         errorLed(ERROR_APDS);
@@ -85,6 +85,9 @@ int Arduino_Robot_Firmware::begin(){
         //errorLed(ERROR_TOUCH);
     }
 
+    if (beginImu()!=){
+        errorLed(ERROR_IMU);
+    }
 
     return 0;
 }
@@ -386,6 +389,12 @@ int Arduino_Robot_Firmware::beginImu(){
     imu->Set_G_FS(2000);
     imu->Enable_X();
     imu->Enable_G();
+    return 0;
+}
+
+void Arduino_Robot_Firmware::updateImu(){
+    imu->Get_X_Axes(accelerometer);
+    imu->Get_G_Axes(gyroscope);
 }
 
 

src/Arduino_Robot_Firmware.h

@@ -34,6 +34,8 @@ class Arduino_Robot_Firmware{
         AT42QT2120::Status touch_status;
 
         LSM6DSOSensor * imu;
+        int32_t accelerometer[3];
+        int32_t gyroscope[3];
 
 
     public:
@@ -130,8 +132,7 @@ class Arduino_Robot_Firmware{
 
         // Imu
         int beginImu();
-
-
+        void updateImu();
 
         void errorLed(const int error_code);
 

src/robot_definitions.h

@@ -14,4 +14,11 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO;
 #define MOTOR_KD_RIGHT 0.4
 #define MOTOR_CONTROL_PERIOD 0.01
 
+
+
+#define FROM_MG_TO_G  0.001f
+#define FROM_G_TO_MG  1000.0f
+#define FROM_MDPS_TO_DPS  0.001f
+#define FROM_DPS_TO_MDPS  1000.0f
+
 #endif