Magnezijum-Robotics/magrob
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Added calibration sequence for encoder diameter diff

Browse Source
This commit is contained in:
Pimpest
2025-04-29 14:59:27 +02:00
parent 6fefe74442
commit ed9c5bab45
4 changed files with 93 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -43,6 +43,18 @@ typedef struct instr_buf_t{
instr_buf_t stepper_buf = {};
typedef struct calib_diff_t {
long long left_pulse;
long long right_pulse;
double left_gain;
double right_gain;
} calib_diff_t;
static calib_diff_t calib_enc = {
.left_gain = 0.997648,
.right_gain = 1.002333
};
bool update_pos_cb() {
substep_update(&state_l);
@ -51,16 +63,22 @@ bool update_pos_cb() {
int position_l= state_l.position;
int position_r= state_r.position;
double vel_l = position_l - prev_position_l;
double vel_r = position_r - prev_position_r;
const int diff_l = position_l - prev_position_l;
const int diff_r = position_r - prev_position_r;
calib_enc.left_pulse += diff_l;
calib_enc.right_pulse -= diff_r;
double vel_l = diff_l;
double vel_r = diff_r;
prev_position_l = state_l.position;
prev_position_r = state_r.position;
vel_l /=64 * ENCODER_CPR;
vel_r /=64 * ENCODER_CPR;
vel_l *= WHEEL_RADIUS * 2 * M_PI;
vel_r *= -WHEEL_RADIUS * 2 * M_PI;
vel_l *= WHEEL_RADIUS * 2 * M_PI * calib_enc.left_gain;
vel_r *= -WHEEL_RADIUS * 2 * M_PI * calib_enc.right_gain;
const double linear = (vel_l + vel_r) / 2;
const double angular = (vel_r - vel_l) / wheel_separation;
@ -85,6 +103,23 @@ void zero() {
base_x = 0;
base_y = 0;
base_theta = 0;
calib_enc.left_pulse = 0;
calib_enc.right_pulse = 0;
}
void start_calib() {
calib_enc.left_pulse = 0;
calib_enc.right_pulse = 0;
}
void end_calib() {
const double l_adjust = (double)calib_enc.left_pulse * calib_enc.left_gain;
const double r_adjust = (double)calib_enc.right_pulse * calib_enc.right_gain;
const double delta_angle = l_adjust - r_adjust;
const double distance = 0.5 * (l_adjust + r_adjust);
const double factor = delta_angle / distance * 0.7;
calib_enc.left_gain = (1.0 - factor) * calib_enc.left_gain;
calib_enc.right_gain = (1.0 + factor) * calib_enc.right_gain;
}
@ -140,6 +175,9 @@ void core2_entry()
type = (cmd >> 8) & 0xFF;
} else if(cmd == (((uint16_t)'z' << 8) | ';')) {
zero();
} else if(cmd == (((uint16_t)'c' << 8) | ';')) {
end_calib();
printf("%lf %lf\n", calib_enc.left_gain, calib_enc.right_gain);
}
}