feat: kinematics theta simulating magnetic field

arduino-libraries · Dec 19, 2024 · 0fe62e0 · 0fe62e0
1 parent 28fb1d2
commit 0fe62e0
Show file tree

Hide file tree

Showing 4 changed files with 32 additions and 5 deletions.
diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp
@@ -686,8 +686,8 @@ int Arduino_AlvikCarrier::beginImu(){
     ipKnobs->modx = MOTION_FX_DECIMATION;
 
     MotionFX_setKnobs(mfxstate, ipKnobs);
-    MotionFX_enable_6X(mfxstate, MFX_ENGINE_ENABLE);
-    MotionFX_enable_9X(mfxstate, MFX_ENGINE_DISABLE);
+    MotionFX_enable_6X(mfxstate, MFX_ENGINE_DISABLE);
+    MotionFX_enable_9X(mfxstate, MFX_ENGINE_ENABLE);
 
     return 0;
 }
@@ -701,6 +701,10 @@ void Arduino_AlvikCarrier::updateImu(){
     imu_data.acc[0] = (float)accelerometer[0] * FROM_MG_TO_G;
     imu_data.acc[1] = (float)accelerometer[1] * FROM_MG_TO_G;
     imu_data.acc[2] = (float)accelerometer[2] * FROM_MG_TO_G;
+    imu_data.mag[0] = -getSinTheta() * SIM_MAG_FILED_UT50;
+    imu_data.mag[1] = getCosTheta() * SIM_MAG_FILED_UT50;
+    imu_data.mag[2] = 0;
+
 
     if (sample_to_discard>MOTION_FX_SAMPLETODISCARD){
         MotionFX_propagate(mfxstate, &filter_data, &imu_data, &imu_delta_time);
@@ -884,6 +888,14 @@ float Arduino_AlvikCarrier::getTheta(){
     return kinematics->getTheta();
 }
 
+float Arduino_AlvikCarrier::getSinTheta(){
+    return kinematics->getSinTheta();
+}
+
+float Arduino_AlvikCarrier::getCosTheta(){
+    return kinematics->getCosTheta();
+}
+
 void Arduino_AlvikCarrier::resetPose(const float x0, const float y0, const float theta0){
     kinematics->resetPose(x0,y0,theta0);
 }

diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h
@@ -252,6 +252,8 @@ class Arduino_AlvikCarrier{
         float getX();                                                                   // absolute position in mm
         float getY();                                                                   // absolute position in mm
         float getTheta();                                                               // angle in deg
+        float getSinTheta();                                                            // stored sin of current theta
+        float getCosTheta();                                                            // stored cos of current theta
         void resetPose(const float x0=0.0, const float y0=0.0, const float theta0=0.0); // reset pose in kinematics
 
         void move(const float distance);                                                // move of distance millimeters

diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h
@@ -88,7 +88,9 @@ const float MOTION_FX_PERIOD = (1000U / MOTION_FX_FREQ);
 #define GBIAS_ACC_TH_SC                 (2.0f*0.000765f)
 #define GBIAS_GYRO_TH_SC                (2.0f*0.002f)
 #define MOTION_FX_DECIMATION                      1U
-
+#define FROM_MGAUSS_TO_UT50  (0.1f/50.0f)
+#define SIM_MAG_FIELD_MG         500        // simulated magnetic field intensity in mGauss
+#define SIM_MAG_FILED_UT50      SIM_MAG_FIELD_MG * FROM_MGAUSS_TO_UT50
 
 // Library version
 #define VERSION_BYTE_HIGH 1

diff --git a/src/robotics/kinematics.h b/src/robotics/kinematics.h
@@ -30,6 +30,7 @@ class Kinematics{
         float left_vel, right_vel;
 
         float theta, delta_theta;
+        float sin_theta, cos_theta;
         float x, y;
         float delta_left, delta_right;
         float delta_travel;
@@ -154,8 +155,10 @@ class Kinematics{
 
         void updatePose(){
             delta_theta=angular_velocity*control_period;
-            delta_x=linear_velocity*cos(theta)*control_period;
-            delta_y=linear_velocity*sin(theta)*control_period;
+            sin_theta = sin(theta);
+            cos_theta = cos(theta);
+            delta_x=linear_velocity*cos_theta*control_period;
+            delta_y=linear_velocity*sin_theta*control_period;
             delta_travel=sqrt(delta_x*delta_x+delta_y*delta_y);
             x+=delta_x;
             y+=delta_y;
@@ -192,6 +195,14 @@ class Kinematics{
             return rads_to_degs(theta);
         }
 
+        float getSinTheta(){
+            return sin_theta;
+        }
+
+        float getCosTheta(){
+            return cos_theta;
+        }
+
         float getTravel(){
             return m_to_mm(travel);
         }