Advanced Search

loadrt trivkins
loadrt motmod base_period=1000000 servo_period=1000000

# Load parallel port drivers
loadrt hal_parport cfg="0x378 out 0x278 out"

# Define DAC (AD664JN-BIP) control and data pins
net dac-data <= parport.0.pin-02-09
net dac-wr => parport.0.pin-10-out
net dac-cs => parport.0.pin-11-out

# Define quadrature encoders
net x-pos-a => parport.0.pin-12-in
net x-pos-b => parport.0.pin-13-in
net y-pos-a => parport.0.pin-14-in
net y-pos-b => parport.0.pin-15-in
net z-pos-a => parport.0.pin-16-in
net z-pos-b => parport.0.pin-17-in
net spindle-pos-a => parport.1.pin-10-in
net spindle-pos-b => parport.1.pin-11-in

# Define limit and home switches
net x-neg-limit => parport.1.pin-12-in
net x-pos-limit => parport.1.pin-13-in
net x-home => parport.1.pin-14-in

net y-neg-limit => parport.1.pin-15-in
net y-pos-limit => parport.1.pin-16-in
net y-home => parport.1.pin-17-in

net z-neg-limit => parport.1.pin-18-in
net z-pos-limit => parport.1.pin-19-in
net z-home => parport.1.pin-20-in

# Define global enable output
net global-enable => parport.0.pin-01-out

# Define GPIO pins
net gpio-in-00 <= parport.1.pin-02-in
net gpio-in-01 <= parport.1.pin-03-in
net gpio-in-02 <= parport.1.pin-04-in
net gpio-in-03 <= parport.1.pin-05-in
net gpio-in-04 <= parport.1.pin-06-in
net gpio-in-05 <= parport.1.pin-07-in

net gpio-out-00 => parport.1.pin-08-out
net gpio-out-01 => parport.1.pin-09-out
net gpio-out-02 => parport.1.pin-15-out
net gpio-out-03 => parport.1.pin-16-out
net gpio-out-04 => parport.1.pin-01-out
net gpio-out-05 => parport.1.pin-14-out

addf parport.0.read base-thread
addf parport.1.read base-thread
addf parport.0.write base-thread
addf parport.1.write base-thread

start

#include "hal.h"
#include "rtapi.h"
#include "rtapi_app.h"
#include "rtapi_bitops.h"
#include "hal_parport.h"

// Define pins and parameters
static int comp_id;
static hal_float_t *x_output;
static hal_float_t *y_output;
static hal_float_t *z_output;
static hal_float_t *spindle_output;
static hal_bit_t *global_enable;
static hal_bit_t *x_home;
static hal_bit_t *y_home;
static hal_bit_t *z_home;

// Limit switches
static hal_bit_t *x_neg_limit, *x_pos_limit;
static hal_bit_t *y_neg_limit, *y_pos_limit;
static hal_bit_t *z_neg_limit, *z_pos_limit;

// Quadrature encoder positions
static hal_bit_t *x_pos_a, *x_pos_b;
static hal_bit_t *y_pos_a, *y_pos_b;
static hal_bit_t *z_pos_a, *z_pos_b;
static hal_bit_t *spindle_pos_a, *spindle_pos_b;

// GPIO pins
static hal_bit_t *gpio_in_00, *gpio_in_01, *gpio_in_02, *gpio_in_03, *gpio_in_04, *gpio_in_05;
static hal_bit_t *gpio_out_00, *gpio_out_01, *gpio_out_02, *gpio_out_03, *gpio_out_04, *gpio_out_05;

// Previous states for quadrature decoding
static int prev_x_pos = 0, prev_y_pos = 0, prev_z_pos = 0, prev_spindle_pos = 0;
// Position counters
static int x_counter = 0, y_counter = 0, z_counter = 0, spindle_counter = 0;

#define SCALE_FACTOR 204.8 // Scale factor for 12-bit DAC with ±10V range

// Lookup table for quadrature decoding
const int quad_table[16] = {
    0, -1,  1,  0,
    1,  0,  0, -1,
   -1,  0,  0,  1,
    0,  1, -1,  0
};

