dagoma m600

/**
   * M600: Pause for filament change
   *
   *  E[distance] - Retract the filament this far (negative value)
   *  Z[distance] - Move the Z axis by this distance
   *  X[position] - Move to this X position, with Y
   *  Y[position] - Move to this Y position, with X
   *  L[distance] - Retract distance for removal (manual reload)
   *  P[pin]      - Pin to wait for, if not specified use lcd button
   *              - Pin can be A, B or C respectively for X, Y and Z endstops.
   *  S[0|1]      - If Pin, state to wait for, if not specified use LOW
   *
   *  Default values are used for omitted arguments.
   *
   */
  inline void gcode_M600() {

    SERIAL_ECHOLNPGM( "Pause for filament change" );

    if (cant_enter_M600_or_D600()) return;

    #if ENABLED(SUMMON_PRINT_PAUSE)
      // Simulate direct call M600
      print_pause_summoned = true;
    #endif

    float lastpos[NUM_AXIS];
    float previous_feedrate;

    for (int i = 0; i < NUM_AXIS; i++)
      lastpos[i] = destination[i] = current_position[i];

    previous_feedrate = feedrate;

    //finish moves
    // st_synchronize();

    //retract by E
    if (code_seen('E')) destination[E_AXIS] += code_value();
    #ifdef FILAMENTCHANGE_FIRSTRETRACT
      else destination[E_AXIS] += FILAMENTCHANGE_FIRSTRETRACT;
    #endif

    SET_FEEDRATE_FOR_EXTRUDER_MOVE;
    RUNPLAN;

    //lift Z
    #if ENABLED(DELTA_EXTRA)
      float z_destination = destination[Z_AXIS];
      if (code_seen('Z')) z_destination += code_value();
      #ifdef FILAMENTCHANGE_ZADD
        else z_destination += FILAMENTCHANGE_ZADD;
      #endif
      NOMORE(z_destination, (Z_MAX_POS-25.0));
      if (z_destination > destination[Z_AXIS]) {
        destination[Z_AXIS] = z_destination;
      }
    #else
      if (code_seen('Z')) destination[Z_AXIS] += code_value();
      #ifdef FILAMENTCHANGE_ZADD
        else destination[Z_AXIS] += FILAMENTCHANGE_ZADD;
      #endif
    #endif

    SET_FEEDRATE_FOR_MOVE;
    RUNPLAN;

    //move xy
    if (code_seen('X')) destination[X_AXIS] = code_value();
    #ifdef FILAMENTCHANGE_XPOS
      else destination[X_AXIS] = FILAMENTCHANGE_XPOS;
    #endif

    if (code_seen('Y')) destination[Y_AXIS] = code_value();
    #ifdef FILAMENTCHANGE_YPOS
      else destination[Y_AXIS] = FILAMENTCHANGE_YPOS;
    #endif

    SET_FEEDRATE_FOR_MOVE;
    RUNPLAN;

    if (code_seen('L')) destination[E_AXIS] += code_value();
    #ifdef FILAMENTCHANGE_FINALRETRACT
      else destination[E_AXIS] += FILAMENTCHANGE_FINALRETRACT;
    #endif

    SET_FEEDRATE_FOR_EXTRUDER_MOVE;
    RUNPLAN;

    // validate planned all moves
    st_synchronize();

    // DAGOMA added
    // Determine exit/pin state after moving away
    int pin_number = -1;
    int target = -1;
    if (code_seen('P')) {
      char nextChar = *(seen_pointer + 1);
      if (nextChar == 'A') {
        pin_number = X_MIN_PIN;
      }
      else if (nextChar == 'B') {
        pin_number = Y_MAX_PIN;
      }
      else if (nextChar == 'C') {
        pin_number = Z_MIN_PIN;
      }
      else {
        pin_number = code_value();
      }

      int pin_state = code_seen('S') ? code_value() : -1; // required pin state - default is inverted

      if (pin_state >= -1 && pin_state <= 1) {

        // DAGOMA - byPass sensitive pin
        /*
        for (uint8_t i = 0; i < COUNT(sensitive_pins); i++) {
          if (sensitive_pins[i] == pin_number) {
            pin_number = -1;
            break;
          }
        }
        */
        if (pin_number > -1) {
          target = LOW;

          //pinMode(pin_number, INPUT);

          switch (pin_state) {
            case 1:
              target = HIGH;
              break;

            case 0:
              target = LOW;
              break;

            case -1:
              target = !digitalRead(pin_number);
              break;
          }
        } // pin_number > -1
      } // pin_state -1 0 1
    } // code_seen('P')
    // END DAGOMA added