From c5a0c58ebc143da8e903865b969c31f33363ba60 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 13 Sep 2007 11:20:39 +0200 Subject: Added encoders model. --- digital/asserv/models/encoders.m | 195 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 195 insertions(+) create mode 100644 digital/asserv/models/encoders.m diff --git a/digital/asserv/models/encoders.m b/digital/asserv/models/encoders.m new file mode 100644 index 00000000..84d24277 --- /dev/null +++ b/digital/asserv/models/encoders.m @@ -0,0 +1,195 @@ +global robot; +global trace; + +% Robot characteristics: +robot.res = 2 * pi * 20 / 2000; % Resolution (mm/step). +robot.radius = 250 / 2; % Half distance between wheels (mm). + +% Reset all simulation results. +function reset () + global robot; + global trace; + % Robot physical parameters: + robot.t = 0; % Current time (s). + robot.tx = 0; % Theta position (mm). + robot.ts = 0; % Theta speed (mm/s). + robot.ta = 0; % Theta acceleration (mm/s^2). + robot.ax = 0; % Alpha "position" (rad). + robot.as = 0; % Alpha speed (rad/s). + robot.aa = 0; % Alpha acceleration (rad/s^2). + + % Applied to encoders: + robot.lc = 0; % Absolute left coder position (mm). + robot.rc = 0; % Absolute right coder position (mm). + + % Integrated position: + robot.x = 0; % X position (mm). + robot.y = 0; % Y position (mm). + robot.a = 0; % Angle between the X axis and the robot front (rad). + + % Integer encoders: + robot.lc_i = 0; % Integer absolute left coder position (step). + robot.rc_i = 0; % Integer absolute right coder position (step). + + % Calculated position from integer encoder output: + % TODO: is 2f248 sufficient? Look at the a = pi/4 example. + robot.x_2f248 = 0; % X position (2 * 2^8 => 1 step). + robot.y_2f248 = 0; % Y position (2 * 2^8 => 1 step). + robot.a_f824 = 0; % Angle (2^24 => 2 * pi). + + % Trace: + trace.t = [ 0 ]; + trace.xya = [ 0, 0, 0 ]; + trace.xya_f = [ 0, 0, 0 ]; + trace.c = [ 0, 0 ]; + trace.c_i = [ 0, 0 ]; +end + +% Make a step of dt (s). +function step (delta) + global robot; + %% Floating point part: + % First of all, update physical parameters. + % a = constant + % s = s_0 + a t + % x = x_0 + s_0 t + 1/2 a_0 t^2 + dt = robot.ts * delta + robot.ta * delta^2 / 2; + da = robot.as * delta + robot.aa * delta^2 / 2; + robot.tx += dt; + robot.ts += robot.ta * delta; + robot.ax += da; + robot.as += robot.aa * delta; + % Compute encoders. + robot.lc = robot.tx - robot.ax * robot.radius; + robot.rc = robot.tx + robot.ax * robot.radius; + % Integrate position. + if (da == 0) + % No angle, going right ahead. + robot.x += cos (robot.a) * dt; + robot.y += sin (robot.a) * dt; + else + % With an angle, draw an arc. + arc_radius = dt / da; + robot.x += arc_radius * (sin (robot.a + da) - sin (robot.a)); + robot.y += arc_radius * (cos (robot.a) - cos (robot.a + da)); + end + robot.a += da; + %% Fixed part: + % Integer encoder value. + % TODO: sampling error. + lc_i = floor (robot.lc / robot.res); % We want a regular step, + rc_i = floor (robot.rc / robot.res); % even around 0. + dlc_i = lc_i - robot.lc_i; + drc_i = rc_i - robot.rc_i; + robot.lc_i = lc_i; + robot.rc_i = rc_i; + % Integrate position. + dt_2f248 = (dlc_i + drc_i) * 2^8; + if (dlc_i == drc_i) + robot.x_2f248 += mul_f24 (dt_2f248, cos_f824 (robot.a_f824)); + robot.y_2f248 += mul_f24 (dt_2f248, sin_f824 (robot.a_f824)); + else + c1_2pi_f032 = round (1/(2*pi) * 2^32); + radius_2i = round (2 * robot.radius / robot.res); + radius_factor_f032 = round (c1_2pi_f032 / radius_2i); + dd_2f1616 = (drc_i - dlc_i) * 2^16; + da_f824 = mul_f24 (dd_2f1616, radius_factor_f032); + na_f824 = robot.a_f824 + da_f824; + da_rad_f824 = mul_f24 (da_f824, round (2*pi * 2^24)); + dsc_f824 = sin_f824 (na_f824) - sin_f824 (robot.a_f824); + dsc_f824 = div_f24 (dsc_f824, da_rad_f824); + robot.x_2f248 += mul_f24 (dt_2f248, dsc_f824); + dsc_f824 = cos_f824 (robot.a_f824) - cos_f824 (na_f824); + dsc_f824 = div_f24 (dsc_f824, da_rad_f824); + robot.y_2f248 += mul_f24 (dt_2f248, dsc_f824); + robot.a_f824 = na_f824; + end + % Advance time and trace. + robot.t += delta; + global trace; + trace.t = [ trace.t; robot.t ]; + trace.xya = [ trace.xya; robot.x, robot.y, robot.a ]; + trace.xya_f = [ trace.xya_f; robot.x_2f248, robot.y_2f248, robot.a_f824 ]; + trace.c = [ trace.c; robot.lc, robot.rc ]; + trace.c_i = [ trace.c_i; robot.lc_i, robot.rc_i ]; +end + +% Fixed point multiply, with 24 bit fractional part. +function r = mul_f24 (a, b) + r = round (a * b / 2^24); +end + +% Fixed point divide, with 24 bit fractional part. +function r = div_f24 (a, b) + r = round (a * 2^24 / b); +end + +% Fixed point sinus, with 24 bit fractional part. +function r_f824 = sin_f824 (a_f824) + a_rad = a_f824 / 2^24 * 2*pi; + r = sin (a_rad); + r_f824 = round (r * 2^24); +end + +% Fixed point cosinus, with 24 bit fractional part. +function r_f824 = cos_f824 (a_f824) + a_rad = a_f824 / 2^24 * 2*pi; + r = cos (a_rad); + r_f824 = round (r * 2^24); +end + +% Make a trajectory with the given acceleration, during the given time. +function traj (ta, aa, t) + global robot; + robot.ta = ta; + robot.aa = aa / robot.radius; + stop = robot.t + t; + while (robot.t < stop) + step (0.010); + end +end + +% Make sure the robot is stopped. +function stop () + global robot; + robot.ts = 0; + robot.as = 0; +end + +% Example path. +function path () + global robot; + traj (0, 500, 0.3); + traj (0, 0, 0.5); + traj (0, -500, 0.3); + stop (); + traj (1000, 0, 0.2); + traj (0, 0, 2); + traj (-1000, 0, 0.2); + stop (); + traj (1000, 500, 0.3); + traj (0, 0, 2); + traj (-1000, -500, 0.3); + stop (); +end + +motor; +global to_file = 0; + +reset (); +path (); + +conv = diag ([ robot.res / 2 / 2^8, robot.res / 2 / 2^8, 2*pi / 2^24 ]); + +plotterm ("trace"); +multiplot (2, 2); +mplot (trace.xya (:,1), trace.xya (:,2), "1;trace xy;"); +mplot (trace.t, trace.xya (:,3), "2;trace a;"); +xya_f_conv = trace.xya_f * conv; +mplot (xya_f_conv (:,1), xya_f_conv (:,2), "1;trace xy f;"); +mplot (trace.t, xya_f_conv (:,3), "2;trace a f;"); +multiplot (0, 0); + +final_pos = [ robot.x, robot.y, robot.a ] +final_pos_f = [ robot.x_2f248, robot.y_2f248, robot.a_f824 ] +final_pos_f_conv = final_pos_f * conv -- cgit v1.2.3