encoder_ratio.c
1 /******************************************************************** 2 * Description: encoder_ratio.c 3 * A HAL component that can be used to synchronize two 4 * axes (like an "electronic gear"). 5 * 6 * Author: John Kasunich 7 * License: GPL Version 2 8 * 9 * Copyright (c) 2004 All rights reserved. 10 * 11 * Last change: 12 ********************************************************************/ 13 /** This file, 'encoder_ratio.c', is a HAL component that can be used 14 to synchronize two axes (like an "electronic gear"). It counts 15 encoder pulses from both axes in software, and produces an error 16 value that can be used with a PID loop to make the slave encoder 17 track the master encoder with a specific ratio. The maximum count 18 rate will depend on the speed of the PC, but is expected to exceed 19 5KHz for even the slowest computers, and may reach 10-15KHz on fast 20 ones. It is a realtime component. 21 22 This module supports up to eight axis pairs. The number of pairs 23 is set by the module parameter 'num_chan' when the component is 24 insmod'ed. Alternatively, use the names= specifier and a list of 25 unique names separated by commas. 26 The names= and num_chan= specifiers are mutually exclusive. 27 28 The module exports pins and parameters for each axis pair as follows: 29 30 Input Pins: 31 32 encoder-ratio.N.master-A (bit) Phase A of master axis encoder 33 encoder-ratio.N.master-B (bit) Phase B of master axis encoder 34 encoder-ratio.N.slave-A (bit) Phase A of slave axis encoder 35 encoder-ratio.N.slave-B (bit) Phase B of slave axis encoder 36 encoder-ratio.N.enable (bit) Enables master-slave tracking 37 38 Output Pins: 39 40 encoder-ratio.N.error (float) Position error of slave (in revs) 41 42 Parameters: 43 44 encoder-ratio.N.master-ppr (u32) Master axis PPR 45 encoder-ratio.N.slave-ppr (u32) Slave axis PPR 46 encoder-ratio.N.master-teeth (u32) "teeth" on master "gear" 47 encoder-ratio.N.slave-teeth (u32) "teeth" on slave "gear" 48 49 The module also exports two functions. "encoder-ratio.sample" 50 must be called in a high speed thread, at least twice the maximum 51 desired count rate. "encoder-ratio.update" can be called at a 52 much slower rate, and updates the output pin(s). 53 54 When the enable pin is FALSE, the error pin simply reports the 55 slave axis position, in revolutions. As such, it would normally 56 be connected to the feedback pin of a PID block for closed loop 57 control of the slave axis. Normally the command input of the 58 PID block is left unconnected (zero), so the slave axis simply 59 sits still. However when the enable input goes TRUE, the error 60 pin becomes the slave position minus the scaled master position. 61 The scale factor is the ratio of master teeth to slave teeth. 62 As the master moves, error becomes non-zero, and the PID loop 63 will drive the slave axis to track the master. 64 */ 65 66 /** Copyright (C) 2004 John Kasunich 67 <jmkasunich AT users DOT sourceforge DOT net> 68 */ 69 70 /** This program is free software; you can redistribute it and/or 71 modify it under the terms of version 2 of the GNU General 72 Public License as published by the Free Software Foundation. 73 This library is distributed in the hope that it will be useful, 74 but WITHOUT ANY WARRANTY; without even the implied warranty of 75 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 76 GNU General Public License for more details. 77 78 You should have received a copy of the GNU General Public 79 License along with this library; if not, write to the Free Software 80 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 81 82 THE AUTHORS OF THIS LIBRARY ACCEPT ABSOLUTELY NO LIABILITY FOR 83 ANY HARM OR LOSS RESULTING FROM ITS USE. IT IS _EXTREMELY_ UNWISE 84 TO RELY ON SOFTWARE ALONE FOR SAFETY. Any machinery capable of 85 harming persons must have provisions for completely removing power 86 from all motors, etc, before persons enter any danger area. All 87 machinery must be designed to comply with local and national safety 88 codes, and the authors of this software can not, and do not, take 89 any responsibility for such compliance. 90 91 This code was written as part of the EMC HAL project. For more 92 information, go to www.linuxcnc.org. 93 */ 94 95 #include "rtapi.h" /* RTAPI realtime OS API */ 96 #include "rtapi_app.h" /* RTAPI realtime module decls */ 97 #include "hal.h" /* HAL public API decls */ 98 99 /* module information */ 100 MODULE_AUTHOR("John Kasunich"); 101 MODULE_DESCRIPTION("Encoder Ratio Module for HAL"); 102 MODULE_LICENSE("GPL"); 103 static int num_chan; /* number of channels*/ 104 static int default_num_chan = 1; 105 RTAPI_MP_INT(num_chan, "number of channels"); 106 107 static int howmany; 108 #define MAX_CHAN 8 109 static char *names[MAX_CHAN] = {0,}; 110 RTAPI_MP_ARRAY_STRING(names,MAX_CHAN,"encoder_ratio names"); 111 112 /*********************************************************************** 113 * STRUCTURES AND GLOBAL VARIABLES * 114 ************************************************************************/ 115 116 /* this structure contains the runtime data for a single counter */ 117 118 typedef struct { 119 hal_bit_t *master_A; /* quadrature input */ 120 hal_bit_t *master_B; /* quadrature input */ 121 hal_bit_t *slave_A; /* quadrature input */ 122 hal_bit_t *slave_B; /* quadrature input */ 123 hal_bit_t *enable; /* enable input */ 124 unsigned char master_state; /* quad decode state machine state */ 125 unsigned char slave_state; /* quad decode state machine state */ 126 int raw_error; /* internal data */ 127 int master_increment; /* internal data */ 128 int slave_increment; /* internal data */ 129 double output_scale; /* internal data */ 130 hal_float_t *error; /* error output */ 131 hal_u32_t *master_ppr; /* parameter: master encoder PPR */ 132 hal_u32_t *slave_ppr; /* parameter: slave encoder PPR */ 133 hal_u32_t *master_teeth; /* parameter: master "gear" tooth count */ 134 hal_u32_t *slave_teeth; /* parameter: slave "gear" tooth count */ 135 } encoder_pair_t; 136 137 /* pointer to array of counter_t structs in shmem, 1 per counter */ 138 static encoder_pair_t *encoder_pair_array; 139 140 /* bitmasks for quadrature decode state machine */ 141 #define SM_PHASE_A_MASK 0x01 142 #define SM_PHASE_B_MASK 0x02 143 #define SM_LOOKUP_MASK 0x0F 144 #define SM_CNT_UP_MASK 0x40 145 #define SM_CNT_DN_MASK 0x80 146 147 /* Lookup table for quadrature decode state machine. This machine 148 will reject glitches on either input (will count up 1 on glitch, 149 down 1 after glitch), and on both inputs simultaneously (no count 150 at all) In theory, it can count once per cycle, in practice the 151 maximum count rate should be at _least_ 10% below the sample rate, 152 and preferrable around half the sample rate. It counts every 153 edge of the quadrature waveform, 4 counts per complete cycle. 154 */ 155 static const unsigned char lut[16] = { 156 0x00, 0x44, 0x88, 0x0C, 0x80, 0x04, 0x08, 0x4C, 157 0x40, 0x04, 0x08, 0x8C, 0x00, 0x84, 0x48, 0x0C 158 }; 159 160 /* other globals */ 161 static int comp_id; /* component ID */ 162 163 /*********************************************************************** 164 * LOCAL FUNCTION DECLARATIONS * 165 ************************************************************************/ 166 167 static int export_encoder_pair(int num, encoder_pair_t * addr, char* prefix); 168 static void sample(void *arg, long period); 169 static void update(void *arg, long period); 170 171 /*********************************************************************** 172 * INIT AND EXIT CODE * 173 ************************************************************************/ 174 175 176 int rtapi_app_main(void) 177 { 178 int n, retval,i; 179 180 if(num_chan && names[0]) { 181 rtapi_print_msg(RTAPI_MSG_ERR,"num_chan= and names= are mutually exclusive\n"); 182 return -EINVAL; 183 } 184 if(!num_chan && !names[0]) num_chan = default_num_chan; 185 186 if(num_chan) { 187 howmany = num_chan; 188 } else { 189 howmany = 0; 190 for (i = 0; i < MAX_CHAN; i++) { 191 if ( (names[i] == NULL) || (*names[i] == 0) ){ 192 break; 193 } 194 howmany = i + 1; 195 } 196 } 197 198 /* test for number of channels */ 199 if ((howmany <= 0) || (howmany > MAX_CHAN)) { 200 rtapi_print_msg(RTAPI_MSG_ERR, 201 "ENCODER_RATIO: ERROR: invalid number of channels: %d\n", howmany); 202 return -1; 203 } 204 /* have good config info, connect to the HAL */ 205 comp_id = hal_init("encoder_ratio"); 206 if (comp_id < 0) { 207 rtapi_print_msg(RTAPI_MSG_ERR, "ENCODER_RATIO: ERROR: hal_init() failed\n"); 208 return -1; 209 } 210 /* allocate shared memory for encoder data */ 211 encoder_pair_array = hal_malloc(howmany * sizeof(encoder_pair_t)); 212 if (encoder_pair_array == 0) { 213 rtapi_print_msg(RTAPI_MSG_ERR, 214 "ENCODER_RATIO: ERROR: hal_malloc() failed\n"); 215 hal_exit(comp_id); 216 return -1; 217 } 218 /* set up each encoder pair */ 219 i = 0; // for names= items 220 for (n = 0; n < howmany; n++) { 221 /* export all vars */ 222 if(num_chan) { 223 char buf[HAL_NAME_LEN + 1]; 224 rtapi_snprintf(buf, sizeof(buf), "encoder-ratio.%d", n); 225 retval = export_encoder_pair(n, &(encoder_pair_array[n]), buf); 226 } else { 227 retval = export_encoder_pair(n, &(encoder_pair_array[n]), names[i++]); 228 } 229 230 if (retval != 0) { 231 rtapi_print_msg(RTAPI_MSG_ERR, 232 "ENCODER_RATIO: ERROR: counter %d var export failed\n", n); 233 hal_exit(comp_id); 234 return -1; 235 } 236 /* init encoder pair */ 237 encoder_pair_array[n].master_state = 0; 238 encoder_pair_array[n].slave_state = 0; 239 encoder_pair_array[n].master_increment = 0; 240 encoder_pair_array[n].slave_increment = 0; 241 encoder_pair_array[n].raw_error = 0; 242 encoder_pair_array[n].output_scale = 1.0; 243 *(encoder_pair_array[n].error) = 0.0; 244 } 245 /* export functions */ 246 retval = hal_export_funct("encoder-ratio.sample", sample, 247 encoder_pair_array, 0, 0, comp_id); 248 if (retval != 0) { 249 rtapi_print_msg(RTAPI_MSG_ERR, 250 "ENCODER_RATIO: ERROR: sample funct export failed\n"); 251 hal_exit(comp_id); 252 return -1; 253 } 254 retval = hal_export_funct("encoder-ratio.update", update, 255 encoder_pair_array, 1, 0, comp_id); 256 if (retval != 0) { 257 rtapi_print_msg(RTAPI_MSG_ERR, 258 "ENCODER_RATIO: ERROR: update funct export failed\n"); 259 hal_exit(comp_id); 260 return -1; 261 } 262 rtapi_print_msg(RTAPI_MSG_INFO, 263 "ENCODER_RATIO: installed %d encoder_ratio blocks\n", howmany); 264 hal_ready(comp_id); 265 return 0; 266 } 267 268 void rtapi_app_exit(void) 269 { 270 hal_exit(comp_id); 271 } 272 273 /*********************************************************************** 274 * REALTIME ENCODER COUNTING AND UPDATE FUNCTIONS * 275 ************************************************************************/ 276 277 static void sample(void *arg, long period) 278 { 279 encoder_pair_t *pair; 280 int n; 281 unsigned char state; 282 283 pair = arg; 284 for (n = 0; n < howmany; n++) { 285 /* detect transitions on master encoder */ 286 /* get state machine current state */ 287 state = pair->master_state; 288 /* add input bits to state code */ 289 if (*(pair->master_A)) { 290 state |= SM_PHASE_A_MASK; 291 } 292 if (*(pair->master_B)) { 293 state |= SM_PHASE_B_MASK; 294 } 295 /* look up new state */ 296 state = lut[state & SM_LOOKUP_MASK]; 297 /* are we enabled? */ 298 if ( *(pair->enable) != 0 ) { 299 /* has an edge been detected? */ 300 if (state & SM_CNT_UP_MASK) { 301 pair->raw_error -= pair->master_increment; 302 } else if (state & SM_CNT_DN_MASK) { 303 pair->raw_error += pair->master_increment; 304 } 305 } 306 /* save state machine state */ 307 pair->master_state = state; 308 /* detect transitions on slave encoder */ 309 /* get state machine current state */ 310 state = pair->slave_state; 311 /* add input bits to state code */ 312 if (*(pair->slave_A)) { 313 state |= SM_PHASE_A_MASK; 314 } 315 if (*(pair->slave_B)) { 316 state |= SM_PHASE_B_MASK; 317 } 318 /* look up new state */ 319 state = lut[state & SM_LOOKUP_MASK]; 320 /* has an edge been detected? */ 321 if (state & SM_CNT_UP_MASK) { 322 pair->raw_error += pair->slave_increment; 323 } else if (state & SM_CNT_DN_MASK) { 324 pair->raw_error -= pair->slave_increment; 325 } 326 /* save state machine state */ 327 pair->slave_state = state; 328 /* move on to next pair */ 329 pair++; 330 } 331 /* done */ 332 } 333 334 static void update(void *arg, long period) 335 { 336 encoder_pair_t *pair; 337 int n; 338 339 pair = arg; 340 for (n = 0; n < howmany; n++) { 341 /* scale raw error to output pin */ 342 if ( pair->output_scale > 0 ) { 343 *(pair->error) = pair->raw_error / pair->output_scale; 344 } 345 /* update scale factors (only needed if params change, but 346 it's faster to do it every time than to detect changes.) */ 347 pair->master_increment = *(pair->master_teeth) * *(pair->slave_ppr); 348 pair->slave_increment = *(pair->slave_teeth) * *(pair->master_ppr); 349 pair->output_scale = *(pair->master_ppr) * *(pair->slave_ppr) * *(pair->slave_teeth); 350 /* move on to next pair */ 351 pair++; 352 } 353 /* done */ 354 } 355 356 /*********************************************************************** 357 * LOCAL FUNCTION DEFINITIONS * 358 ************************************************************************/ 359 360 static int export_encoder_pair(int num, encoder_pair_t * addr, char* prefix) 361 { 362 int retval, msg; 363 364 /* This function exports a lot of stuff, which results in a lot of 365 logging if msg_level is at INFO or ALL. So we save the current value 366 of msg_level and restore it later. If you actually need to log this 367 function's actions, change the second line below */ 368 msg = rtapi_get_msg_level(); 369 rtapi_set_msg_level(RTAPI_MSG_WARN); 370 371 /* export pins for the quadrature inputs */ 372 retval = hal_pin_bit_newf(HAL_IN, &(addr->master_A), comp_id, 373 "%s.master-A", prefix); 374 if (retval != 0) { 375 return retval; 376 } 377 retval = hal_pin_bit_newf(HAL_IN, &(addr->master_B), comp_id, 378 "%s.master-B", prefix); 379 if (retval != 0) { 380 return retval; 381 } 382 retval = hal_pin_bit_newf(HAL_IN, &(addr->slave_A), comp_id, 383 "%s.slave-A", prefix); 384 if (retval != 0) { 385 return retval; 386 } 387 retval = hal_pin_bit_newf(HAL_IN, &(addr->slave_B), comp_id, 388 "%s.slave-B", prefix); 389 if (retval != 0) { 390 return retval; 391 } 392 /* export pin for the enable input */ 393 retval = hal_pin_bit_newf(HAL_IN, &(addr->enable), comp_id, 394 "%s.enable", prefix); 395 if (retval != 0) { 396 return retval; 397 } 398 /* export pin for output */ 399 retval = hal_pin_float_newf(HAL_OUT, &(addr->error), comp_id, 400 "%s.error", prefix); 401 if (retval != 0) { 402 return retval; 403 } 404 /* export pins for config info() */ 405 retval = hal_pin_u32_newf(HAL_IO, &(addr->master_ppr), comp_id, 406 "%s.master-ppr", prefix); 407 if (retval != 0) { 408 return retval; 409 } 410 retval = hal_pin_u32_newf(HAL_IO, &(addr->slave_ppr), comp_id, 411 "%s.slave-ppr", prefix); 412 if (retval != 0) { 413 return retval; 414 } 415 retval = hal_pin_u32_newf(HAL_IO, &(addr->master_teeth), comp_id, 416 "%s.master-teeth", prefix); 417 if (retval != 0) { 418 return retval; 419 } 420 retval = hal_pin_u32_newf(HAL_IO, &(addr->slave_teeth), comp_id, 421 "%s.slave-teeth", prefix); 422 if (retval != 0) { 423 return retval; 424 } 425 /* restore saved message level */ 426 rtapi_set_msg_level(msg); 427 return 0; 428 }