moveoff.comp
1 component moveoff "Component for Hal-only offsets"; 2 3 description 4 """ 5 The moveoff component is used to offset joint positions using custom Hal 6 connections. Implementing an offset-while-program-is-paused functionality is 7 supported with appropriate connections for the input pins. Nine joints are 8 supported. 9 10 The axis offset pin values (offset-in-M) are continuously applied (respecting 11 limits on value, velocity, and acceleration) to the output pins 12 (offset-current-M, pos-plusoffset-M, fb-minusoffset-M) when both enabling input 13 pins (apply-offsets and move-enable) are TRUE. The two enabling inputs are 14 anded internally. A \\fBwarning\\fR pin is set and a message issued if the 15 apply-offsets pin is deasserted while offsets are applied. The warning pin 16 remains TRUE until the offsets are removed or the apply-offsets pin is set. 17 18 Typically, the move-enable pin is connected to external controls and the 19 apply-offsets pin is connected to halui.program.is-paused (for offsets only 20 while paused) or set to TRUE (for continuously applied offsets). 21 22 Applied offsets are \\fBautomatically returned\\fR to zero (respecting limits) 23 when either of the enabling inputs is deactivated. The zero value 24 tolerance is specified by the epsilon input pin value. 25 26 Waypoints are recorded when the moveoff componenent is enabled. Waypoints are 27 managed with the waypoint-sample-secs and waypoint-threshold pins. When the 28 backtrack-enable pin is TRUE, the auto-return path follows the recorded 29 waypoints. When the memory available for waypoints is exhausted, offsets are 30 frozen and the waypoint-limit pin is asserted. This restriction applies 31 regardless of the state of the backtrack-enable pin. An enabling pin must be 32 deasserted to allow a return to the original (non-offset position). 33 34 Backtracking through waypoints results in \\fBslower\\fR movement rates as the 35 moves are point-to-point respecting velocity and acceleration settings. The 36 velocity and acceleration limit pins can be managed dynamically to control 37 offsets at all times. 38 39 When backtrack-enable is FALSE, the auto-return move is \\fBNOT\\fR 40 coordinated, each axis returns to zero at its own rate. If a controlled path 41 is wanted in this condition, each axis should be manually returned to zero 42 before deasserting an enabling pin. 43 44 The waypoint-sample-secs, waypoint-threshold, and epsilon pins are evaluated 45 only when the component is idle. 46 47 The offsets-applied output pin is provided to indicate the current state to a 48 GUI so that program resumption can be managed. If the offset(s) are non-zero 49 when the apply-offsets pin is deasserted (for example when resuming a program 50 when offsetting during a pause), offsets are returned to zero (respecting 51 limits) and an \\fBError\\fR message is issued. 52 53 \\fBCaution:\\fR If offsets are enabled and applied and the machine is turned off 54 for any reason, any \\fBexternal\\fR Hal logic that manages the enabling pins 55 and the offset-in-M inputs is responsible for their state when the 56 machine is subsequently turned on again. 57 58 This Hal-only means of offsetting is typically not known to LinuxCNC 59 nor available in GUI preview displays. \\fBNo protection is provided\\fR for 60 offset moves that exceed soft limits managed by LinuxCNC. Since soft limits 61 are not honored, an offset move may encounter hard limits (or \\fBCRASH\\fR if 62 there are no limit switches). Use of the offset-min-M and offset-max-M inputs 63 to limit travel is recommended. Triggering a hard limit will turn off 64 the machine -- see \\fBCaution\\fR above. 65 66 The offset-in-M values may be set with inifile settings, controlled by a GUI, 67 or managed by other Hal components and connections. Fixed values may be 68 appropriate in simple cases where the direction and amount of offset is 69 well-defined but a control method is required to deactivate an enabling 70 pin in order to return offsets to zero. GUIs may provide means for users to 71 set, increment, decrement, and accumulate offset values for each axis and may 72 set offset-in-M values to zero before deasserting an enabling pin. 73 74 The default values for accel, vel, min, max, epsilon, waypoint-sample-secs, and 75 waypoint-threshold may not be suitable for any particular application. This 76 Hal component is unaware of limits enforced elsewhere by LinuxCNC. 77 Users should test usage in a simulator application and understand all 78 hazards \\fBbefore\\fR use on hardware. 79 80 The module personality item sets the number of joints supported (default==3, 81 maximum is 9). 82 83 Use of the names= option for naming is \\fBrequired\\fR for compatibility 84 with the gui provided as scripts/moveoff_gui: 85 loadrt moveoff names=\\fBmv\\fR personality=number_of_joints 86 87 \\fBman moveoff_gui\\fR for more information 88 89 .SH EXAMPLES 90 Example simulator configs that demonstrate the moveoff component and a simple gui 91 (scripts/moveoff_gui) are located in configs/sim/axis/moveoff. The axis gui is 92 used for the demonstrations and the configs can be adapted for other guis like 93 touchy and gscreen. An example with the touchy gui is provided in 94 configs/sim/touchy/ngcgui/. 95 96 """; 97 98 //"""" quote char for vim highlighting 99 100 /* 101 Copyright: 2014-2015 102 Authors: Dewey Garrett <dgarrett@panix.com>, Andy Pugh <bodgesoc@gmail.com> 103 104 This program is free software; you can redistribute it and/or modify 105 it under the terms of the GNU General Public License as published by 106 the Free Software Foundation; either version 2 of the License, or 107 (at your option) any later version. 108 109 This program is distributed in the hope that it will be useful, 110 but WITHOUT ANY WARRANTY; without even the implied warranty of 111 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 112 GNU General Public License for more details. 113 114 You should have received a copy of the GNU General Public License 115 along with this program; if not, write to the Free Software 116 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 117 */ 118 119 pin in bit power_on "Connect to motion.motion-enabled"; 120 pin in bit move_enable "Enable offsets (Enabling requires apply-offset TRUE also)"; 121 pin in bit apply_offsets "Enable offsets (Enabling requires move-enable TRUE also)"; 122 pin in bit backtrack-enable = 1 "Enable backtrack on auto-return"; 123 124 pin in float epsilon=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 \\fBsilently enforced\\fR"; 125 pin in float waypoint-threshold = 0.02 "Minimum distance (in a single axis) for a new waypoint"; 126 pin in float waypoint-sample-secs = 0.02 "Minimum sample interval (in seconds) for a new waypoint"; 127 128 pin out bit warning "Set TRUE if apply-offsets is deasserted while offset-applied is TRUE"; 129 pin out bit offset_applied "TRUE if one or more offsets are applied"; 130 pin out bit waypoint-limit = 0 "Indicates waypoint limit reached (motion ceases), an enabling pin must be deasserted to initiate return to original position"; 131 pin out s32 waypoint-ct "Waypoint count (for debugging)"; 132 pin out s32 waypoint-percent-used "Percent of available waypoints used"; 133 134 pin in float offset-in-#[9 : personality] "Joint offset input value"; 135 pin in float pos-#[9 : personality] "Joint position (typ: axis.0.motor-pos-cmd)"; 136 pin in float fb-#[9 : personality] "Joint feedback (typ from encoder and input to pid controller (pid.feedback))"; 137 138 pin out float offset-current-#[9 : personality]"Joint offset current value"; 139 pin out float pos-plusoffset-#[9 : personality] "Computed joint position plus offset (typically connect to pid command input)"; 140 pin out float fb-minusoffset-#[9 : personality] "Computed Joint feedback minus offset (typically connected to axis.0.motor-pos-fb"; 141 142 pin in float offset-vel-#[9 : personality]=10 "Joint offset velocity limit"; 143 pin in float offset-accel-#[9 : personality]=100 "Joint offset acceleration limit"; 144 pin in float offset-min-#[9 : personality] = -1e20 "Minimum limit for applied joint offset (typ negative)"; 145 pin in float offset-max-#[9 : personality] = 1e20 "Maximum limit for applied offset (typ positive)"; 146 147 // inputs for debugging: 148 pin in bit dbg_waypoint_limit_test "Debug input to test with limited number of waypoints"; 149 150 // outputs for debugging: 151 pin out s32 dbg_state "Debug output for current state of state machine"; 152 153 option personality; 154 option extra_setup; 155 function read_inputs "Read all inputs"; 156 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()"; 157 158 license "GPL"; 159 ;; 160 161 #include "rtapi_math.h" 162 #define NCHANNELS 9 163 #define NWAYPOINTS 1000 164 #define TST_NWAYPOINTS 50 165 #define MIN_EPSILON 0.0001 166 167 typedef enum { 168 IDLE, 169 MOVE_AWAY, 170 MOVE_BACK, 171 } the_state; 172 173 typedef struct { 174 int state[NCHANNELS]; 175 hal_float_t old_in[NCHANNELS]; 176 hal_float_t old_out[NCHANNELS]; 177 hal_float_t old_v[NCHANNELS]; 178 hal_float_t old_limited_in[NCHANNELS]; 179 } old_values_t; 180 181 struct lim3_input { 182 hal_float_t minlimit; 183 hal_float_t maxlimit; 184 hal_float_t maxvlimit; 185 hal_float_t maxalimit; 186 hal_float_t in; 187 hal_float_t old_in; 188 hal_float_t old_out; 189 hal_float_t old_v; 190 }; 191 192 static void reset_old(int nchan, old_values_t* d) { 193 int i; 194 for (i = 0;i < nchan; i++) { 195 d->old_in[i] = 0; 196 d->old_out[i] = 0; 197 d->old_v[i] = 0; 198 d->old_limited_in[i] = 0; 199 } 200 } 201 202 static int offset_removed(int nchan, old_values_t d, hal_float_t eps) { 203 int removed = 1; 204 int i; 205 for (i = 0 ; i < nchan ; i++) { 206 if (fabs(d.old_out[i]) > eps) { 207 removed = 0; 208 break; 209 } 210 } 211 return removed; 212 } 213 214 static int at_waypoint(int nchan, 215 hal_float_t g[], 216 hal_float_t p[],hal_float_t eps) { 217 int within_eps = 1; 218 int i; 219 220 for (i = 0 ; i < nchan ; i++) { 221 if ( fabs(p[i] - g[i]) > eps ) { 222 within_eps = 0; 223 break; 224 } 225 } 226 return within_eps; 227 } 228 229 static long theperiod; 230 static void lim3(struct lim3_input input, 231 hal_float_t* old_in, 232 hal_float_t* limited_out, 233 hal_float_t* old_v 234 ) { 235 /* following code is adapted from limit3.comp */ 236 hal_float_t dt = theperiod * 1e-9; 237 hal_float_t in_v, min_v, max_v, avg_v; 238 hal_float_t min_out,max_out; 239 hal_float_t ramp_a, match_time, est_in, est_out; 240 hal_float_t err, dv, dp; 241 hal_float_t limited_in; 242 243 /* apply first order limit */ 244 limited_in = input.in; 245 if (input.in < input.minlimit) { 246 limited_in = input.minlimit; 247 } 248 if (input.in > input.maxlimit) { 249 limited_in = input.maxlimit; 250 } 251 *old_in = limited_in; 252 253 /* calculate input derivative */ 254 in_v = (limited_in - input.old_in) / dt; 255 256 /* determine v and out that can be reached in one period */ 257 min_v = input.old_v - input.maxalimit * dt; 258 if (min_v < -input.maxvlimit) { 259 min_v = -input.maxvlimit; 260 } 261 max_v = input.old_v + input.maxalimit * dt; 262 if (max_v > input.maxvlimit) { 263 max_v = input.maxvlimit; 264 } 265 266 min_out = input.old_out + min_v * dt; 267 max_out = input.old_out + max_v * dt; 268 if ( ( limited_in >= min_out ) && ( limited_in <= max_out ) 269 && ( in_v >= min_v ) && ( in_v <= max_v ) ) { 270 /* we can follow the command without hitting a limit */ 271 *limited_out = limited_in; 272 *old_v = ( *limited_out - input.old_out ) / dt; 273 } else { 274 /* can't follow commanded path while obeying limits */ 275 /* determine which way we need to ramp to match v */ 276 if ( in_v > input.old_v ) { 277 ramp_a = input.maxalimit; 278 } else { 279 ramp_a = -input.maxalimit; 280 } 281 /* determine how long the match would take */ 282 match_time = ( in_v - input.old_v ) / ramp_a; 283 /* where we will be at the end of the match */ 284 285 avg_v = ( in_v + input.old_v + ramp_a * dt ) * 0.5; 286 est_out = input.old_out + avg_v * match_time; 287 /* calculate the expected command position at that time */ 288 est_in = input.old_in + in_v * match_time; 289 /* calculate position error at that time */ 290 err = est_out - est_in; 291 /* calculate change in final position if we ramp in the 292 opposite direction for one period */ 293 dv = -2.0 * ramp_a * dt; 294 dp = dv * match_time; 295 /* decide what to do */ 296 if ( fabs(err + dp*2.0) < fabs(err) ) { 297 ramp_a = -ramp_a; 298 } 299 if ( ramp_a < 0.0 ) { 300 *limited_out = min_out; 301 *old_v = min_v; 302 } else { 303 *limited_out = max_out; 304 *old_v = max_v; 305 } 306 } 307 return; 308 } 309 310 static the_state state = IDLE; 311 static int next_waypoint_index; 312 static rtapi_s64 last_waypoint_time; 313 static rtapi_s64 now; 314 static hal_float_t time_since_last_sample; 315 static hal_float_t move_threshold; 316 static hal_float_t min_sample_interval; 317 static bool backtrack; 318 static bool gave_msg; 319 static int max_waypoints = NWAYPOINTS; 320 static struct lim3_input input; 321 static hal_float_t goal[NCHANNELS]; 322 static hal_float_t waypoints[NCHANNELS][NWAYPOINTS]; 323 static old_values_t data; 324 325 static hal_float_t eps_in_use; 326 static bool move_in_progress = 0; 327 static the_state next_state; 328 329 //compile time setting: 330 static const bool allow_backtrack_enable_change = 1; 331 // 1 ==> backtrack-enable can be changed while enabled 332 // waypoints are always accumulated and waypoint_limit enforced 333 // 0 ==> backtrack-enable is sampled only while IDLE 334 // if backtrack-enable == 0, no waypoints and no waypoint_limit 335 // (e.g., unlimited no. of offset samples) 336 337 FUNCTION(read_inputs) { 338 hal_float_t last,delta; 339 int r; 340 bool all_enables = power_on && move_enable && apply_offsets; 341 342 if (allow_backtrack_enable_change) { 343 backtrack = backtrack_enable; 344 // change at any time (controls auto-return) 345 } 346 347 theperiod = period; 348 now = rtapi_get_time(); 349 if (state == IDLE) { 350 backtrack = backtrack_enable; // ref: allow_backtrack_enable_change 351 // allow changes only when IDLE for these inputs: 352 move_threshold = waypoint_threshold; 353 min_sample_interval = waypoint_sample_secs; 354 eps_in_use = epsilon; 355 if (eps_in_use < MIN_EPSILON) { eps_in_use = MIN_EPSILON; } 356 357 358 if (dbg_waypoint_limit_test) { 359 max_waypoints = TST_NWAYPOINTS; // tiny limit for testing 360 } else { 361 max_waypoints = NWAYPOINTS; 362 } 363 } 364 365 if (backtrack || allow_backtrack_enable_change) { 366 int r; 367 bool sufficient_movement_for_new_waypoint = 0; 368 369 time_since_last_sample = (hal_float_t)(now - last_waypoint_time)/1e9; 370 switch (state) { 371 case IDLE: 372 next_waypoint_index = 0; 373 break; 374 case MOVE_AWAY: 375 //note: gui must handle waypoint_limit -- here we just stop 376 if (waypoint_limit) break; // no more room for waypoints 377 378 if (time_since_last_sample < min_sample_interval) break; 379 for (r=0; r < personality; r++) { 380 last = waypoints[r][next_waypoint_index - 1]; 381 delta = fabs(offset_current(r) - last); 382 if (delta > move_threshold) { 383 sufficient_movement_for_new_waypoint = 1; 384 break; //for loop 385 } 386 } 387 if (!sufficient_movement_for_new_waypoint) break; 388 for (r=0; r < personality; r++) { 389 waypoints[r][next_waypoint_index] = offset_current(r); 390 } 391 last_waypoint_time = now; 392 next_waypoint_index++; 393 if (next_waypoint_index > max_waypoints - 1) { 394 waypoint_limit = 1; 395 } else { 396 waypoint_limit = 0; 397 } 398 break; 399 case MOVE_BACK: break; 400 } 401 } //end save waypoints 402 403 //{begin state control 404 switch (state) { 405 case IDLE: 406 last_waypoint_time = now; 407 if ( all_enables ) { 408 next_state = MOVE_AWAY; 409 move_in_progress = 1; 410 for (r = 0; r < personality; r++) { 411 goal[r] = offset_in(r); 412 } 413 } 414 break; 415 case MOVE_AWAY: 416 move_in_progress = 1; 417 if ( all_enables ) { 418 // allow offset movements 419 for (r = 0; r < personality; r++) { 420 goal[r] = offset_in(r); 421 } 422 break; 423 } 424 // one (or more) enablers is gone 425 next_state = MOVE_BACK; 426 if (!power_on) { 427 reset_old(personality, &data); 428 move_in_progress = 0; 429 offset_applied = 0; 430 next_state = IDLE; 431 break; 432 } 433 for (r = 0; r < personality; r++) { 434 goal[r] = 0; // default (eg not waypoint backtrack) 435 } 436 if (backtrack) { 437 if ( next_waypoint_index > 0 ) { 438 for (r = 0; r < personality; r++) { 439 goal[r] = waypoints[r][next_waypoint_index-1]; 440 } 441 waypoint_limit = 0; 442 next_waypoint_index--; 443 } 444 } 445 break; 446 447 case MOVE_BACK: 448 if (!power_on) { 449 reset_old(personality, &data); 450 move_in_progress = 0; 451 offset_applied = 0; 452 next_state = IDLE; 453 break; 454 } 455 move_in_progress = 1; 456 if (backtrack) { 457 if ( next_waypoint_index > 0 ) { 458 hal_float_t pcur[NCHANNELS]; 459 for (r=0; r < personality; r++) { 460 pcur[r] = offset_current(r); 461 } 462 if (at_waypoint(personality, goal, pcur, eps_in_use)) { 463 for (r = 0; r < personality; r++) { 464 goal[r] = waypoints[r][next_waypoint_index]; 465 } 466 next_waypoint_index--; 467 } 468 } else { 469 for (r = 0; r < personality; r++) { 470 goal[r] = 0; //final goal 471 } 472 } 473 } 474 475 if (!offset_applied) { 476 // offsets gone, return to IDLE 477 next_state = IDLE; 478 move_in_progress = 0; 479 reset_old(personality, &data); 480 } 481 break; 482 } 483 484 if ( !apply_offsets 485 && offset_applied 486 ) { 487 warning = 1; 488 if (!gave_msg) { 489 // apply_offsets deasserted while offset_applied 490 // for example: 491 // 1) program stopped with offsets applied 492 // 2) (no *.resume-inhibit pin) or (-no_resume_inhibit option) 493 // and program resumed with offsets applied 494 for (r = 0; r < personality; r++) { 495 rtapi_print_msg(RTAPI_MSG_ERR, 496 "Index: %i, offset=%f", 497 r, data.old_out[r]); 498 } 499 rtapi_print_msg(RTAPI_MSG_ERR, 500 "apply_offsets deasserted before offsets removed " 501 "moveoff.comp: WARNING" 502 ); 503 gave_msg = 1; 504 } 505 } else { 506 gave_msg = 0; 507 warning = 0; 508 } 509 //}end state control 510 } //read_inputs 511 512 FUNCTION(write_outputs) { 513 int r; 514 // move with limits on postion, velocity, acceleration 515 for (r = 0; r < personality; r++) { 516 if (move_in_progress) { 517 input.in = goal[r]; 518 input.minlimit = offset_min(r); 519 input.maxlimit = offset_max(r); 520 input.maxvlimit = offset_vel(r); 521 input.maxalimit = offset_accel(r); 522 input.old_in = data.old_in[r]; 523 input.old_out = data.old_out[r]; 524 input.old_v = data.old_v[r]; 525 526 if ( waypoint_limit && (state == MOVE_AWAY) ) { 527 // no movement in MOVE_AWAY (require: remove enable) 528 } else { 529 hal_float_t last_old = data.old_out[r]; 530 lim3(input, 531 &data.old_in[r], 532 &data.old_out[r], 533 &data.old_v[r] 534 ); 535 offset_current(r) = data.old_out[r]; 536 fb_minusoffset(r) = fb(r) - offset_current(r) 537 - (last_old - data.old_out[r]); 538 pos_plusoffset(r) = pos(r) + offset_current(r); 539 } 540 } else { 541 pos_plusoffset(r) = pos(r); 542 offset_current(r) = 0; 543 fb_minusoffset(r) = fb(r); 544 } 545 } 546 547 offset_applied = ! offset_removed(personality, data, eps_in_use); 548 if ( !offset_applied ) { 549 // reset backtrack 550 next_waypoint_index = 0; 551 waypoint_limit = 0; 552 } 553 554 waypoint_ct = next_waypoint_index; 555 waypoint_percent_used = 100*next_waypoint_index/max_waypoints; 556 557 dbg_state = state; 558 state = next_state; 559 } //write_outputs 560 561 EXTRA_SETUP() { 562 if (personality == 0) personality = 3; 563 return 0; 564 }