Swapping a single axis out

[devtanc]

I'm using the grblHAL breakout board and teensy 4.1 board to run a machine and have a use case where I have multiple nozzles extruding onto a surface. I have a X, Y, and Z axis, but each nozzle ends up needing it's own axis as well, and I have more than 2 of them. This exceeds the 5 axis limit for the current software and the board.

However, I only need to drive a single nozzle at a time, so at any given point in the operation, only 4 axis are running simultaneously. What I'd like to do is have more axis, but switch between which one is currently being synced with the motion of the other axis. I'm hoping that this sort of use isn't too bad to figure out, since the main brains of syncing axis doesn't need to support more, I would just need the additional axis positional data be able to swap out in memory when that axis becomes active, basically making it so that there are multiple like, virtual axis for a single axis, but only one of them is driven at a time.

Is this something that would be possible via a plugin, or is it already possible and I'm just not understanding how to do it the right way, or would this take a bigger overhaul of things to implement?

Thanks!

[terjeio]

A plugin should be feasible - with some electronically controlled switches (logic gates) for the output signals.
Or switch to a board that can handle 6 or more axes. The most capable is reference map for the STM32Fxx driver, it can handle up to 8 axes - but no board for that as of now.

FYI the Bigtreetech Octopus board has 8 stepper drivers, but grblHAL does not support that (yet?). It is STM32F4xx based. However, I am not sure each driver is individually accessible.

[devtanc]

Update here, I was able to write a plugin that tracked and saved the current positions of each axis, and would simply swap them out when changing axis and it worked really well. With the setup, even homing multiple sets of duplicate axis worked. I was able to drive X, Y, Z1, Z2, A1, A2, and B and could swap between Z1+A1 and Z2+A2 and keep track of the position between swaps without issue. So the concept does work, though may not recommend it for general use cases. Theoretically, this should be able to handle more states than just a few.

I had a custom muxing board to swap where the motor signals were routed based on the aux pins and a couple unused features on our machine. Be kind, this code is outside my typical comfort zone, so it may have some weird things in it, but it did work well for the purposes of this test.

#include "grbl/hal.h"
#include "grbl/protocol.h"

#define SELECTION_DEBOUNCE 50 // ms - increase if relay is slow and/or bouncy
#define NUM_AXIS_STATES 4
const uint8_t max_pin_selection = 64;

typedef enum {
  Aux_0 = 0,
  Aux_1,
  Aux_2,
} pin_aux_t;

typedef enum {
  x_axis = 0,
  y_axis,
  z_axis,
  a_axis,
  b_axis
} axis_i_t;

typedef struct {
  axis_i_t axis;
  uint8_t new_state;
} swap_t;

uint8_t state_sel_per_axis[N_AXIS];
uint32_t axis_states[NUM_AXIS_STATES][N_AXIS];
/**
 * axis_states gives us an array for each axis state like:
 * [
 *    [x0, y0, z0, a0, b0],
 *    [x1, y1, z1, a1, b1],
 *    [x2, y2, z2, a2, b2],
 *    [x3, y3, z3, a3, b3]
 * ]
 * We can use this to set the machine position to the state we want based on any combination of axes
 * by using the state_sel_per_axis array to select the state for each axis.
 * 
 * state_sel_per_axis is an array of indexes, that correspond to the axes [x,y,z,a,b]
 * 
 * state_sel_per_axis     [0, 0, 2, 2, 1]
 * [
 *   0: [(x0), (y0),  z0 ,  a0 ,  b0 ],
 *   1: [ x1 ,  y1 ,  z1 ,  a1 , (b1)],
 *   2: [ x2 ,  y2 , (z2), (a2),  b2 ],
 *   3: [ x3 ,  y3 ,  z3 ,  a3 ,  b3 ]
 * ]
 * 
 * With the resulting machine position being: [x0, y0, z2, a2, b1]
*/
static uint8_t prev_pin_selection = 0; // Default pin select
static uint8_t pin_selection = 0; // Default pin select
static user_mcode_ptrs_t user_mcode;
static on_report_options_ptr on_report_options;
static on_realtime_report_ptr on_realtime_report;
static on_homing_completed_ptr on_homing_completed;

char pin_flags[6][2] = { "0", "1", "2", "M", "F", "S" };

static void report_options (bool newopt)
{
    on_report_options(newopt);

    if(!newopt)
        hal.stream.write("[PLUGIN:MUXv0.1]" ASCII_EOL);
}

