From ff6dda0e86593c75a2f3dc944da616f79f43e9fd Mon Sep 17 00:00:00 2001 From: Julie Date: Wed, 7 Feb 2024 17:19:25 +0100 Subject: [PATCH 01/10] Checking if the yaw calibration (with IMU and Vision) is done, if not the robot will be in halt, leaving enough time for the robot to do the yaw calibration --- Core/Src/robot.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Core/Src/robot.c b/Core/Src/robot.c index 6984b92..2556fc6 100644 --- a/Core/Src/robot.c +++ b/Core/Src/robot.c @@ -540,7 +540,7 @@ void loop(void){ /* === Determine HALT state === */ xsens_CalibrationDone = (MTi->statusword & (0x18)) == 0; // if bits 3 and 4 of status word are zero, calibration is done - halt = !xsens_CalibrationDone || !(is_connected_wireless || is_connected_serial) || !REM_last_packet_had_correct_version; + halt = !xsens_CalibrationDone || !(is_connected_wireless || is_connected_serial) || !REM_last_packet_had_correct_version || !yaw_hasCalibratedOnce(); // halt is true when no yaw calibration was done, thus calibration will be required (halt is neceassry to have enough time for yaw calibration) if(IS_RUNNING_TEST || DRAIN_BATTERY) halt = false; if (halt) { From 3adcf382004a981f9f0e88e89cc3fabf2f19cdb7 Mon Sep 17 00:00:00 2001 From: Julie Date: Wed, 7 Feb 2024 17:54:47 +0100 Subject: [PATCH 02/10] Uncomment this part if you want to check the yaw calibration (IMU and Vision), if no Calibration was done, the wheel will sstop and yaw will be re-calibrated again --- Core/Src/robot.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/Core/Src/robot.c b/Core/Src/robot.c index 2556fc6..afc20d8 100644 --- a/Core/Src/robot.c +++ b/Core/Src/robot.c @@ -875,6 +875,12 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { return; } + // if(!yaw_hasCalibratedOnce()){ + // wheels_Stop(); + // yaw_Calibrate(stateInfo.xsensYaw, stateInfo.visionYaw, stateInfo.visionAvailable, stateInfo.rateOfTurn); + // return; + // } + // State control float stateLocal[4] = {0.0f}; stateEstimation_GetState(stateLocal); From 1cab75789b29e70a270a93986c1c4f0ed40bf41f Mon Sep 17 00:00:00 2001 From: Julie Date: Mon, 12 Feb 2024 13:36:19 +0100 Subject: [PATCH 03/10] Correct slippage factors --- Core/Inc/Control/stateEstimation.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Core/Inc/Control/stateEstimation.h b/Core/Inc/Control/stateEstimation.h index e9b0590..8845a76 100644 --- a/Core/Inc/Control/stateEstimation.h +++ b/Core/Inc/Control/stateEstimation.h @@ -22,8 +22,8 @@ ///////////////////////////////////////////////////// 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_U 0.87 // Correction constant for slippage u direction +#define SLIPPAGE_FACTOR_V 0.91 // Correction constant for slippage v direction #define SLIPPAGE_FACTOR_W 1.00 // Correction constant for slippage w direction (angular velocity) ///////////////////////////////////////////////////// STRUCTS From bcd89f2d6f5b9c3e0425455904abc958b44bcda0 Mon Sep 17 00:00:00 2001 From: Julie Date: Mon, 12 Feb 2024 15:38:10 +0100 Subject: [PATCH 04/10] no feedforward --- Core/Src/peripherals/wheels.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Core/Src/peripherals/wheels.c b/Core/Src/peripherals/wheels.c index 9bf8b52..76f7e9e 100644 --- a/Core/Src/peripherals/wheels.c +++ b/Core/Src/peripherals/wheels.c @@ -184,7 +184,7 @@ void wheels_Update(){ } // Add PID to commanded speed and convert to PWM - int32_t wheel_speed = OMEGAtoPWM * (feed_forward[wheel] + PID(angular_velocity_error, &wheelsK[wheel])); + int32_t wheel_speed = OMEGAtoPWM * (feed_forward[wheel]*0 + PID(angular_velocity_error, &wheelsK[wheel])); // Determine direction and if pwm is negative, switch directions // PWM < 0 : CounterClockWise. Direction = 0 From 213b51938808c0f8556ece335fcc845e347f11d8 Mon Sep 17 00:00:00 2001 From: Julie Date: Mon, 12 Feb 2024 15:40:17 +0100 Subject: [PATCH 05/10] second try for yaw calibration with wheel stop --- Core/Src/robot.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/Core/Src/robot.c b/Core/Src/robot.c index afc20d8..adbabf4 100644 --- a/Core/Src/robot.c +++ b/Core/Src/robot.c @@ -540,7 +540,7 @@ void loop(void){ /* === Determine HALT state === */ xsens_CalibrationDone = (MTi->statusword & (0x18)) == 0; // if bits 3 and 4 of status word are zero, calibration is done - halt = !xsens_CalibrationDone || !(is_connected_wireless || is_connected_serial) || !REM_last_packet_had_correct_version || !yaw_hasCalibratedOnce(); // halt is true when no yaw calibration was done, thus calibration will be required (halt is neceassry to have enough time for yaw calibration) + halt = !xsens_CalibrationDone || !(is_connected_wireless || is_connected_serial) || !REM_last_packet_had_correct_version; if(IS_RUNNING_TEST || DRAIN_BATTERY) halt = false; if (halt) { @@ -875,11 +875,11 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { return; } - // if(!yaw_hasCalibratedOnce()){ - // wheels_Stop(); - // yaw_Calibrate(stateInfo.xsensYaw, stateInfo.visionYaw, stateInfo.visionAvailable, stateInfo.rateOfTurn); - // return; - // } + if(!yaw_hasCalibratedOnce()){ + wheels_Stop(); + yaw_Calibrate(stateInfo.xsensYaw, stateInfo.visionYaw, stateInfo.visionAvailable, stateInfo.rateOfTurn); + return; + } // State control float stateLocal[4] = {0.0f}; From e24b048e2152324d33439de01024976d6934afbd Mon Sep 17 00:00:00 2001 From: Julie Date: Thu, 28 Mar 2024 13:09:43 +0100 Subject: [PATCH 06/10] yaw calibration happens in state Estimation Update, wheels stop for 5 seconds to check calibration with Vision --- Core/Src/robot.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/Core/Src/robot.c b/Core/Src/robot.c index adbabf4..f0ce7d2 100644 --- a/Core/Src/robot.c +++ b/Core/Src/robot.c @@ -862,7 +862,13 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { stateInfo.xsensAcc[vel_y] = MTi->acc[vel_y]; stateInfo.xsensYaw = (MTi->angles[2]*M_PI/180); //Gradients to Radians stateInfo.rateOfTurn = MTi->gyr[2]; - stateEstimation_Update(&stateInfo); + + if(counter_TIM_CONTROL < 500) { // Robot tries to connect to Vision for 5 seconds, after that, control loop will be normally executed (if Vision is not detected, eg: during python tests) + if(!yaw_hasCalibratedOnce()) { + wheels_Stop(); + stateEstimation_Update(&stateInfo); + return; + } if(test_isTestRunning(wheels) || test_isTestRunning(normal)) { wheels_Update(); @@ -875,12 +881,6 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { return; } - if(!yaw_hasCalibratedOnce()){ - wheels_Stop(); - yaw_Calibrate(stateInfo.xsensYaw, stateInfo.visionYaw, stateInfo.visionAvailable, stateInfo.rateOfTurn); - return; - } - // State control float stateLocal[4] = {0.0f}; stateEstimation_GetState(stateLocal); From 4831c1f28d5ff1cef0fdc5fef1edd2c603ad1018 Mon Sep 17 00:00:00 2001 From: Julie Date: Thu, 28 Mar 2024 13:16:47 +0100 Subject: [PATCH 07/10] fix bug (couldnt build) --- Core/Src/robot.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/Core/Src/robot.c b/Core/Src/robot.c index f0ce7d2..6e5149c 100644 --- a/Core/Src/robot.c +++ b/Core/Src/robot.c @@ -865,9 +865,10 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { if(counter_TIM_CONTROL < 500) { // Robot tries to connect to Vision for 5 seconds, after that, control loop will be normally executed (if Vision is not detected, eg: during python tests) if(!yaw_hasCalibratedOnce()) { - wheels_Stop(); - stateEstimation_Update(&stateInfo); - return; + wheels_Stop(); + stateEstimation_Update(&stateInfo); + return; + } } if(test_isTestRunning(wheels) || test_isTestRunning(normal)) { From eb0b249e7f83f06b3873f89a09c3cfc28d95c845 Mon Sep 17 00:00:00 2001 From: Julie Date: Thu, 28 Mar 2024 13:32:00 +0100 Subject: [PATCH 08/10] No need for the counter because within yaw calibration, we already checked whethere Vision is available, if not we just continue the code --- Core/Src/robot.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/Core/Src/robot.c b/Core/Src/robot.c index 6e5149c..33e3c54 100644 --- a/Core/Src/robot.c +++ b/Core/Src/robot.c @@ -863,13 +863,13 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { stateInfo.xsensYaw = (MTi->angles[2]*M_PI/180); //Gradients to Radians stateInfo.rateOfTurn = MTi->gyr[2]; - if(counter_TIM_CONTROL < 500) { // Robot tries to connect to Vision for 5 seconds, after that, control loop will be normally executed (if Vision is not detected, eg: during python tests) - if(!yaw_hasCalibratedOnce()) { - wheels_Stop(); - stateEstimation_Update(&stateInfo); - return; - } + // if(counter_TIM_CONTROL < 500) { // Robot tries to connect to Vision for 5 seconds, after that, control loop will be normally executed (if Vision is not detected, eg: during python tests) + if(!yaw_hasCalibratedOnce()) { + wheels_Stop(); + stateEstimation_Update(&stateInfo); + return; } + // } if(test_isTestRunning(wheels) || test_isTestRunning(normal)) { wheels_Update(); From 6fb0528d1c51b37288b5b3a614139aafcb8dfa11 Mon Sep 17 00:00:00 2001 From: Julie Date: Tue, 2 Apr 2024 15:25:45 +0200 Subject: [PATCH 09/10] Fixing the yaw calibration (state Estimation Update needs to happen outside of the if-loop, if not state Estimation is not only called once (and not at every timesteps)) --- Core/Src/robot.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/Core/Src/robot.c b/Core/Src/robot.c index 33e3c54..6a0783d 100644 --- a/Core/Src/robot.c +++ b/Core/Src/robot.c @@ -862,14 +862,15 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { stateInfo.xsensAcc[vel_y] = MTi->acc[vel_y]; stateInfo.xsensYaw = (MTi->angles[2]*M_PI/180); //Gradients to Radians stateInfo.rateOfTurn = MTi->gyr[2]; + + stateEstimation_Update(&stateInfo); - // if(counter_TIM_CONTROL < 500) { // Robot tries to connect to Vision for 5 seconds, after that, control loop will be normally executed (if Vision is not detected, eg: during python tests) - if(!yaw_hasCalibratedOnce()) { + if(counter_TIM_CONTROL < 500) { // Robot tries to connect to Vision for 5 seconds, after that, control loop will be normally executed (if Vision is not detected, eg: during python tests) + if(!yaw_hasCalibratedOnce()) { // Force to check yaw calibration every time robot boots up wheels_Stop(); - stateEstimation_Update(&stateInfo); return; } - // } + } if(test_isTestRunning(wheels) || test_isTestRunning(normal)) { wheels_Update(); From 6cd10e1bb723d8d80b58638cdd07532b5fe9552a Mon Sep 17 00:00:00 2001 From: Julie Date: Tue, 2 Apr 2024 16:43:32 +0200 Subject: [PATCH 10/10] Enabling feedfoward --- Core/Src/peripherals/wheels.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Core/Src/peripherals/wheels.c b/Core/Src/peripherals/wheels.c index 76f7e9e..9bf8b52 100644 --- a/Core/Src/peripherals/wheels.c +++ b/Core/Src/peripherals/wheels.c @@ -184,7 +184,7 @@ void wheels_Update(){ } // Add PID to commanded speed and convert to PWM - int32_t wheel_speed = OMEGAtoPWM * (feed_forward[wheel]*0 + PID(angular_velocity_error, &wheelsK[wheel])); + int32_t wheel_speed = OMEGAtoPWM * (feed_forward[wheel] + PID(angular_velocity_error, &wheelsK[wheel])); // Determine direction and if pwm is negative, switch directions // PWM < 0 : CounterClockWise. Direction = 0