path: root/src/hal/drivers/pluto_servo.comp

//    Copyright (C) 2007 Jeff Epler
//
//    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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA

component pluto_servo """Hardware driver and firmware for the Pluto-P parallel-port FPGA, for use with servo machines.""";

description """
Pluto_servo is a LinuxCNC software driver and associated firmware that allow the Pluto-P board to be used to control a servo-based CNC machine.

The driver has 4 PWM channels, 4 quadrature channels with index pulse, 18
digital outputs (8 shared with PWM), and 20 digital inputs (12 shared with
quadrature).

.SS Encoders
The encoder pins and parameters conform to the `canonical encoder' interface
described in the HAL manual.  It operates in `x4 mode'.

The sample rate of the encoder is 40MHz.  The maximum number quadrature rate is
8191 counts per LinuxCNC servo cycle.  For correct handling of the index pulse, the
number of encoder counts per revolution must be less than 8191.

.SS PWM
The PWM pins and parameters conform to the `canonical analog output' interface
described in the HAL manual.  The output pins are `up/down' or `pwm/dir'
pins as described in the documentation of the `pwmgen' component.

Internally the PWM generator is based on a 12-bit, 40MHz counter, giving 4095
duty cycles from -100% to +100% and a frequency of approximately 19.5kHz.  In
PDM mode, the duty periods are approximately 100ns long.

.SS Digital I/O
The digital output pins conform to the `canonical digital output' interface
described in the HAL manual.

The digital input pins conform to the `canonical digital input' interface
described in the HAL manual.
""";
pin out s32 encoder.#.count[4];
pin out float encoder.#.position[4];
pin out float encoder.#.velocity[4];
pin in bit encoder.#.reset[4];
pin io bit encoder.#.index-enable[4] """encoder.\\fIM\\fR corresponds to the
pins labeled QA\\fIM\\fR, QB\\fIM\\fR, and QZ\\fIM\\fR on the pinout diagram""";
param rw float encoder.#.scale[4] =1;

param rw bit encoder.z-polarity "Set to TRUE if the index pulse is active low, FALSE if it is active high.  Affects all encoders.";

pin in float pwm.#.value[4];
pin in bit pwm.#.enable[4] "pwm.\\fIM\\fR corresponds to the pins labeled UP\\fIM\\fR and DN\\fIM\\fR on the pinout diagram";
param rw float pwm.#.offset[4];
param rw float pwm.#.scale[4]=1;
param rw float pwm.#.max-dc[4]=1;
param rw float pwm.#.min-dc[4]=0;
param rw bit pwm.#.pwmdir[4]=0
"Set to TRUE use PWM+direction mode.  Set to FALSE to use Up/Down mode.";

param rw bit pwm.is-pdm "Set to TRUE to use PDM (also called interleaved PWM) mode.  Set to FALSE to use traditional PWM mode.  Affects all PWM outputs.";


pin in bit dout.##[20]
"""dout.\\fI0M\\fR corresponds to the pin labeled
OUT\\fIM\\fR on the pinout diagram.  Other pins are shared with the PWM
function, as follows:
.RS 6
.ta T 10 6
.PP
\\fBPin\tShared\\fR
.PP
\\fBLabel\twith\\fR
.PP
dout.10\tUP0
.PP
dout.10\tUP0
.PP
dout.12\tUP1
.PP
dout.14\tUP2
.PP
dout.18\tUP3
.PP
dout.11\tDOWN0
.PP
dout.13\tDOWN1
.PP
dout.15\tDOWN2
.PP
dout.19\tDOWN3
.PP
.RE 
.DT""";
param rw bit dout.##-invert[20]
"If TRUE, the output on the corresponding \\fBdout.\\fIMM\\fR is inverted.";

pin out bit din.##[20];
pin out bit din.##_not[20]
"""For M=0 through 7, din.\\fI0M\\fR corresponds to the pin labeled
IN\\fIM\\fR on the pinout diagram.  Other pins are shared with the encoder
function, as follows:
.RS 6
.ta T 10 6
.PP
\\fBPin\tShared\\fR
.PP
\\fBLabel\twith\\fR
.PP
din.8\tQZ0
.PP
din.9\tQZ1
.PP
din.10\tQZ2
.PP
din.11\tQZ3
.PP
din.12\tQB0
.PP
din.13\tQB1
.PP
din.14\tQB2
.PP
din.15\tQB3
.PP
din.16\tQA0
.PP
din.17\tQA1
.PP
din.18\tQA2
.PP
din.19\tQA3
.PP
.RE
.DT""";

param rw u32 communication_error """Incremented each time 
pluto-servo.read detects an error code in the EPP status register.  While
this register is nonzero, new values are not being written to the Pluto-P
board, and the status of digital outputs and the PWM duty cycle of the PWM
outputs will remain unchanged.  If the watchdog is enabled, it will activate
soon after the communication error is detected.  To continue after a
communication error, set this parameter back to zero.""";

param rw s32 debug_0;
param rw s32 debug_1 """These parameters can display values which are useful to developers or for debugging the driver and firmware.  They are not useful for integrators or users.""";

option singleton;
option extra_setup;
option extra_cleanup;

option data internal;

function read "Read all the inputs from the pluto-servo board";
function write "Write all the outputs on the pluto-servo board";

see_also """The \\fIpluto_servo\\fR section in the HAL User Manual, which shows the location of each physical pin on the pluto board.""";

modparam dummy ioaddr = 0x378 "The base address of the parallel port.";
modparam dummy ioaddr_hi = 0
"""The secondary address of the parallel port, used to set EPP
mode.  0 means to use ioaddr + 0x400.  -1 means there is no
secondary address.  The secondary address is used to set the port to EPP
mode.""";

modparam dummy epp_wide = 1
    """Set to zero to disable the "wide EPP mode".  "Wide" mode allows a 16-
