Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Now vl and vr return v_x and omega respectively

Browse Source
This commit is contained in:
pimpest
2026-02-09 15:09:24 +01:00
parent 17d19e393f
commit 1a25dcb977
1 changed files with 10 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
firmware/base/src/main.c
View File

@@ -66,7 +66,7 @@ static calib_diff_t calib_enc = {
.right_gain = 1.0000 .right_gain = 1.0000
}; };
bool update_pos_cb() { void update_pos_cb() {
substep_update(&state_l); substep_update(&state_l);
substep_update(&state_r); substep_update(&state_r);
@@ -89,12 +89,18 @@ bool update_pos_cb() {
vel_l /=64 * ENCODER_CPR; vel_l /=64 * ENCODER_CPR;
vel_r /=64 * ENCODER_CPR; vel_r /=64 * ENCODER_CPR;
base_vl = vel_l;
base_vr = vel_r;
vel_l *= WHEEL_RADIUS * 2 * M_PI * calib_enc.left_gain; vel_l *= WHEEL_RADIUS * 2 * M_PI * calib_enc.left_gain;
vel_r *= -WHEEL_RADIUS * 2 * M_PI * calib_enc.right_gain; vel_r *= -WHEEL_RADIUS * 2 * M_PI * calib_enc.right_gain;
const double vl_enc = state_l.speed;
const double vr_enc = state_r.speed;
const double vl = (vl_enc/(64 * ENCODER_CPR)) * WHEEL_RADIUS * 2 * M_PI * calib_enc.left_gain;
const double vr = (vr_enc/(64 * ENCODER_CPR)) * -WHEEL_RADIUS * 2 * M_PI * calib_enc.right_gain;
base_vl = (vl + vr)/2;
base_vr = (vr - vl)/wheel_separation;
const double linear = (vel_l + vel_r) / 2; const double linear = (vel_l + vel_r) / 2;
const double angular = (vel_r - vel_l) / wheel_separation; const double angular = (vel_r - vel_l) / wheel_separation;
const double r = linear / angular; const double r = linear / angular;