только обратил на него внимание...
до конца еще не понял как он работает ,поэтому оставлю здесь по нему информацию
-----------------------
описание
есть примеры с sim конфигов и видео автора:
Код: Выделить всё
MOVEOFF
NAME
SYNOPSIS
DESCRIPTION
EXAMPLES
FUNCTIONS
PINS
LICENSE
NAME
moveoff − Component for Hal-only offsets
SYNOPSIS
loadrt moveoff [count=N|names=name1[,name2...]] [personality=P,P,...]
DESCRIPTION
The moveoff component is used to offset joint positions using custom Hal connections. Implementing an offset-while-program-is-paused functionality is supported with appropriate connections for the input pins. Nine joints are supported.
The axis offset pin values (offset-in-M) are continuously applied (respecting limits on value, velocity, and acceleration) to the output pins (offset-current-M, pos-plusoffset-M, fb-minusoffset-M) when both enabling input pins (apply-offsets and move-enable) are TRUE. The two enabling inputs are anded internally. A warning pin is set and a message issued if the apply-offsets pin is deasserted while offsets are applied. The warning pin remains TRUE until the offsets are removed or the apply-offsets pin is set.
Typically, the move-enable pin is connected to external controls and the apply-offsets pin is connected to halui.program.is-paused (for offsets only while paused) or set to TRUE (for continuously applied offsets).
Applied offsets are automatically returned to zero (respecting limits) when either of the enabling inputs is deactivated. The zero value tolerance is specified by the epsilon input pin value.
Waypoints are recorded when the moveoff componenent is enabled. Waypoints are managed with the waypoint-sample-secs and waypoint-threshold pins. When the backtrack-enable pin is TRUE, the auto-return path follows the recorded waypoints. When the memory available for waypoints is exhausted, offsets are frozen and the waypoint-limit pin is asserted. This restriction applies regardless of the state of the backtrack-enable pin. An enabling pin must be deasserted to allow a return to the original (non-offset position).
Backtracking through waypoints results in slower movement rates as the moves are point-to-point respecting velocity and acceleration settings. The velocity and acceleration limit pins can be managed dynamically to control offsets at all times.
When backtrack-enable is FALSE, the auto-return move is NOT coordinated, each axis returns to zero at its own rate. If a controlled path is wanted in this condition, each axis should be manually returned to zero before deasserting an enabling pin.
The waypoint-sample-secs, waypoint-threshold, and epsilon pins are evaluated only when the component is idle.
The offsets-applied output pin is provided to indicate the current state to a GUI so that program resumption can be managed. If the offset(s) are non-zero when the apply-offsets pin is deasserted (for example when resuming a program when offsetting during a pause), offsets are returned to zero (respecting limits) and an Error message is issued.
Caution: If offsets are enabled and applied and the machine is turned off for any reason, any external Hal logic that manages the enabling pins and the offset-in-M inputs is responsible for their state when the machine is subsequently turned on again.
This Hal-only means of offsetting is typically not known to LinuxCNC nor available in GUI preview displays. No protection is provided for offset moves that exceed soft limits managed by LinuxCNC. Since soft limits are not honored, an offset move may encounter hard limits (or CRASH if there are no limit switches). Use of the offset-min-M and offset-max-M inputs to limit travel is recommended. Triggering a hard limit will turn off the machine -- see Caution above.
The offset-in-M values may be set with inifile settings, controlled by a GUI, or managed by other Hal components and connections. Fixed values may be appropriate in simple cases where the direction and amount of offset is well-defined but a control method is required to deactivate an enabling pin in order to return offsets to zero. GUIs may provide means for users to set, increment, decrement, and accumulate offset values for each axis and may set offset-in-M values to zero before deasserting an enabling pin.
The default values for accel, vel, min, max, epsilon, waypoint-sample-secs, and waypoint-threshold may not be suitable for any particular application. This Hal component is unaware of limits enforced elsewhere by LinuxCNC. Users should test usage in a simulator application and understand all hazards before use on hardware.
The module personality item sets the number of joints supported (default==3, maximum is 9).
Use of the names= option for naming is required for compatibilty with the gui provided as scripts/moveoff_gui:
loadrt moveoff names=mv personality=number_of_joints
man moveoff_gui for more information
EXAMPLES
Example simulator configs that demonstrate the moveoff component and a simple gui (scripts/moveoff_gui) are located in configs/sim/axis/moveoff. The axis gui is used for the demonstrations and the configs can be adapted for other guis like touchy and gscreen. An example with the touchy gui is provided in configs/sim/touchy/ngcgui/.
FUNCTIONS
moveoff.N.read-inputs (requires a floating-point thread)
Read all inputs
moveoff.N.write-outputs (requires a floating-point thread)
Write computed offset outputs (offset-current-M, pos-plusoffset-M, fb-minusoffset-M). All other outputs are updated by read-inputs()
PINS
moveoff.N.power-on bit in
Connect to motion.motion-enabled
moveoff.N.move-enable bit in
Enable offsets (Enabling requires apply-offset TRUE also)
moveoff.N.apply-offsets bit in
Enable offsets (Enabling requires move-enable TRUE also)
moveoff.N.backtrack-enable bit in (default: 1)
Enable backtrack on auto-return
moveoff.N.epsilon float in (default: 0.0005)
When enabling pins are deactivated, return to un-offsetted position within epsilon units. Warning: values that are too small in value may cause overshoot, A minimum value of 0.0001 is silently enforced
moveoff.N.waypoint-threshold float in (default: 0.02)
Minimum distance (in a single axis) for a new waypoint
moveoff.N.waypoint-sample-secs float in (default: 0.02)
Minimum sample interval (in seconds) for a new waypoint
moveoff.N.warning bit out
Set TRUE if apply-offsets is deasserted while offset-applied is TRUE
moveoff.N.offset-applied bit out
TRUE if one or more offsets are applied
moveoff.N.waypoint-limit bit out (default: 0)
Indicates waypoint limit reached (motion ceases), an enabling pin must be deasserted to initiate return to original position
moveoff.N.waypoint-ct s32 out
Waypoint count (for debugging)
moveoff.N.waypoint-percent-used s32 out
Percent of available waypoints used
moveoff.N.offset-in-M float in (M=0..personality)
Joint offset input value
moveoff.N.pos-M float in (M=0..personality)
Joint position (typ: axis.0.motor-pos-cmd)
moveoff.N.fb-M float in (M=0..personality)
Joint feedback (typ from encoder and input to pid controller (pid.feedback))
moveoff.N.offset-current-M float out (M=0..personality)
Joint offset current value
moveoff.N.pos-plusoffset-M float out (M=0..personality)
Computed joint position plus offset (typically connect to pid command input)
moveoff.N.fb-minusoffset-M float out (M=0..personality)
Computed Joint feedback minus offset (typically connected to axis.0.motor-pos-fb
moveoff.N.offset-vel-M float in (M=0..personality) (default: 10)
Joint offset velocity limit
moveoff.N.offset-accel-M float in (M=0..personality) (default: 100)
Joint offset acceleration limit
moveoff.N.offset-min-M float in (M=0..personality) (default: -1e20)
Minimum limit for applied joint offset (typ negative)
moveoff.N.offset-max-M float in (M=0..personality) (default: 1e20)
Maximum limit for applied offset (typ positive)
moveoff.N.dbg-waypoint-limit-test bit in
Debug input to test with limited number of waypoints
moveoff.N.dbg-state s32 out
Debug output for current state of state machine
LICENSE
GPLКод: Выделить всё
# halfile to connect a moveoff.comp for axis gui examples
# This halfile modifies the conections made in core_sim.hal
# to add a moveoff component and addfs its functions
# This halfile must follow core_sim.hal
# moveoff offsetting component (use names=mv is required):
loadrt moveoff names=mv
# addf the moveoff funcs in correct sequence:
addf mv.write-outputs servo-thread 3
addf mv.read-inputs servo-thread 1
# modify connections set in core_sim.hal
unlinkp axis.0.motor-pos-cmd
unlinkp axis.1.motor-pos-cmd
unlinkp axis.2.motor-pos-cmd
unlinkp axis.0.motor-pos-fb
unlinkp axis.1.motor-pos-fb
unlinkp axis.2.motor-pos-fb
# connect motor-pos-cmd pins to the component:
net Xpos axis.0.motor-pos-cmd => mv.pos-0
net Ypos axis.1.motor-pos-cmd => mv.pos-1
net Zpos axis.2.motor-pos-cmd => mv.pos-2
# generated positions with offsets ADDED are
# 'short circuited' to the feedback input pins:
net mx:plus mv.pos-plusoffset-0 => mv.fb-0
net my:plus mv.pos-plusoffset-1 => mv.fb-1
net mz:plus mv.pos-plusoffset-2 => mv.fb-2
# synthesized feedback values with offsets SUBTRACTED
# are sent back to motion:
net mx:fb mv.fb-minusoffset-0 => axis.0.motor-pos-fb
net my:fb mv.fb-minusoffset-1 => axis.1.motor-pos-fb
net mz:fb mv.fb-minusoffset-2 => axis.2.motor-pos-fb
# example setting global inputs from ini file:
setp mv.epsilon [MOVEOFF]EPSILON
setp mv.waypoint-sample-secs [MOVEOFF]WAYPOINT_SAMPLE_SECS
setp mv.waypoint-threshold [MOVEOFF]WAYPOINT_THRESHOLD
# offset rates from ini file:
setp mv.offset-vel-0 [MOVEOFF_0]MAX_VELOCITY
setp mv.offset-vel-1 [MOVEOFF_1]MAX_VELOCITY
setp mv.offset-vel-2 [MOVEOFF_2]MAX_VELOCITY
# offset accels from ini file:
setp mv.offset-accel-0 [MOVEOFF_0]MAX_ACCELERATION
setp mv.offset-accel-1 [MOVEOFF_1]MAX_ACCELERATION
setp mv.offset-accel-2 [MOVEOFF_2]MAX_ACCELERATION
# offset min, max limits
setp mv.offset-min-0 [MOVEOFF_0]MIN_LIMIT
setp mv.offset-min-1 [MOVEOFF_1]MIN_LIMIT
setp mv.offset-min-2 [MOVEOFF_2]MIN_LIMIT
setp mv.offset-max-0 [MOVEOFF_0]MAX_LIMIT
setp mv.offset-max-1 [MOVEOFF_1]MAX_LIMIT
setp mv.offset-max-2 [MOVEOFF_2]MAX_LIMIT
# overall enable for the component:
# 1 ==> enabled as soon as apply-offsets asserted
# 0 ==> external control of move-enable required
setp mv.move-enable 0
# offset values (set as required with your gui)
setp mv.offset-in-0 0
setp mv.offset-in-1 0
setp mv.offset-in-2 0
Код: Выделить всё
component moveoff "Component for Hal-only offsets";
description
"""
The moveoff component is used to offset joint positions using custom Hal
connections. Implementing an offset-while-program-is-paused functionality is
supported with appropriate connections for the input pins. Nine joints are
supported.
The axis offset pin values (offset-in-M) are continuously applied (respecting
limits on value, velocity, and acceleration) to the output pins
(offset-current-M, pos-plusoffset-M, fb-minusoffset-M) when both enabling input
pins (apply-offsets and move-enable) are TRUE. The two enabling inputs are
anded internally. A \\fBwarning\\fR pin is set and a message issued if the
apply-offsets pin is deasserted while offsets are applied. The warning pin
remains TRUE until the offsets are removed or the apply-offsets pin is set.
Typically, the move-enable pin is connected to external controls and the
apply-offsets pin is connected to halui.program.is-paused (for offsets only
while paused) or set to TRUE (for continuously applied offsets).
Applied offsets are \\fBautomatically returned\\fR to zero (respecting limits)
when either of the enabling inputs is deactivated. The zero value
tolerance is specified by the epsilon input pin value.
Waypoints are recorded when the moveoff componenent is enabled. Waypoints are
managed with the waypoint-sample-secs and waypoint-threshold pins. When the
backtrack-enable pin is TRUE, the auto-return path follows the recorded
waypoints. When the memory available for waypoints is exhausted, offsets are
frozen and the waypoint-limit pin is asserted. This restriction applies
regardless of the state of the backtrack-enable pin. An enabling pin must be
deasserted to allow a return to the original (non-offset position).
Backtracking through waypoints results in \\fBslower\\fR movement rates as the
moves are point-to-point respecting velocity and acceleration settings. The
velocity and acceleration limit pins can be managed dynamically to control
offsets at all times.
When backtrack-enable is FALSE, the auto-return move is \\fBNOT\\fR
coordinated, each axis returns to zero at its own rate. If a controlled path
is wanted in this condition, each axis should be manually returned to zero
before deasserting an enabling pin.
The waypoint-sample-secs, waypoint-threshold, and epsilon pins are evaluated
only when the component is idle.
The offsets-applied output pin is provided to indicate the current state to a
GUI so that program resumption can be managed. If the offset(s) are non-zero
when the apply-offsets pin is deasserted (for example when resuming a program
when offsetting during a pause), offsets are returned to zero (respecting
limits) and an \\fBError\\fR message is issued.
\\fBCaution:\\fR If offsets are enabled and applied and the machine is turned off
for any reason, any \\fBexternal\\fR Hal logic that manages the enabling pins
and the offset-in-M inputs is responsible for their state when the
machine is subsequently turned on again.
This Hal-only means of offsetting is typically not known to LinuxCNC
nor available in GUI preview displays. \\fBNo protection is provided\\fR for
offset moves that exceed soft limits managed by LinuxCNC. Since soft limits
are not honored, an offset move may encounter hard limits (or \\fBCRASH\\fR if
there are no limit switches). Use of the offset-min-M and offset-max-M inputs
to limit travel is recommended. Triggering a hard limit will turn off
the machine -- see \\fBCaution\\fR above.
The offset-in-M values may be set with inifile settings, controlled by a GUI,
or managed by other Hal components and connections. Fixed values may be
appropriate in simple cases where the direction and amount of offset is
well-defined but a control method is required to deactivate an enabling
pin in order to return offsets to zero. GUIs may provide means for users to
set, increment, decrement, and accumulate offset values for each axis and may
set offset-in-M values to zero before deasserting an enabling pin.
The default values for accel, vel, min, max, epsilon, waypoint-sample-secs, and
waypoint-threshold may not be suitable for any particular application. This
Hal component is unaware of limits enforced elsewhere by LinuxCNC.
Users should test usage in a simulator application and understand all
hazards \\fBbefore\\fR use on hardware.
The module personality item sets the number of joints supported (default==3,
maximum is 9).
Use of the names= option for naming is \\fBrequired\\fR for compatibilty
with the gui provided as scripts/moveoff_gui:
loadrt moveoff names=\\fBmv\\fR personality=number_of_joints
""";
//"""" quote char for vim highlighting
/*
Copyright: 2014-2015
Authors: Dewey Garrett <dgarrett@panix.com>, Andy Pugh <bodgesoc@gmail.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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
pin in bit power_on "Connect to motion.motion-enabled";
pin in bit move_enable "Enable offsets (Enabling requires apply-offset TRUE also)";
pin in bit apply_offsets "Enable offsets (Enabling requires move-enable TRUE also)";
pin in bit backtrack-enable = 1 "Enable backtrack on auto-return";
pin in float epsilon=0.0005 "When enablin pins are deactivated, return to un-offsetted position within epsilon units. Warning: values that are too small in value may cause overshoot, A minimum value of 0.0001 is \\fBsilently enforced\\fR";
pin in float waypoint-threshold = 0.02 "Minimum distance (in a single axis) for a new waypoint";
pin in float waypoint-sample-secs = 0.02 "Minimum sample interval (in seconds) for a new waypoint";
pin out bit warning "Set TRUE if apply-offsets is deasserted while offset-applied is TRUE";
pin out bit offset_applied "TRUE if one or more offsets are applied";
pin out bit waypoint-limit = 0 "Indicates waypoint limit reached (motion ceases), an enabling pin must be deasserted to initiate return to original position";
pin out s32 waypoint-ct "Waypoint count (for debugging)";
pin out s32 waypoint-percent-used "Percent of available waypoints used";
pin in float offset-in-#[9 : personality] "Joint offset input value";
pin in float pos-#[9 : personality] "Joint position (typ: axis.0.motor-pos-cmd)";
pin in float fb-#[9 : personality] "Joint feedback (typ from encoder and input to pid controller (pid.feedback))";
pin out float offset-current-#[9 : personality]"Joint offset current value";
pin out float pos-plusoffset-#[9 : personality] "Computed joint position plus offset (typically connect to pid command input)";
pin out float fb-minusoffset-#[9 : personality] "Computed Joint feedback minus offset (typically connected to axis.0.motor-pos-fb";
pin in float offset-vel-#[9 : personality]=10 "Joint offset velocity limit";
pin in float offset-accel-#[9 : personality]=100 "Joint offset acceleration limit";
pin in float offset-min-#[9 : personality] = -1e20 "Minimum limit for applied joint offset (typ negative)";
pin in float offset-max-#[9 : personality] = 1e20 "Maximum limit for applied offset (typ positive)";
// inputs for debugging:
pin in bit dbg_waypoint_limit_test "Debug input to test with limited number of waypoints";
// outputs for debugging:
pin out s32 dbg_state "Debug output for current state of state machine";
option personality;
option extra_setup;
function read_inputs "Read all inputs";
function write_outputs "Write computed offset outputs\n(offset-current-M, pos-plusoffset-M, fb-minusoffset-M).\nAll other outputs are updated by read-inputs()";
license "GPL";
;;
#include "rtapi_math.h"
#define NCHANNELS 9
#define NWAYPOINTS 1000
#define TST_NWAYPOINTS 50
#define MIN_EPSILON 0.0001
typedef enum {
IDLE,
MOVE_AWAY,
MOVE_BACK,
} the_state;
typedef struct {
int state[NCHANNELS];
hal_float_t old_in[NCHANNELS];
hal_float_t old_out[NCHANNELS];
hal_float_t old_v[NCHANNELS];
hal_float_t old_limited_in[NCHANNELS];
} old_values_t;
struct lim3_input {
hal_float_t minlimit;
hal_float_t maxlimit;
hal_float_t maxvlimit;
hal_float_t maxalimit;
hal_float_t in;
hal_float_t old_in;
hal_float_t old_out;
hal_float_t old_v;
};
static void reset_old(int nchan, old_values_t* d) {
int i;
for (i = 0;i < nchan; i++) {
d->old_in[i] = 0;
d->old_out[i] = 0;
d->old_v[i] = 0;
d->old_limited_in[i] = 0;
}
}
static int offset_removed(int nchan, old_values_t d, hal_float_t eps) {
int removed = 1;
int i;
for (i = 0 ; i < nchan ; i++) {
if (fabs(d.old_out[i]) > eps) {
removed = 0;
break;
}
}
return removed;
}
static int at_waypoint(int nchan,
hal_float_t g[],
hal_float_t p[],hal_float_t eps) {
int within_eps = 1;
int i;
for (i = 0 ; i < nchan ; i++) {
if ( fabs(p[i] - g[i]) > eps ) {
within_eps = 0;
break;
}
}
return within_eps;
}
static long theperiod;
static void lim3(struct lim3_input input,
hal_float_t* old_in,
hal_float_t* limited_out,
hal_float_t* old_v
) {
/* following code is adapted from limit3.comp */
hal_float_t dt = theperiod * 1e-9;
hal_float_t in_v, min_v, max_v, avg_v;
hal_float_t min_out,max_out;
hal_float_t ramp_a, match_time, est_in, est_out;
hal_float_t err, dv, dp;
hal_float_t limited_in;
/* apply first order limit */
limited_in = input.in;
if (input.in < input.minlimit) {
limited_in = input.minlimit;
}
if (input.in > input.maxlimit) {
limited_in = input.maxlimit;
}
*old_in = limited_in;
/* calculate input derivative */
in_v = (limited_in - input.old_in) / dt;
/* determine v and out that can be reached in one period */
min_v = input.old_v - input.maxalimit * dt;
if (min_v < -input.maxvlimit) {
min_v = -input.maxvlimit;
}
max_v = input.old_v + input.maxalimit * dt;
if (max_v > input.maxvlimit) {
max_v = input.maxvlimit;
}
min_out = input.old_out + min_v * dt;
max_out = input.old_out + max_v * dt;
if ( ( limited_in >= min_out ) && ( limited_in <= max_out )
&& ( in_v >= min_v ) && ( in_v <= max_v ) ) {
/* we can follow the command without hitting a limit */
*limited_out = limited_in;
*old_v = ( *limited_out - input.old_out ) / dt;
} else {
/* can't follow commanded path while obeying limits */
/* determine which way we need to ramp to match v */
if ( in_v > input.old_v ) {
ramp_a = input.maxalimit;
} else {
ramp_a = -input.maxalimit;
}
/* determine how long the match would take */
match_time = ( in_v - input.old_v ) / ramp_a;
/* where we will be at the end of the match */
avg_v = ( in_v + input.old_v + ramp_a * dt ) * 0.5;
est_out = input.old_out + avg_v * match_time;
/* calculate the expected command position at that time */
est_in = input.old_in + in_v * match_time;
/* calculate position error at that time */
err = est_out - est_in;
/* calculate change in final position if we ramp in the
opposite direction for one period */
dv = -2.0 * ramp_a * dt;
dp = dv * match_time;
/* decide what to do */
if ( fabs(err + dp*2.0) < fabs(err) ) {
ramp_a = -ramp_a;
}
if ( ramp_a < 0.0 ) {
*limited_out = min_out;
*old_v = min_v;
} else {
*limited_out = max_out;
*old_v = max_v;
}
}
return;
}
static the_state state = IDLE;
static int next_waypoint_index;
static rtapi_s64 last_waypoint_time;
static rtapi_s64 now;
static hal_float_t time_since_last_sample;
static hal_float_t move_threshold;
static hal_float_t min_sample_interval;
static bool backtrack;
static bool gave_msg;
static int max_waypoints = NWAYPOINTS;
static struct lim3_input input;
static hal_float_t goal[NCHANNELS];
static hal_float_t waypoints[NCHANNELS][NWAYPOINTS];
static old_values_t data;
static hal_float_t eps_in_use;
static bool move_in_progress = 0;
static the_state next_state;
//compile time setting:
static const bool allow_backtrack_enable_change = 1;
// 1 ==> backtrack-enable can be changed while enabled
// waypoints are always accumulated and waypoint_limit enforced
// 0 ==> backtrack-enable is sampled only while IDLE
// if backtrack-enable == 0, no waypoints and no waypoint_limit
// (e.g., unlimited no. of offset samples)
FUNCTION(read_inputs) {
hal_float_t last,delta;
int r;
bool all_enables = power_on && move_enable && apply_offsets;
if (allow_backtrack_enable_change) {
backtrack = backtrack_enable;
// change at any time (controls auto-return)
}
theperiod = period;
now = rtapi_get_time();
if (state == IDLE) {
backtrack = backtrack_enable; // ref: allow_backtrack_enable_change
// allow changes only when IDLE for these inputs:
move_threshold = waypoint_threshold;
min_sample_interval = waypoint_sample_secs;
eps_in_use = epsilon;
if (eps_in_use < MIN_EPSILON) { eps_in_use = MIN_EPSILON; }
if (dbg_waypoint_limit_test) {
max_waypoints = TST_NWAYPOINTS; // tiny limit for testing
} else {
max_waypoints = NWAYPOINTS;
}
}
if (backtrack || allow_backtrack_enable_change) {
int r;
bool sufficient_movement_for_new_waypoint = 0;
time_since_last_sample = (hal_float_t)(now - last_waypoint_time)/1e9;
switch (state) {
case IDLE:
next_waypoint_index = 0;
break;
case MOVE_AWAY:
//note: gui must handle waypoint_limit -- here we just stop
if (waypoint_limit) break; // no more room for waypoints
if (time_since_last_sample < min_sample_interval) break;
for (r=0; r < personality; r++) {
last = waypoints[r][next_waypoint_index - 1];
delta = fabs(offset_current(r) - last);
if (delta > move_threshold) {
sufficient_movement_for_new_waypoint = 1;
break; //for loop
}
}
if (!sufficient_movement_for_new_waypoint) break;
for (r=0; r < personality; r++) {
waypoints[r][next_waypoint_index] = offset_current(r);
}
last_waypoint_time = now;
next_waypoint_index++;
if (next_waypoint_index > max_waypoints - 1) {
waypoint_limit = 1;
} else {
waypoint_limit = 0;
}
break;
case MOVE_BACK: break;
}
} //end save waypoints
//{begin state control
switch (state) {
case IDLE:
last_waypoint_time = now;
if ( all_enables ) {
next_state = MOVE_AWAY;
move_in_progress = 1;
for (r = 0; r < personality; r++) {
goal[r] = offset_in(r);
}
}
break;
case MOVE_AWAY:
move_in_progress = 1;
if ( all_enables ) {
// allow offset movements
for (r = 0; r < personality; r++) {
goal[r] = offset_in(r);
}
break;
}
// one (or more) enablers is gone
next_state = MOVE_BACK;
if (!power_on) {
reset_old(personality, &data);
move_in_progress = 0;
offset_applied = 0;
next_state = IDLE;
break;
}
for (r = 0; r < personality; r++) {
goal[r] = 0; // default (eg not waypoint backtrack)
}
if (backtrack) {
if ( next_waypoint_index > 0 ) {
for (r = 0; r < personality; r++) {
goal[r] = waypoints[r][next_waypoint_index-1];
}
waypoint_limit = 0;
next_waypoint_index--;
}
}
break;
case MOVE_BACK:
if (!power_on) {
reset_old(personality, &data);
move_in_progress = 0;
offset_applied = 0;
next_state = IDLE;
break;
}
move_in_progress = 1;
if (backtrack) {
if ( next_waypoint_index > 0 ) {
hal_float_t pcur[NCHANNELS];
for (r=0; r < personality; r++) {
pcur[r] = offset_current(r);
}
if (at_waypoint(personality, goal, pcur, eps_in_use)) {
for (r = 0; r < personality; r++) {
goal[r] = waypoints[r][next_waypoint_index];
}
next_waypoint_index--;
}
} else {
for (r = 0; r < personality; r++) {
goal[r] = 0; //final goal
}
}
}
if (!offset_applied) {
// offsets gone, return to IDLE
next_state = IDLE;
move_in_progress = 0;
reset_old(personality, &data);
}
break;
}
if ( !apply_offsets
&& offset_applied
) {
warning = 1;
if (!gave_msg) {
// apply_offsets deasserted while offset_applied
// for example:
// 1) program stopped with offsets applied
// 2) (no *.resume-inhibit pin) or (-no_resume_inhibit option)
// and program resumed with offsets applied
for (r = 0; r < personality; r++) {
rtapi_print_msg(RTAPI_MSG_ERR,
"Index: %i, offset=%f",
r, data.old_out[r]);
}
rtapi_print_msg(RTAPI_MSG_ERR,
"apply_offsets deasserted before offsets removed "
"moveoff.comp: WARNING"
);
gave_msg = 1;
}
} else {
gave_msg = 0;
warning = 0;
}
//}end state control
} //read_inputs
FUNCTION(write_outputs) {
int r;
// move with limits on postion, velocity, acceleration
for (r = 0; r < personality; r++) {
if (move_in_progress) {
input.in = goal[r];
input.minlimit = offset_min(r);
input.maxlimit = offset_max(r);
input.maxvlimit = offset_vel(r);
input.maxalimit = offset_accel(r);
input.old_in = data.old_in[r];
input.old_out = data.old_out[r];
input.old_v = data.old_v[r];
if ( waypoint_limit && (state == MOVE_AWAY) ) {
// no movement in MOVE_AWAY (require: remove enable)
} else {
hal_float_t last_old = data.old_out[r];
lim3(input,
&data.old_in[r],
&data.old_out[r],
&data.old_v[r]
);
offset_current(r) = data.old_out[r];
fb_minusoffset(r) = fb(r) - offset_current(r)
- (last_old - data.old_out[r]);
pos_plusoffset(r) = pos(r) + offset_current(r);
}
} else {
pos_plusoffset(r) = pos(r);
offset_current(r) = 0;
fb_minusoffset(r) = fb(r);
}
}
offset_applied = ! offset_removed(personality, data, eps_in_use);
if ( !offset_applied ) {
// reset backtrack
next_waypoint_index = 0;
waypoint_limit = 0;
}
waypoint_ct = next_waypoint_index;
waypoint_percent_used = 100*next_waypoint_index/max_waypoints;
dbg_state = state;
state = next_state;
} //write_outputs
EXTRA_SETUP() {
if (personality == 0) personality = 3;
return 0;
}
.
