diff --git a/Core/Inc/Control/stateEstimation.h b/Core/Inc/Control/stateEstimation.h index 68ffcaf..e9b0590 100644 --- 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 index 50ac11c..b6e1087 100644 --- 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]) {