and 32-bit EPP transfers, which can reduce the time spent in the read and write
functions.  However, this may not work on all EPP parallel ports.""";

modparam dummy watchdog = 1
    """Set to zero to disable the "hardware watchdog".  "Watchdog" will
tristate all outputs approximately 6ms after the last execution of
\\fBpluto-servo.write\\fR, which adds some protection in the case of LinuxCNC
crashes.""";

modparam int test_encoder = 0 
    "Internally connect dout0..2 to QA0, QB0, QZ0 to test quadrature counting";

license "GPL";

include "hal/drivers/pluto_common.h";
;;


typedef struct {
    int64_t last_index[4];
    int64_t last_count[4];
    int64_t reset_count[4];
} internal;

#define W 14
#define MASK ((1<<W)-1)

int PWM(int enable, double value, double offset, double scale, double min_dc, double max_dc, int dio0, int dio1, int is_pdm, int is_pwmdir) {
    int result;
    if(enable == 0) return 0;

    value = value / scale + offset;
    if(value < -max_dc) value = -max_dc;
    else if(value > -min_dc && value < 0) value = -min_dc;
    else if(value > 0 && value < min_dc) value = min_dc;
    else if(value > max_dc) value = max_dc;

    value = 2047 * value;
    if(value < -2047) value = -2047;
    if(value > 2047) value = 2047;

    if(value < 0) {
	if(is_pwmdir) {
	     result = (1<<13) | (int)(-value);
	} else {
	     result = (1<<15) | (int)(-value);
	}
    } else result = value;
    if(is_pdm) result |= 1<<14;
    if(dio0) result ^= 1<<12;
    if(dio1) result ^= 1<<13;

    return result;
}

FUNCTION(write) {
    int r = 0;
    int i;
    if(communication_error) return;

    EPP_ADDR(0);

    for(i=0; i<4; i++) {
        if(pwm_max_dc(i) > 1) pwm_max_dc(i) = 1;
        else if(pwm_max_dc(i) < 0) pwm_max_dc(i) = 0;
        if(pwm_min_dc(i) < 0) pwm_min_dc(i) = 0;
        else if(pwm_min_dc(i) > pwm_max_dc(i)) pwm_min_dc(i) = pwm_max_dc(i);
    }

#define D(x) (!dout(x) ^ !dout_invert(x))
#define P(x,y) PWM(pwm_enable(x), pwm_value(x), pwm_offset(x), pwm_scale(x), \
        pwm_min_dc(x), pwm_max_dc(x), D(y), D(y+1), pwm_is_pdm, pwm_pwmdir(x))
    write32( P(0,10) | (P(1, 12) << 16));
    write32( P(2,14) | (P(3, 16) << 16));

    for(i=0; i<10; i++) {
        if(!dout(i) ^ !dout_invert(i)) r |= (1<<i);
    }
    if (encoder_z_polarity) r |= 0x8000;
    if (watchdog) r |= 0x4000;
    if (test_encoder) r |= 0x2000;
    write16(r);
}

FUNCTION(read) {
    int i;
    __u32 ppdata;

    EPP_ADDR(0);
    EPP_DIR_READ();

    for(i=0; i<4; i++) {
        int64_t count, index;
	int newlow, indlow, indexed;
        int reset;
        ppdata = read32();
        reset = encoder_reset(i);
        if(i == 0) {
            int status = inb(ioaddr+1) & 1;
            if(status) {
                communication_error ++;
		pluto_clear_error_register();
            }
            if(communication_error) { EPP_DIR_WRITE(); return; }
        }

	newlow = ppdata & MASK;
	indlow = (ppdata >> W) & MASK;
	indexed = ppdata & (1<<(2*W));

        count = extend(data.last_count[i], newlow, W);
	if(indexed)
	    index = extend(count, indlow, W);
	else
	    index = data.last_index[i];

        if(encoder_index_enable(i) && indexed) {
            encoder_index_enable(i) = 0;
            data.reset_count[i] = index;
        }
        if(reset) encoder_velocity(i) = 0;
        else encoder_velocity(i) = (count - data.last_count[i]) /
            encoder_scale(i) / fperiod;
        data.last_index[i] = index;
        data.last_count[i] = count;
        if(reset) data.reset_count[i] = count;
        encoder_count(i) = count - data.reset_count[i];
        encoder_position(i) = encoder_count(i) / encoder_scale(i);

        if(i == 0) {
            debug_0 = ppdata; debug_1 = count;
        }
    }

    ppdata = read32();

    for(i=0; i< 20; i++) {
        int b = ppdata & (1<<i);
        din(i) = !!b; din_not(i) = !b;
    }

    EPP_DIR_WRITE();
}

#include "hal/drivers/pluto_servo_rbf.h"

EXTRA_SETUP() {
    return pluto_setup(firmware);
}

EXTRA_CLEANUP() {
    pluto_cleanup();
}

// vim:sts=4:et:sw=4