static void write_dac_value(uint8_t data)
{
    hal_parport_write_data(0, data);
    hal_parport_write_pin(0, 11, 0); // CS low
    hal_parport_write_pin(0, 10, 0); // WR low
    hal_parport_write_pin(0, 10, 1); // WR high
    hal_parport_write_pin(0, 11, 1); // CS high
}

static void update(void *arg, long period)
{
    uint16_t x_value = (uint16_t)((*x_output + 10.0) * SCALE_FACTOR); // Scaling from -10V to 10V
    uint16_t y_value = (uint16_t)((*y_output + 10.0) * SCALE_FACTOR);
    uint16_t z_value = (uint16_t)((*z_output + 10.0) * SCALE_FACTOR);
    uint16_t spindle_value = (uint16_t)((*spindle_output + 10.0) * SCALE_FACTOR);

    // Send data to DAC for each axis in sequence
    write_dac_value((x_value >> 4) & 0xFF); // Send upper 8 bits
    write_dac_value((x_value << 4) & 0xF0); // Send lower 4 bits

    write_dac_value((y_value >> 4) & 0xFF); // Send upper 8 bits
    write_dac_value((y_value << 4) & 0xF0); // Send lower 4 bits

    write_dac_value((z_value >> 4) & 0xFF); // Send upper 8 bits
    write_dac_value((z_value << 4) & 0xF0); // Send lower 4 bits

    write_dac_value((spindle_value >> 4) & 0xFF); // Send upper 8 bits
    write_dac_value((spindle_value << 4) & 0xF0); // Send lower 4 bits

    hal_parport_write_pin(0, 1, *global_enable);

    // Read limit and home switches
    *x_neg_limit = hal_parport_read_pin(1, 12);
    *x_pos_limit = hal_parport_read_pin(1, 13);
    *x_home = hal_parport_read_pin(1, 14);

    *y_neg_limit = hal_parport_read_pin(1, 15);
    *y_pos_limit = hal_parport_read_pin(1, 16);
    *y_home = hal_parport_read_pin(1, 17);

    *z_neg_limit = hal_parport_read_pin(1, 18);
    *z_pos_limit = hal_parport_read_pin(1, 19);
    *z_home = hal_parport_read_pin(1, 20);

    // Read quadrature encoder inputs
    int x_pos = hal_parport_read_pin(0, 12) | (hal_parport_read_pin(0, 13) << 1);
    int y_pos = hal_parport_read_pin(0, 14) | (hal_parport_read_pin(0, 15) << 1);
    int z_pos = hal_parport_read_pin(0, 16) | (hal_parport_read_pin(0, 17) << 1);
    int spindle_pos = hal_parport_read_pin(1, 10) | (hal_parport_read_pin(1, 11) << 1);

    // Quadrature decoding
    int x_delta = quad_table[(prev_x_pos << 2) | x_pos];
    int y_delta = quad_table[(prev_y_pos << 2) | y_pos];
    int z_delta = quad_table[(prev_z_pos << 2) | z_pos];
    int spindle_delta = quad_table[(prev_spindle_pos << 2) | spindle_pos];

    x_counter += x_delta;
    y_counter += y_delta;
    z_counter += z_delta;
    spindle_counter += spindle_delta;

    prev_x_pos = x_pos;
    prev_y_pos = y_pos;
    prev_z_pos = z_pos;
    prev_spindle_pos = spindle_pos;

    // Update GPIO output pins
    hal_parport_write_pin(1, 8, *gpio_out_00);
    hal_parport_write_pin(1, 9, *gpio_out_01);
    hal_parport_write_pin(1, 15, *gpio_out_02);
    hal_parport_write_pin(1, 16, *gpio_out_03);
    hal_parport_write_pin(1, 1, *gpio_out_04);
    hal_parport_write_pin(1, 14, *gpio_out_05);

    // Read GPIO input pins
    *gpio_in_00 = hal_parport_read_pin(1, 2);
    *gpio_in_01 = hal_parport_read_pin(1, 3);
    *gpio_in_02 = hal_parport_read_pin(1, 4);
    *gpio_in_03 = hal_parport_read_pin(1, 5);
    *gpio_in_04 = hal_parport_read_pin(1, 6);
    *gpio_in_05 = hal_parport_read_pin(1, 7);
}

