summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2007-09-13 11:20:39 +0200
committerNicolas Schodet2007-09-13 11:20:39 +0200
commitc5a0c58ebc143da8e903865b969c31f33363ba60 (patch)
tree9f3684517092377207e144dc690a677fdb640398
parentd637068b6209bf4552d4bc7d2fe6b8b769c9b8e4 (diff)
Added encoders model.
-rw-r--r--digital/asserv/models/encoders.m195
1 files changed, 195 insertions, 0 deletions
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