encoder_ratio.c

Source code for an Numeric Cmputer

/********************************************************************
* Description:  encoder_ratio.c
*               A HAL component that can be used to synchronize two
*               axes (like an "electronic gear").
*
* Author: John Kasunich
* License: GPL Version 2
*    
* Copyright (c) 2004 All rights reserved.
*
* Last change: 
# $Revision: 1.4 $
* $Author: jmkasunich $
* $Date: 2005/11/25 19:38:57 $
********************************************************************/
/** This file, 'encoder_ratio.c', is a HAL component that can be used
    to synchronize two axes (like an "electronic gear").  It counts
    encoder pulses from both axes in software, and produces an error
    value that can be used with a PID loop to make the slave encoder
    track the master encoder with a specific ratio. The maximum count
    rate will depend on the speed of the PC, but is expected to exceed
    5KHz for even the slowest computers, and may reach 10-15KHz on fast
    ones.  It is a realtime component.

    This module supports up to eight axis pairs.  The number of pairs
    is set by the module parameter 'num_chan' when the component is
    insmod'ed.  The module can optionally create a realtime thread,
    which is useful if a free-running module is desired.  The module
    parameter 'period' is a long int, corresponding to the thread
    period in nano-seconds.  If omitted, no thread will be created.

    The module exports pins and parameters for each axis pair as follows:

    Input Pins:

    encoder-ratio.N.master-A   (bit) Phase A of master axis encoder
    encoder-ratio.N.master-B   (bit) Phase B of master axis encoder
    encoder-ratio.N.slave-A    (bit) Phase A of slave axis encoder
    encoder-ratio.N.slave-B    (bit) Phase B of slave axis encoder
    encoder-ratio.N.enable     (bit) Enables master-slave tracking

    Output Pins:

    encoder-ratio.N.error      (float) Position error of slave (in revs)

    Parameters:

    encoder-ratio.N.master-ppr     (u32) Master axis PPR
    encoder-ratio.N.slave-ppr      (u32) Slave axis PPR
    encoder-ratio.N.master-teeth   (u32) "teeth" on master "gear"
    encoder-ratio.N.slave-teeth    (u32) "teeth" on slave "gear"

    The module also exports two functions.  "encoder-ratio.sample"
    must be called in a high speed thread, at least twice the maximum
    desired count rate.  "encoder-ratio.update" can be called at a
    much slower rate, and updates the output pin(s).

    When the enable pin is FALSE, the error pin simply reports the
    slave axis position, in revolutions.  As such, it would normally
    be connected to the feedback pin of a PID block for closed loop
    control of the slave axis.  Normally the command input of the
    PID block is left unconnected (zero), so the slave axis simply
    sits still.  However when the enable input goes TRUE, the error
    pin becomes the slave position minus the scaled master position.
    The scale factor is the ratio of master teeth to slave teeth.
    As the master moves, error becomes non-zero, and the PID loop
    will drive the slave axis to track the master.
*/

/** Copyright (C) 2004 John Kasunich
                       <jmkasunich AT users DOT sourceforge DOT net>
*/

/** This program is free software; you can redistribute it and/or
    modify it under the terms of version 2.1 of the GNU General
    Public License as published by the Free Software Foundation.
    This library 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 library; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111 USA

    THE AUTHORS OF THIS LIBRARY ACCEPT ABSOLUTELY NO LIABILITY FOR
    ANY HARM OR LOSS RESULTING FROM ITS USE.  IT IS _EXTREMELY_ UNWISE
    TO RELY ON SOFTWARE ALONE FOR SAFETY.  Any machinery capable of
    harming persons must have provisions for completely removing power
    from all motors, etc, before persons enter any danger area.  All
    machinery must be designed to comply with local and national safety
    codes, and the authors of this software can not, and do not, take
    any responsibility for such compliance.

    This code was written as part of the EMC HAL project.  For more
    information, go to www.linuxcnc.org.
*/

#ifndef RTAPI
#error This is a realtime component only!
#endif

#include "rtapi.h"		/* RTAPI realtime OS API */
#include "rtapi_app.h"		/* RTAPI realtime module decls */
#include "hal.h"		/* HAL public API decls */