static void realtime_report (stream_write_ptr stream_write, report_tracking_flags_t report)
{
  if (prev_pin_selection != pin_selection) {
    prev_pin_selection = pin_selection;
    stream_write("|MUX:");
    for (int shift = 0; shift < 6; shift++) {
      if (pin_selection>>shift & 0b0001)
        stream_write(pin_flags[shift]);
    }
    stream_write("'");
    for (int i = 0; i < N_AXIS; i++) {
      stream_write(uitoa(state_sel_per_axis[i]));
    }
  }

  if(on_realtime_report)
    on_realtime_report(stream_write, report);
}

static void homing_completed ()
{
  // Copy the homed position as the initial state for all axis and axis states
  for (int i = 0; i < NUM_AXIS_STATES; i++) {
    memcpy(axis_states[i], sys.position, N_AXIS * sizeof(int32_t));
  }

  hal.stream.write("Mux plugin initialized!" ASCII_EOL);

  if(on_homing_completed)
    on_homing_completed();
}

/**
 * Swaps the current sys.position axis with a new axis position chosen by new_state and axis
 * Given the state:
 * 
 * sys.position           [x0, y0, z2, a2, b1]
 * state_sel_per_axis     [0, 0, 2, 2, 1]
 * [
 *   0: [(x0), (y0),  z0 ,  a0 ,  b0 ],
 *   1: [ x1 ,  y1 ,  z1 ,  a1 , (b1)],
 *   2: [ x2 ,  y2 , (z2), (a2),  b2 ],
 *   3: [ x3 ,  y3 ,  z3 ,  a3 ,  b3 ]
 * ]
 * 
 * And given the input (z, 1), the resulting state would be:
 * 
 * sys.position           [x0, y0, z1, a2, b1]
 * state_sel_per_axis     [0, 0, 1, 2, 1]
 * [
 *   0: [(x0), (y0),  z0 ,  a0 ,  b0 ],
 *   1: [ x1 ,  y1 , (z1),  a1 , (b1)],
 *   2: [ x2 ,  y2 , -z2-, (a2),  b2 ],  // the z2 value is now the old sys.position[z] value
 *   3: [ x3 ,  y3 ,  z3 ,  a3 ,  b3 ]
 * ]
*/
static void swap_axis_state (axis_i_t axis, uint8_t new_state) {
  if (axis >= N_AXIS || new_state >= NUM_AXIS_STATES) {
    return;
  }
  uint8_t curr_state_for_axis = state_sel_per_axis[axis];
  if (curr_state_for_axis == new_state) {
    return;
  }

  // store the current position for the axis
  uint32_t temp = sys.position[axis];
  // insert the position for the axis at the new_state into the sys.position at axis
  sys.position[axis] = axis_states[new_state][axis];
  // insert the temp variable into the curr_state position for the axis
  axis_states[curr_state_for_axis][axis] = temp;
  state_sel_per_axis[axis] = new_state;
}

/**
 * Swaps multiple axes states at once, then syncs the machine position with the new state
*/
static void swap_axis_states (swap_t *swaps, uint8_t num_swaps) {
  for (int i = 0; i < num_swaps; i++) {
    hal.stream.write(uitoa(swaps[i].axis));
    hal.stream.write(": [");
    hal.stream.write(uitoa(state_sel_per_axis[swaps[i].axis]));
    hal.stream.write("->");
    hal.stream.write(uitoa(swaps[i].new_state));
    hal.stream.write("]" ASCII_EOL);
    swap_axis_state(swaps[i].axis, swaps[i].new_state);
  }
  sync_position();
}

/**
 * Determines axes to swap based on pin selection
*/
static void swap_axes (uint8_t pin_selection) {
  static swap_t swaps[N_AXIS];
  for (int i = 0; i < N_AXIS; i++) {
    swaps[i] = (swap_t){.axis = i, .new_state = state_sel_per_axis[i]};
  }

  // TODO: This next bit is not very dynamic, but it works for now
  // It'll need to change when the new mux board is done
  bool flood_pin_set = pin_selection>>4 & 1;
  if (flood_pin_set) {
    // Swap z, a, b
    swaps[z_axis].new_state = 1;
    swaps[a_axis].new_state = 1;
    swaps[b_axis].new_state = 1;
  } else {
    swaps[z_axis].new_state = 0;
    swaps[a_axis].new_state = 0;
    swaps[b_axis].new_state = 0;
  }

  swap_axis_states(swaps, N_AXIS);
}

static user_mcode_t mcode_check (user_mcode_t mcode)
{
    return mcode == (user_mcode_t)101 || mcode == (user_mcode_t)102
                     ? mcode
                     : (user_mcode.check ? user_mcode.check(mcode) : UserMCode_Ignore);
}

