Problem with custom M-code order

[devtanc]

I wrote a plugin that adds a custom M command to drive the AUX and flood/mist/spindle pins such that when you send M101 Q1 it would turn on a binary representation of that number on the pins, allowing for 64 different pin states (since we're not using the cool or spindle functions for anything else in my use-case). My issue is that when I send the Gcode via websocket to the board, the m code command, while very clearly being after all movement, is executed before the machine stops moving. I'm not sure with the documentation, but is there a certain step I'm missing in my plugin to handle waiting for the movement of the axis to complete before execution?

This is the current code for the m-code handling of my plugin:

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;
                    gc_block->words.q = Off;
                }
            }
            break;

        case 102:
          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_selection = (uint8_t)gc_block->values.q;
                hal.port.digital_out(Aux_0, pin_selection & 0b0001);
                hal.port.digital_out(Aux_1, pin_selection>>1 & 0b0001);
                hal.port.digital_out(Aux_2, pin_selection>>2 & 0b0001);

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

                spindle_state.on = pin_selection>>5 & 0b0001;
                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.
            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);
}

Is there a place here where I can basically enqueue the execution of the m command until its order in the actual execution of the gcode? This is a snippet of the end of the gcode where I'm issuing the M102 command which turns off all pins. The pins are actually being triggered time-wise at about the beginning of this section of gcode, rather than nearly 5 seconds later, after the G0 X0 Y0 Z0 command is complete.

G1 X2.84391 Y1.76805  A0.04465
G1 X2.70321 Y1.99107  A0.04476
G1 X2.54500 Y2.20289  A0.04488
G1 X2.37018 Y2.40212  A0.04499
G1 F2500 ; Pins change around here
G1 X4.54249 Y0.87083  
G1 X2.75876 Y2.54603  
G1 X2.58425 Y2.81492  
G1 X2.38436 Y3.07175  
G1 X2.15992 Y3.31418  
G1 X1.91199 Y3.53994  
G1 X1.64183 Y3.74681  
G1 X1.35092 Y3.93269  
G1 X1.04092 Y4.09560  
G1Z10
G90
G0 X0 Y0 Z0
G91
M102 ; Command to turn off pins is here

[terjeio]

Set the flag telling the core to execute after synchronizing.

[devtanc]

This worked wonderfully, thank you!