Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix/yaw calibration ai #102

Open
wants to merge 10 commits into
base: development
Choose a base branch
from
Open

Fix/yaw calibration ai #102

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
ff6dda0
Checking if the yaw calibration (with IMU and Vision) is done, if not…
jujumuch Feb 7, 2024
3adcf38
Uncomment this part if you want to check the yaw calibration (IMU and…
jujumuch Feb 7, 2024
1cab757
Correct slippage factors
jujumuch Feb 12, 2024
bcd89f2
no feedforward
jujumuch Feb 12, 2024
213b519
second try for yaw calibration with wheel stop
jujumuch Feb 12, 2024
e24b048
yaw calibration happens in state Estimation Update, wheels stop for 5…
jujumuch Mar 28, 2024
4831c1f
fix bug (couldnt build)
jujumuch Mar 28, 2024
eb0b249
No need for the counter because within yaw calibration, we already ch…
jujumuch Mar 28, 2024
6fb0528
Fixing the yaw calibration (state Estimation Update needs to happen o…
jujumuch Apr 2, 2024
6cd10e1
Enabling feedfoward
jujumuch Apr 2, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions Core/Inc/Control/stateEstimation.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
10 changes: 9 additions & 1 deletion Core/Src/robot.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
if(IS_RUNNING_TEST || DRAIN_BATTERY) halt = false;

if (halt) {
Expand Down Expand Up @@ -862,7 +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()) { // Force to check yaw calibration every time robot boots up
wheels_Stop();
return;
}
}

if(test_isTestRunning(wheels) || test_isTestRunning(normal)) {
wheels_Update();
Expand Down
Loading