static status_code_t mcode_validate (parser_block_t *gc_block, parameter_words_t *deprecated)
{
    status_code_t state = Status_OK;

    switch((uint16_t)gc_block->user_mcode) {

        case 101:
            if(gc_block->words.q) {
                if(isnanf(gc_block->values.q))
                    state = Status_BadNumberFormat;
                else {
                    if(!isintf(gc_block->values.q) || gc_block->values.q < 0.0f || (uint8_t)gc_block->values.q > max_pin_selection - 1)
                        state = Status_GcodeValueOutOfRange;
                    else
                        state = Status_OK;
                    gc_block->words.q = Off;
                    gc_block->user_mcode_sync = true;
                }
            }
            break;

        case 102:
          gc_block->user_mcode_sync = true;
          break;

        default:
            state = Status_Unhandled;
            break;
    }

    return state == Status_Unhandled && user_mcode.validate ? user_mcode.validate(gc_block, deprecated) : state;
}

static void mcode_execute (uint_fast16_t state, parser_block_t *gc_block)
{
    bool handled = true;

    if (state != STATE_CHECK_MODE)
    {
      coolant_state_t coolant_state = hal.coolant.get_state();
      spindle_state_t spindle_state = hal.spindle.get_state();

      switch((uint16_t)gc_block->user_mcode) {
        case 101:
            if(gc_block->words.q)
                /** 
                  * |--- PIN NUMBER
                  * V
                  ************** SHIFT REGISTER 2 (SR2)
                  *     |------------------------------ Spin
                  *     ||----------------------------- Flod
                  *     || |--------------------------- Mist
                  *     || ||-------------------------- Aux2
                  *     || |||------------------------- Aux1
                  *     || ||||------------------------ Aux0
                  *     VV VVVV
                  *   0b00'0000
                  *
                  *   0b00'1000 = Q8  = enabled: mist 
                  *   0b00'1100 = Q12 = enabled: mist                   aux2
                  *   0b01'1000 = Q24 = enabled: mist  flod       
                  *   0b01'1010 = Q26 = enabled: mist  flod       aux1
                */
                pin_selection = (uint8_t)gc_block->values.q;
                hal.port.digital_out(Aux_0, pin_selection & 1);
                hal.port.digital_out(Aux_1, pin_selection>>1 & 1);
                hal.port.digital_out(Aux_2, pin_selection>>2 & 1);

                coolant_state.mist = pin_selection>>3 & 1;
                coolant_state.flood = pin_selection>>4 & 1;
                hal.coolant.set_state(coolant_state);
                sys.report.coolant = On;

                spindle_state.on = pin_selection>>5 & 1;
                hal.spindle.set_state(spindle_state, 0);
                sys.report.spindle = On;

                hal.delay_ms(SELECTION_DEBOUNCE, NULL); // Delay a bit to let any contact bounce settle.

                swap_axes(pin_selection);
            break;

        case 102:
            hal.port.digital_out(Aux_0, 0);
            hal.port.digital_out(Aux_1, 0);
            hal.port.digital_out(Aux_2, 0);

            hal.coolant.set_state((coolant_state_t){0});
            sys.report.coolant = On;
            hal.spindle.set_state((spindle_state_t){0}, 0);
            sys.report.spindle = On;

            pin_selection = 0;
            hal.delay_ms(SELECTION_DEBOUNCE, NULL); // Delay a bit to let any contact bounce settle.
          break;

        default:
            handled = false;
            break;
      }
    }

    if(!handled && user_mcode.execute)
        user_mcode.execute(state, gc_block);
}

static void warning_msg (uint_fast16_t state)
{
    report_message("Mux plugin failed to initialize!", Message_Warning);
}

void plug_init(void)
{
    if(hal.port.num_digital_out >= 3) {
      if(hal.port.set_pin_description) {
            hal.port.set_pin_description(Port_Digital, Port_Output, Aux_0, "MUX PIN 1");
            hal.port.set_pin_description(Port_Digital, Port_Output, Aux_1, "MUX PIN 2"); 
            hal.port.set_pin_description(Port_Digital, Port_Output, Aux_2, "MUX PIN 3");
      }
      
      memcpy(&user_mcode, &hal.user_mcode, sizeof(user_mcode_ptrs_t));

      hal.user_mcode.check = mcode_check;
      hal.user_mcode.validate = mcode_validate;
      hal.user_mcode.execute = mcode_execute;

      on_report_options = grbl.on_report_options;
      grbl.on_report_options = report_options;

      on_realtime_report = grbl.on_realtime_report;
      grbl.on_realtime_report = realtime_report;

      on_homing_completed = grbl.on_homing_completed;
      grbl.on_homing_completed = homing_completed;

      pin_selection = 0;
      prev_pin_selection = 64;
      for (int i = 0; i < N_AXIS; i++) {
        // make sure that the current state selections are all the first state
        state_sel_per_axis[i] = 0;
      }
    }
    else {
      protocol_enqueue_rt_command(warning_msg);
    }
}