eoffset_per_angle.comp
1 component eoffset_per_angle "Compute External Offset Per Angle"; 2 //" vim 3 description 4 """ 5 6 An offset is computed (from one of several functions) based on 7 an input angle in degrees. The angle could be a rotary 8 coordinate value or a spindle angle. 9 10 The computed offset is represented as an s32 \\fBkcounts\\fR output 11 pin that is a compatible input to external offset pins like 12 \\fBaxis.L.eoffset-counts\\fR where \\fBL\\fR is the coordinate 13 letter. Scaling of the s32 \\fBkcounts\\fR is controlled by the 14 input (\\fBk\\fR) -- its reciprocal value is presented on an output pin 15 (\\fBkreciprocal\\fR) for connection to \\fBaxis.L.eoffset-scale\\fR. 16 The default value for \\fBk\\fR should be suitable for most uses. 17 18 The built-in functions use pins \\fBfmult\\fR and \\fBrfraction\\fR 19 to control the output frequency (or number of polygon sides) 20 and amplitude respectively. The \\fBrfraction\\fR pin controls 21 the offset amplitude as a fraction of the \\fBradius-ref\\fR pin. 22 23 One of the four built-in functions is specified by the \\fBfnum\\fR pin: 24 \\fB0\\fR: f0 inside polygon (requires \\fBfmult\\fR == nsides >= 3) 25 \\fB1\\fR: f1 outside polygon (requires \\fBfmult\\fR == nsides >= 3) 26 \\fB2\\fR: f2 sinusoid 27 \\fB3\\fR: f3 square wave 28 29 Unsupported \\fBfnum\\fR values default to use function f0. 30 31 \\fBNOTES:\\fR 32 33 \\fBradius-ref\\fR: 34 The computed offsets are based on the \\fBradius-ref\\fR pin 35 value. This pin may be set to a constant radius value or 36 controlled by a user interface or by g code program (using 37 \\fBM68\\fR and a \\fBmotion.analog-out-NN pin for instance). 38 39 \\fBStopping\\fR: 40 When the \\fBenable-in\\fR pin is deasserted, the offset is 41 returned to zero respecting the allocated acceleration 42 and velocity limits. The allocations for coordinate \\fBL\\fR 43 are typically controlled by an ini file setting: 44 \\fB[AXIS_L]OFFSET_AV_RATIO\\fR. 45 46 \\fBNOTES\\fR: 47 If unsupported parameters are supplied to a function (for instance 48 a polygon with fewer than three sides), the current offset 49 will be returned to zero (respecting velocity and acceleration 50 constraints). After correcting the offending parameter, the 51 \\fBenable-in\\fR pin must be toggled to resume offset 52 computations. 53 54 \\fBEXAMPLE:\\fR 55 An example simulation configuration is provided at: 56 \\fBconfigs/sim/axis/external_offsets/opa.ini\\fR. A 57 simulated XZC machine uses the \\fBC\\fR coordinate angle to 58 offset the transverse \\fBX\\fR coordinate according to 59 the selected \\fBfnum\\fR function. 60 """; 61 62 //" quote char for vim highlighting 63 64 pin in bit active = 0 "From: motion.eoffset-active"; 65 pin in bit is_on = 0 "From: halui.machine.is-on"; 66 67 pin in bit enable_in = 0 "Enable Input"; 68 pin in float radius_ref = 1 "Radius reference (see notes)"; 69 pin in float angle = 0 "Input angle (in degrees)"; 70 pin in float start_angle = 0 "Start angle (in degrees)"; 71 pin in s32 fnum = 0 "Function selector (default 0)"; 72 pin in float rfraction = 0.1 "Offset amplitude (+/- fraction of radius_ref)"; 73 pin in float fmult = 6 "Offset frequency multiplier"; 74 pin in u32 k = 10000 "Scaling Factor (if 0, use 10000)"; 75 76 pin out bit is_off "invert is_on (for convenience)"; 77 78 pin out bit enable_out "To: axis.L.eoffset-enable"; 79 pin out bit clear "To: axis.L.eoffset-clear"; 80 pin out s32 kcounts "To: axis.L.eoffset-counts"; 81 pin out float kreciprocal "To: axis.L.eoffset-scale (1/k)"; 82 83 pin out float eoffset_dbg "offset (debug pin--use kcounts & kreciprocal)"; 84 pin out u32 state_dbg "state (debug pin)"; 85 86 //--------------------------------------------------------------------- 87 // per-instance items: 88 variable int run_ct = 0; 89 variable int delay_ct = 0; 90 variable int messaged = 0; 91 variable int err_stop = 0; 92 93 // Note: use of 'option data TYPE' with halcompile is deprecated 94 // but the recommended alternative, 'variable CTYPE', 95 // does not support typedefs for structs. 96 // If support for 'option data TYPE' is removed, this file 97 // should be converted from a .comp to a .c file with 98 // additional Makefile support 99 option data the_data; 100 //--------------------------------------------------------------------- 101 102 function _; 103 license "GPL"; 104 ;; 105 106 #include <rtapi_math.h> 107 108 #define FINISH_DELAY 0 109 #define TO_RAD M_PI/180 110 111 typedef enum { 112 OFF, 113 READY, 114 RUNNING, 115 STOPPING, 116 FINISH, 117 } state; 118 119 typedef struct ofunc_data { 120 state thestate; 121 double adeg; 122 double astart; 123 double r_ref; 124 double r_frac; 125 double fmultiplier; 126 double ovalue; 127 } the_data; 128 129 typedef int ofunc(struct ofunc_data*); 130 static ofunc func0,func1,func2,func3; 131 132 #define OPA_DEBUG 133 #undef OPA_DEBUG 134 #ifdef OPA_DEBUG 135 #define LVL RTAPI_MSG_INFO 136 #define dprint(msg,n) do { \ 137 rtapi_set_msg_level(LVL); \ 138 rtapi_print_msg(LVL,"%20s %5d\n",msg,n); \ 139 } while (0) 140 #else 141 #define dprint(msg,n) 142 #endif 143 144 FUNCTION(_) { 145 struct ofunc_data* dptr = &data; 146 #define STATE dptr->thestate 147 148 int kfactor; 149 ofunc* thefunc; 150 151 run_ct++; 152 state_dbg = STATE; 153 if (k == 0) {kfactor = 10000;} 154 kreciprocal = 1/((float)kfactor); 155 156 is_off = !is_on; // convenience pin 157 158 if (is_off) { 159 // note: the external_offsets implementation defines 160 // axis.L.eoffset as zero when machine is off 161 err_stop = 0; 162 enable_out = 0; 163 kcounts = 0; eoffset_dbg = 0; 164 messaged = 0; 165 STATE = OFF; 166 return; 167 } 168 169 switch (STATE) { 170 case OFF: 171 // require an enable_in 0-->1 transition to advance to READY 172 if (enable_in) { 173 if (!messaged) { 174 rtapi_print_msg(RTAPI_MSG_ERR, 175 "eoffset_per_angle: active enable-in not honored at start"); 176 messaged = 1; 177 } 178 return; 179 } 180 err_stop = 0; 181 messaged = 1; 182 kcounts = 0; 183 STATE = READY; 184 dprint("OFF->READY",kcounts); 185 return; 186 break; 187 case READY: 188 if (!enable_in) {return;} 189 kcounts = 0; eoffset_dbg = 0; 190 enable_out = 0; 191 delay_ct = 0; 192 STATE = RUNNING; 193 dprint("READY->RUNNING",kcounts); 194 return; 195 break; 196 case RUNNING: 197 if (enable_in) { 198 enable_out = 1; 199 STATE = RUNNING; 200 } else { 201 /* 202 ** When the enable_in pin is deasserted, kcounts are set to 203 ** zero and the simple trajectory planner removes the offset to 204 ** within its stopping criterion. Under some conditions, a 205 ** residual offset may remain. Connecting the clear pin to 206 ** axis.L.eoffset-clear forces the axis->ext_offset_tp.pos_cmd 207 ** to zero to remove any residual with no modifications to 208 ** simple_tp.c 209 */ 210 clear = 1; 211 kcounts = 0; eoffset_dbg = 0; 212 STATE = STOPPING; 213 delay_ct = run_ct; 214 dprint("RUNNING->STOPPING",kcounts); 215 return; 216 } 217 break; 218 case STOPPING: 219 if (active) { 220 STATE = STOPPING; 221 } else { 222 // !active ==> stopping criterion met 223 delay_ct = run_ct; 224 STATE = FINISH; 225 dprint("STOPPING->FINISH",kcounts); 226 } 227 return; 228 break; 229 case FINISH: 230 // provision for delay if needed 231 if (run_ct < (FINISH_DELAY + delay_ct) ) { 232 STATE = FINISH; 233 } else { 234 enable_out = 0; 235 if (err_stop) { 236 STATE = OFF; 237 dprint("FINISH->OFF",kcounts); 238 } else { 239 STATE = READY; 240 dprint("FINISH->READY",kcounts); 241 } 242 } 243 clear = 0; 244 return; 245 break; 246 } //switch (STATE) 247 248 switch (fnum) { 249 case 0: thefunc = func0; break; 250 case 1: thefunc = func1; break; 251 case 2: thefunc = func2; break; 252 case 3: thefunc = func3; break; 253 default: thefunc = func0; break; 254 } 255 dptr->adeg = angle; 256 dptr->astart = start_angle; 257 dptr->r_ref = radius_ref; 258 dptr->r_frac = rfraction; 259 dptr->fmultiplier = fmult; 260 261 if (thefunc(dptr) ) { 262 // thefunc returned nonzero (problem) 263 err_stop = 1; 264 dptr->ovalue = 0; 265 kcounts = 0; eoffset_dbg = 0; 266 STATE = STOPPING; 267 rtapi_print_msg(RTAPI_MSG_ERR, 268 "eoffset_per_angle stopping:func%d problem\n",fnum); 269 return; 270 } 271 272 kcounts = kfactor * dptr->ovalue; 273 eoffset_dbg = kcounts * kreciprocal; // debug pin 274 return; 275 } 276 277 static int func0 (struct ofunc_data* d) 278 { 279 // for cutting an inside polygon 280 // (start with negative rfraction, and gradually increase to zero) 281 // polygon outscribed by circle of radius r_ref 282 // (fmultiplier > 2) 283 double uangle,divisor; 284 285 if (d->fmultiplier <= 2) { 286 rtapi_print_msg(RTAPI_MSG_ERR, 287 "func0 bad fmultiplier: %g\n",d->fmultiplier); 288 return -1; 289 } 290 291 divisor = 360/d->fmultiplier; 292 uangle = d->adeg + divisor/2 - d->astart; 293 d->ovalue = (1 + d->r_frac) 294 * d->r_ref/cos((divisor/2 - fmod(uangle,divisor))*TO_RAD) 295 - d->r_ref; 296 return 0; 297 } 298 299 static int func1 (struct ofunc_data* d) 300 { 301 // for cutting an outside polygon 302 // (start with positive rfraction, and gradually reduce to zero) 303 // polygon inscribed by circle of radius r_ref 304 // (fmultiplier > 2) 305 double uangle,divisor; 306 307 if (d->fmultiplier <= 2) { 308 rtapi_print_msg(RTAPI_MSG_ERR, 309 "func1 bad fmultiplier: %g\n",d->fmultiplier); 310 return -1; 311 } 312 313 divisor = 360/d->fmultiplier; 314 uangle = d->adeg + divisor/2 - d->astart; 315 d->ovalue = (1 + d->r_frac) 316 * d->r_ref*cos(M_PI/d->fmultiplier)/cos((divisor/2 - fmod(uangle,divisor))*TO_RAD) 317 - d->r_ref; 318 return 0; 319 } 320 321 static int func2 (struct ofunc_data* d) 322 { 323 // sin() -90 ==> start at zero amplitude 324 double uangle; 325 uangle = d->fmultiplier * (d->adeg - d->astart) + -90; 326 d->ovalue = (0.5 * d->r_frac* d->r_ref) * (1 + sin(uangle * TO_RAD)); 327 return 0; 328 } 329 330 static int func3 (struct ofunc_data* d) 331 { 332 // square() -90 ==> start at zero amplitude 333 // useful for looking at affects of ini settings 334 // max vel & accel and offset_av_ratio 335 double uangle; 336 double value = -1; 337 uangle = d->fmultiplier * (d->adeg - d->astart) + -90; 338 if (sin(uangle * TO_RAD) >= 0) {value = 1;} 339 d->ovalue = (0.5 * d->r_frac* d->r_ref) * (1 + value); 340 return 0; 341 }