int rtapi_app_main(void)
{
    comp_id = hal_init("my_driver");
    if (comp_id < 0)
    {
        return comp_id;
    }

    hal_pin_float_new("my_driver.x-output", HAL_OUT, &x_output, comp_id);
    hal_pin_float_new("my_driver.y-output", HAL_OUT, &y_output, comp_id);
    hal_pin_float_new("my_driver.z-output", HAL_OUT, &z_output, comp_id);
    hal_pin_float_new("my_driver.spindle-output", HAL_OUT, &spindle_output, comp_id);
    hal_pin_bit_new("my_driver.global-enable", HAL_OUT, &global_enable, comp_id);
    hal_pin_bit_new("my_driver.x-home", HAL_IN, &x_home, comp_id);
    hal_pin_bit_new("my_driver.y-home", HAL_IN, &y_home, comp_id);
    hal_pin_bit_new("my_driver.z-home", HAL_IN, &z_home, comp_id);

    hal_pin_bit_new("my_driver.x-neg-limit", HAL_IN, &x_neg_limit, comp_id);
    hal_pin_bit_new("my_driver.x-pos-limit", HAL_IN, &x_pos_limit, comp_id);
    hal_pin_bit_new("my_driver.y-neg-limit", HAL_IN, &y_neg_limit, comp_id);
    hal_pin_bit_new("my_driver.y-pos-limit", HAL_IN, &y_pos_limit, comp_id);
    hal_pin_bit_new("my_driver.z-neg-limit", HAL_IN, &z_neg_limit, comp_id);
    hal_pin_bit_new("my_driver.z-pos-limit", HAL_IN, &z_pos_limit, comp_id);

    hal_pin_bit_new("my_driver.x-pos-a", HAL_IN, &x_pos_a, comp_id);
    hal_pin_bit_new("my_driver.x-pos-b", HAL_IN, &x_pos_b, comp_id);
    hal_pin_bit_new("my_driver.y-pos-a", HAL_IN, &y_pos_a, comp_id);
    hal_pin_bit_new("my_driver.y-pos-b", HAL_IN, &y_pos_b, comp_id);
    hal_pin_bit_new("my_driver.z-pos-a", HAL_IN, &z_pos_a, comp_id);
    hal_pin_bit_new("my_driver.z-pos-b", HAL_IN, &z_pos_b, comp_id);
    hal_pin_bit_new("my_driver.spindle-pos-a", HAL_IN, &spindle_pos_a, comp_id);
    hal_pin_bit_new("my_driver.spindle-pos-b", HAL_IN, &spindle_pos_b, comp_id);

    hal_pin_bit_new("my_driver.gpio-in-00", HAL_IN, &gpio_in_00, comp_id);
    hal_pin_bit_new("my_driver.gpio-in-01", HAL_IN, &gpio_in_01, comp_id);
    hal_pin_bit_new("my_driver.gpio-in-02", HAL_IN, &gpio_in_02, comp_id);
    hal_pin_bit_new("my_driver.gpio-in-03", HAL_IN, &gpio_in_03, comp_id);
    hal_pin_bit_new("my_driver.gpio-in-04", HAL_IN, &gpio_in_04, comp_id);
    hal_pin_bit_new("my_driver.gpio-in-05", HAL_IN, &gpio_in_05, comp_id);

    hal_pin_bit_new("my_driver.gpio-out-00", HAL_OUT, &gpio_out_00, comp_id);
    hal_pin_bit_new("my_driver.gpio-out-01", HAL_OUT, &gpio_out_01, comp_id);
    hal_pin_bit_new("my_driver.gpio-out-02", HAL_OUT, &gpio_out_02, comp_id);
    hal_pin_bit_new("my_driver.gpio-out-03", HAL_OUT, &gpio_out_03, comp_id);
    hal_pin_bit_new("my_driver.gpio-out-04", HAL_OUT, &gpio_out_04, comp_id);
    hal_pin_bit_new("my_driver.gpio-out-05", HAL_OUT, &gpio_out_05, comp_id);

    rtapi_snprintf(name, sizeof(name), "my_driver.update");
    hal_periodic_func_new(update, NULL, 1000000);

    hal_ready(comp_id);
    return 0;
}

void rtapi_app_exit(void)
{
    hal_exit(comp_id);
}