From cb29fa4ba7d1492c9f91752a7a07b512fee7dad0 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Thu, 24 Sep 2015 18:43:34 +0200 Subject: [PATCH 1/1] umbmark correction --- avr/motor_ctrl/main.c | 25 ++++++++++++++----------- config/control.yaml | 2 +- scripts/umbmark.py | 4 ++++ 3 files changed, 19 insertions(+), 12 deletions(-) diff --git a/avr/motor_ctrl/main.c b/avr/motor_ctrl/main.c index d403479..2ec14a1 100644 --- a/avr/motor_ctrl/main.c +++ b/avr/motor_ctrl/main.c @@ -106,7 +106,10 @@ #define KI 0.051429 #define KD 0.000378 #define PID_T 0.01 -#define STEP_PER_M 4171.4 // wheel diameter=12cm, encoder=48cpr, gear ratio=1:34, calculated wheel diameter: 0.12454m +// wheel diameter=12cm, encoder=48cpr, gear ratio=1:34, real wheel diameter: 0.12454m +#define STEP_PER_M_AVG 4171.4 +#define STEP_PER_M_LEFT (STEP_PER_M_AVG) +#define STEP_PER_M_RIGHT (STEP_PER_M_AVG) #define WHEEL_DIST 0.36923 // Real: 0.252 enum mode { @@ -267,8 +270,8 @@ ISR(TWI_vect) break; case 0x2B: // Left speed wish LSB tmp_speed.i = tmp_speed.i << 8 | TWDR; - speed1_wish = tmp_speed.f*STEP_PER_M; - speed2_wish = tmp_speed.f*STEP_PER_M; + speed1_wish = tmp_speed.f*STEP_PER_M_LEFT; + speed2_wish = tmp_speed.f*STEP_PER_M_LEFT; motor1_mode = MOTOR_PID; motor2_mode = MOTOR_PID; TWI_ACK; @@ -287,8 +290,8 @@ ISR(TWI_vect) break; case 0x2F: // Right speed wish LSB tmp_speed.i = tmp_speed.i << 8 | TWDR; - speed1_wish = tmp_speed.f*STEP_PER_M; - speed2_wish = tmp_speed.f*STEP_PER_M; + speed1_wish = tmp_speed.f*STEP_PER_M_RIGHT; + speed2_wish = tmp_speed.f*STEP_PER_M_RIGHT; motor1_mode = MOTOR_PID; motor2_mode = MOTOR_PID; TWI_ACK; @@ -772,8 +775,8 @@ static void update_pos(void) { new_speed3 = pos3_diff/PID_T; new_speed4 = pos4_diff/PID_T; - diff_left_m = (pos1_diff + pos2_diff)/(2*STEP_PER_M); - diff_right_m = (pos3_diff + pos4_diff)/(2*STEP_PER_M); + diff_left_m = (pos1_diff + pos2_diff)/(2*STEP_PER_M_LEFT); + diff_right_m = (pos3_diff + pos4_diff)/(2*STEP_PER_M_RIGHT); angle_diff = (diff_right_m - diff_left_m) / WHEEL_DIST; angle_new = angle.f + angle_diff; @@ -786,8 +789,8 @@ static void update_pos(void) { speed_l = (new_speed1+new_speed2)/2; speed_r = (new_speed3+new_speed4)/2; - tmp_speed_lin = (speed_l + speed_r)/(2.0*STEP_PER_M); - tmp_speed_rot = (speed_r - speed_l)/(M_PI*WHEEL_DIST*STEP_PER_M); + tmp_speed_lin = (speed_l + speed_r)/(2.0*STEP_PER_M_AVG); + tmp_speed_rot = (speed_r - speed_l)/(M_PI*WHEEL_DIST*STEP_PER_M_AVG); // copy from tmp cli(); @@ -962,8 +965,8 @@ int main(void) { speed_wish_right = angle*M_PI*WHEEL_DIST/2 + speed; speed_wish_left = speed*2-speed_wish_right; - speed_wish_left*=STEP_PER_M; - speed_wish_right*=STEP_PER_M; + speed_wish_left*=STEP_PER_M_LEFT; + speed_wish_right*=STEP_PER_M_RIGHT; if (speed_wish_left > 0 && aft_handicap_fwd > 0) { speed1_wish = speed_wish_left * (100-aft_handicap_fwd)/100.0; diff --git a/config/control.yaml b/config/control.yaml index ea206e9..256d5ca 100644 --- a/config/control.yaml +++ b/config/control.yaml @@ -23,7 +23,7 @@ diff_drive_controller: # Wheel separation and radius multipliers wheel_separation_multiplier: 1.4126 # default: 1.0 - wheel_radius_multiplier : 0.98284 # default: 1.0 + wheel_radius_multiplier : 1.0 # default: 1.0 # Velocity and acceleration limits # Whenever a min_* is unspecified, default to -max_* diff --git a/scripts/umbmark.py b/scripts/umbmark.py index e6bb5d5..f031a67 100755 --- a/scripts/umbmark.py +++ b/scripts/umbmark.py @@ -21,10 +21,14 @@ # beta = (y_cg_cw + y_cg_ccw)/(-4*L) # R = (L/2)/sin(beta/2) # Ed = Dr/Dl*(R+b/2)/(R-b/2) +# Da = (Dr + Dl)/2 +# Dl = 2/(Ed + 1) * Da +# Dr = 2/((1/Ed) + 1) * Da # # Wheelbase correction: # alpha = (y_cg_cw - y_cg_ccw)/(-4*L) * 180/pi # Eb = (90)/(90-alpha) +# b_new = Eb*b import sys import rospy -- 2.39.2