1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
|
// This is a 'single axis trajectory planner' component for linuxcnc's HAL
// This is a conversion of linuxcnc's internal jogging planner to comp
// see motion/simple_tp.c Author: jmkasunich
// Copyright 2014 Chris Morley <chrisinnanaimo@hotmail.com>
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
component simple_tp """\
This component is a single axis simple trajectory planner, same as used for jogging in LinuxCNC.
Used by PNCconf to allow testing of acceleration and velocity values for an axis.""";
pin in float target-pos "target position to plan for.";
pin in float maxvel "Maximum velocity";
pin in float maxaccel "Acceleration rate";
pin in bit enable "If disabled, planner sets velocity to zero immediately.";
pin out float current-pos "position commanded at this point in time.";
pin out float current-vel "velocity commanded at this moment in time.";
pin out bit active "if active is true, the planner is requesting movement.";
function update;
license "GPL";
author "Chris S Morley";
;;
#include "rtapi_math.h"
FUNCTION(update) {
double max_dv, tiny_dp, pos_err, vel_req;
active = 0;
/* compute max change in velocity per servo period */
max_dv = maxaccel * fperiod;
/* compute a tiny position range, to be treated as zero */
tiny_dp = max_dv * fperiod * 0.001;
/* calculate desired velocity */
if (enable) {
/* planner enabled, request a velocity that tends to drive
pos_err to zero, but allows for stopping without position
overshoot */
pos_err = target_pos - current_pos;
/* positive and negative errors require some sign flipping to
avoid sqrt(negative) */
if (pos_err > tiny_dp) {
vel_req = -max_dv +
sqrt(2.0 * maxaccel * pos_err + max_dv * max_dv);
/* mark planner as active */
active = 1;
} else if (pos_err < -tiny_dp) {
vel_req = max_dv -
sqrt(-2.0 * maxaccel * pos_err + max_dv * max_dv);
/* mark planner as active */
active = 1;
} else {
/* within 'tiny_dp' of desired pos, no need to move */
vel_req = 0.0;
}
} else {
/* planner disabled, request zero velocity */
vel_req = 0.0;
}
/* limit velocity request */
if (vel_req > maxvel) {
vel_req = maxvel;
} else if (vel_req < -maxvel) {
vel_req = -maxvel;
}
/* ramp velocity toward request at accel limit */
if (vel_req > current_vel + max_dv) {
current_vel += max_dv;
} else if (vel_req < current_vel - max_dv) {
current_vel -= max_dv;
} else {
current_vel = vel_req;
}
/* check for still moving */
if (current_vel != 0.0) {
/* yes, mark planner active */
active = 1;
}
/* integrate velocity to get new position */
current_pos += current_vel * fperiod;
}
|
