Merge pull request #88 from RoboTeamTwente/control/move-slippage-cons…

…tants Move slippage correction constants
RoboTeamTwente · Aug 23, 2023 · 2198520 · 2198520
2 parents e39a53e + 1c03130
commit 2198520
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 2 deletions.
diff --git a/Core/Inc/Control/stateEstimation.h b/Core/Inc/Control/stateEstimation.h
@@ -19,6 +19,13 @@
 #include "kalman.h"
 #include "control_util.h"
 
+///////////////////////////////////////////////////// CONSTANTS
+
+/* Factors to align vision velocities with robot velocities (see https://wiki.roboteamtwente.nl/technical/control/slippage) */
+#define SLIPPAGE_FACTOR_U 0.92	// Correction constant for slippage u direction
+#define SLIPPAGE_FACTOR_V 0.90  // Correction constant for slippage v direction
+#define SLIPPAGE_FACTOR_W 1.00  // Correction constant for slippage w direction (angular velocity)
+
 ///////////////////////////////////////////////////// STRUCTS
 
 typedef struct StateInfo {

diff --git a/Core/Src/Control/stateEstimation.c b/Core/Src/Control/stateEstimation.c
@@ -92,8 +92,9 @@ void stateEstimation_Update(StateInfo* input) {
 
 	// Compensate for constant slippage by multiplying with empirically determined values.
 	//  Explanation: https://wiki.roboteamtwente.nl/technical/control/slippage
-	stateLocal[vel_u] = 0.92 * stateLocal[vel_u];
-	stateLocal[vel_v] = 0.9 * stateLocal[vel_v];
+	stateLocal[vel_u] = SLIPPAGE_FACTOR_U * stateLocal[vel_u];
+	stateLocal[vel_v] = SLIPPAGE_FACTOR_V * stateLocal[vel_v];
+	stateLocal[vel_w] = SLIPPAGE_FACTOR_W * stateLocal[vel_w];
 }
 
 void stateEstimation_GetState(float _stateLocal[4]) {