summaryrefslogtreecommitdiffhomepage log msg author committer range
path: root/digital
diff options
 context: 12345678910152025303540 space: includeignore mode: unifiedssdiffstat only
author committer Nicolas Schodet 2007-09-17 14:01:00 +0200 Nicolas Schodet 2007-09-17 14:01:00 +0200 0cda1aab9df6e81f0dcd0f127633e89031d27df8 (patch) dc4395c508231a9dec2b67f58a7fec2a8a175c91 /digital 41bca85dd88b52099464b3aec1fa72a4b2afa65d (diff)
Added curve trajectory.
Diffstat (limited to 'digital')
-rw-r--r--digital/asserv/models/encoders.m41
1 files changed, 40 insertions, 1 deletions
 diff --git a/digital/asserv/models/encoders.m b/digital/asserv/models/encoders.mindex 0cd296a2..52e3ee30 100644--- a/digital/asserv/models/encoders.m+++ b/digital/asserv/models/encoders.m@@ -33,7 +33,6 @@ function reset () 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).@@ -215,6 +214,45 @@ function traj_angle (dax, asmax, aa) stop (); end +% Make a curve.+function traj_curve (dtx, dax, tsmax, ta)+ global robot;+ dtf = robot.tx + dtx;+ daf = robot.ax + dax;+ t_tsmax = tsmax / ta;+ tx_tsmax = 0.5 * ta * t_tsmax^2;+ if (tx_tsmax > dtx / 2)+ % Triangle.+ t_tsmax = sqrt (2 * 0.5 * dtx / ta);+ aa = dax / (t_tsmax^2);+ robot.ta = ta;+ robot.aa = aa;+ step_time (t_tsmax);+ robot.ta = -ta;+ robot.aa = -aa;+ step_time (t_tsmax);+ else+ % Trapeze.+ t_tscst = (dtx - 2 * tx_tsmax) / robot.ts;+ aa = dax / (t_tsmax * t_tscst + t_tsmax^2);+ robot.ta = ta;+ robot.aa = aa;+ step_time (t_tsmax);+ robot.ta = 0;+ robot.aa = 0;+ step_time (t_tscst);+ robot.ta = -ta;+ robot.aa = -aa;+ step_time (t_tsmax);+ end+ % One more step to adjust.+ %robot.ta = 2 * (txf - robot.tx - robot.ts * robot.samp) / robot.samp^2;+ %step (robot.samp);+ robot.ta = 0;+ robot.aa = 0;+ stop ();+end+ % Example path. function path () global robot;@@ -223,6 +261,7 @@ function path () traj_dist (1000, 2000, 1000); traj_angle (pi/4, 2000 / robot.radius, 1000 / robot.radius); traj_dist (1000, 2000, 1000);+ traj_curve (1000, pi, 2000, 1000); end motor;