imu impl

lib/LSM6/src/LSM6.cpp

@@ -29,7 +29,7 @@ LSM6::LSM6(void)
 bool LSM6::checkForNewData(void)
 {
   bool retVal = false;
-  if(getStatus() & 0x01)
+  if(getStatus() & 0x02)
   {
     read();
 
@@ -228,7 +228,7 @@ void LSM6::enableDefault(void)
   {
     // Set the gyro full scale and data rate
     setFullScaleGyro(GYRO_FS245);
-    setGyroDataOutputRate(ODR13);
+    setGyroDataOutputRate(ODR104);
 
     // Set the accelerometer full scale and data rate
     setFullScaleAcc(ACC_FS2);

lib/LSM6/src/LSM6.h

@@ -141,6 +141,12 @@ public:
 
     vector<float> updateGyroBias(void) 
     {
+      if (initialMeasurement) {
+          gyroBias.z = g.z;
+          initialMeasurement = false;
+      }
+      float retain = (0.98);
+      gyroBias.z = g.z * (1- retain) + gyroBias.z * retain;
       /**
        * TODO: add a low-pass filter to update the bias
        */
@@ -169,6 +175,7 @@ public:
     // float estimatedPitchAngle = 0;
     // vector<float> eulerAngles;
     vector<float> gyroBias;
+    bool initialMeasurement = true;
 
     /* We make Robot a friend to avoid all the setters and getters. */
     friend class Robot;

lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h

@@ -25,10 +25,10 @@ protected:
     enum CTRL_MODE : uint8_t {CTRL_DIRECT, CTRL_SPEED};
     volatile CTRL_MODE ctrlMode = CTRL_DIRECT;
 
-    float Kp = 2.50; // proportional to error
-    float Ki = 0.03;
+    float Kp = 6.00; // proportional to error
+    float Ki = 0.05;
     float Kd = 0;
-    float Kf = 3.00; // proportional to setpoint
+    float Kf = 4.50; // proportional to setpoint
     int16_t C = 22; // constant added to effort
 
     // Used to keep track of the target speed, in counts / interval.

src/robot-remote.cpp

@@ -76,16 +76,16 @@ void Robot::HandleKeyCode(int16_t keyCode)
         switch(keyCode)
         {
             case UP_ARROW:
-                chassis.SetTwist(5, 0);
+                chassis.SetTwist(15, 0);
                 break;
             case RIGHT_ARROW:
-                chassis.SetTwist(0, -0.25);
+                chassis.SetTwist(0, -1.25);
                 break;
             case DOWN_ARROW:
-                chassis.SetTwist(-5, 0);
+                chassis.SetTwist(-15, 0);
                 break;
             case LEFT_ARROW:
-                chassis.SetTwist(0, 0.25);
+                chassis.SetTwist(0, 1.25);
                 break;
             case ENTER_SAVE:
                 chassis.SetTwist(0, 0);

src/robot.cpp

@@ -1,5 +1,6 @@
 #include "robot.h"
 #include <IRdecoder.h>
+#include <limits.h>
 
 void Robot::InitializeRobot(void)
 {
@@ -79,11 +80,22 @@ void Robot::HandleOrientationUpdate(void)
 
     else // update orientation
     {
-        // TODO: update the orientation
+        float z_rot = ((float)imu.g.z - imu.gyroBias.z) / (float) INT_MAX;
+        z_rot /= 104;
+        z_rot *= 245;
+        eulerAngles.z += z_rot;
     }
 
 #ifdef __IMU_DEBUG__
-    Serial.println(eulerAngles.z);
+    Serial.print(imu.gyroBias.z);
+    Serial.print('\t');
+    Serial.print(eulerAngles.z);
+    Serial.print('\t');
+    //Serial.print(imu.g.x);
+    //Serial.print('\t');
+    //Serial.print(imu.g.y);
+    //Serial.print('\t');
+    Serial.println(imu.g.z);
 #endif
 }