#ifdef MODULE
/* module information */
MODULE_AUTHOR("John Kasunich");
MODULE_DESCRIPTION("Encoder Ratio Module for HAL");
#ifdef MODULE_LICENSE
MODULE_LICENSE("GPL");
#endif /* MODULE_LICENSE */
static int num_chan = 1;	/* number of channels - default = 1 */
MODULE_PARM(num_chan, "i");
MODULE_PARM_DESC(num_chan, "number of channels");
static long period = 0;		/* thread period - default = no thread */
MODULE_PARM(period, "l");
MODULE_PARM_DESC(period, "thread period (nsecs)");
#endif /* MODULE */

/***********************************************************************
*                STRUCTURES AND GLOBAL VARIABLES                       *
************************************************************************/

/* this structure contains the runtime data for a single counter */

typedef struct {
    hal_bit_t *master_A;	/* quadrature input */
    hal_bit_t *master_B;	/* quadrature input */
    hal_bit_t *slave_A;		/* quadrature input */
    hal_bit_t *slave_B;		/* quadrature input */
    hal_bit_t *enable;		/* enable input */
    unsigned char master_state;	/* quad decode state machine state */
    unsigned char slave_state;	/* quad decode state machine state */
    int raw_error;		/* internal data */
    int master_increment;	/* internal data */
    int slave_increment;	/* internal data */
    float output_scale;		/* internal data */
    hal_float_t *error;		/* error output */
    hal_u32_t master_ppr;	/* parameter: master encoder PPR */
    hal_u32_t slave_ppr;	/* parameter: slave encoder PPR */
    hal_u32_t master_teeth;	/* parameter: master "gear" tooth count */
    hal_u32_t slave_teeth;	/* parameter: slave "gear" tooth count */
} encoder_pair_t;

/* pointer to array of counter_t structs in shmem, 1 per counter */
static encoder_pair_t *encoder_pair_array;

/* bitmasks for quadrature decode state machine */
#define SM_PHASE_A_MASK 0x01
#define SM_PHASE_B_MASK 0x02
#define SM_LOOKUP_MASK  0x0F
#define SM_CNT_UP_MASK  0x40
#define SM_CNT_DN_MASK  0x80

/* Lookup table for quadrature decode state machine.  This machine
   will reject glitches on either input (will count up 1 on glitch,
   down 1 after glitch), and on both inputs simultaneously (no count
   at all)  In theory, it can count once per cycle, in practice the
   maximum count rate should be at _least_ 10% below the sample rate,
   and preferrable around half the sample rate.  It counts every
   edge of the quadrature waveform, 4 counts per complete cycle.
*/
static const unsigned char lut[16] = {
    0x00, 0x44, 0x88, 0x0C, 0x80, 0x04, 0x08, 0x4C,
    0x40, 0x04, 0x08, 0x8C, 0x00, 0x84, 0x48, 0x0C
};

/* other globals */
static int comp_id;		/* component ID */

/***********************************************************************
*                  LOCAL FUNCTION DECLARATIONS                         *
************************************************************************/

static int export_encoder_pair(int num, encoder_pair_t * addr);
static void sample(void *arg, long period);
static void update(void *arg, long period);

/***********************************************************************
*                       INIT AND EXIT CODE                             *
************************************************************************/

#define MAX_CHAN 8

int rtapi_app_main(void)
{
    int n, retval;

    /* test for number of channels */
    if ((num_chan <= 0) || (num_chan > MAX_CHAN)) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "ENCODER_RATIO: ERROR: invalid num_chan: %d\n", num_chan);
	return -1;
    }
    /* have good config info, connect to the HAL */
    comp_id = hal_init("encoder_ratio");
    if (comp_id < 0) {
	rtapi_print_msg(RTAPI_MSG_ERR, "ENCODER_RATIO: ERROR: hal_init() failed\n");
	return -1;
    }
    /* allocate shared memory for encoder data */
    encoder_pair_array = hal_malloc(num_chan * sizeof(encoder_pair_t));
    if (encoder_pair_array == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "ENCODER_RATIO: ERROR: hal_malloc() failed\n");
	hal_exit(comp_id);
	return -1;
    }
    /* set up each encoder pair */
    for (n = 0; n < num_chan; n++) {
	/* export all vars */
	retval = export_encoder_pair(n, &(encoder_pair_array[n]));