#include #include // For round, fabs /* ==== PIN MAP (ESP32-C3-DevKitM-1) ==================================== */ // Motor 1 #define EN_PIN_1 5 // Enable pin for Motor 1 (LOW = on) #define STEP_PIN_1 6 // Step pin for Motor 1 #define DIR_PIN_1 7 // Direction pin for Motor 1 #define LIMIT_SWITCH_PIN_M1 3 // Limit switch for M1 (COM to GPIO3, NC to GND) // Motor 2 #define EN_PIN_2 4 // Enable pin for Motor 2 (LOW = on) #define STEP_PIN_2 18 // Step pin for Motor 2 #define DIR_PIN_2 19 // Direction pin for Motor 2 #define LIMIT_SWITCH_PIN_M2 0 // Limit switch for M2 (COM to GPIO0, NC to GND) /* ==== CONFIGURATION ================================================== */ const bool M1_DIRECTION_LOGIC_INVERTED = false; const bool M2_DIRECTION_LOGIC_INVERTED = true; /* ==== TIMING & HOMING CONSTANTS ======================================= */ const uint16_t STEP_PULSE_WIDTH_US = 5; const uint16_t HOMING_STEP_PERIOD_US_M1 = 150; const uint16_t HOMING_STEP_PERIOD_US_M2 = 150; const uint16_t DEFAULT_MOVE_STEP_PERIOD_US = 150; const long HOMING_BACKOFF_STEPS_M1 = 8960; const long HOMING_BACKOFF_STEPS_M2 = 8960; /* ==== CALIBRATION & LIMITS ============================================ */ float M1_steps_per_mm = 3200.0; float M2_steps_per_mm = 3200.0; float M1_MAX_TRAVEL_MM = 100.0; float M2_MAX_TRAVEL_MM = 50.0; /* ==== STATE FOR MOTOR 1 =============================================== */ volatile long stepsToRun1 = 0; volatile bool motorRunning1 = false; volatile bool continuousRun1 = false; volatile uint32_t currentStepPeriodUs1 = DEFAULT_MOVE_STEP_PERIOD_US; volatile long M1_currentPositionSteps = 0; volatile bool M1_isHomed = false; /* ==== STATE FOR MOTOR 2 =============================================== */ volatile long stepsToRun2 = 0; volatile bool motorRunning2 = false; volatile bool continuousRun2 = false; volatile uint32_t currentStepPeriodUs2 = DEFAULT_MOVE_STEP_PERIOD_US; volatile long M2_currentPositionSteps = 0; volatile bool M2_isHomed = false; /* ==== PANEL MOVE SEQUENCE STATE ====================================== */ enum PanelMoveSequenceState { IDLE, M1_MOVING, M2_MOVING }; volatile PanelMoveSequenceState currentPanelMoveState = IDLE; long panel_move_M2_target_steps_remaining = 0; bool panel_move_M2_direction_physical_down = false; uint32_t panel_move_M2_period_us = DEFAULT_MOVE_STEP_PERIOD_US; String serialCommand; // Forward declarations void handleLine(const String& cmd); void homeMotor(int motorNum); uint8_t getDirPinStateForPhysicalDirection(int motorNum, bool physicalDown); void commandMotorSteps(int motorNum, long stepsCount, uint32_t period_us); void reportFinalPanelPosition(); uint8_t getDirPinStateForPhysicalDirection(int motorNum, bool physicalDown) { bool invertLogic = (motorNum == 1) ? M1_DIRECTION_LOGIC_INVERTED : M2_DIRECTION_LOGIC_INVERTED; uint8_t stateForStandardDown = HIGH; uint8_t stateForStandardUp = LOW; uint8_t effectiveStateForDown = invertLogic ? stateForStandardUp : stateForStandardDown; uint8_t effectiveStateForUp = invertLogic ? stateForStandardDown : stateForStandardUp; return physicalDown ? effectiveStateForDown : effectiveStateForUp; } void setup() { pinMode(STEP_PIN_1, OUTPUT); pinMode(DIR_PIN_1, OUTPUT); pinMode(EN_PIN_1, OUTPUT); pinMode(LIMIT_SWITCH_PIN_M1, INPUT_PULLUP); digitalWrite(DIR_PIN_1, getDirPinStateForPhysicalDirection(1, false)); digitalWrite(EN_PIN_1, HIGH); // Initially disable motor 1 pinMode(STEP_PIN_2, OUTPUT); pinMode(DIR_PIN_2, OUTPUT); pinMode(EN_PIN_2, OUTPUT); pinMode(LIMIT_SWITCH_PIN_M2, INPUT_PULLUP); digitalWrite(DIR_PIN_2, getDirPinStateForPhysicalDirection(2, false)); digitalWrite(EN_PIN_2, HIGH); // Initially disable motor 2 Serial.begin(115200); while (!Serial); Serial.println(F("")); Serial.println(F("ESP32-C3 Dual Stepper Control - Iteration 3 (Holding Torque Fix)")); Serial.println(F("--------------------------------------------------")); Serial.println(F("Commands:")); Serial.println(F(" dir <0|1> (0=UP, 1=DOWN physically)")); Serial.println(F(" steps ")); Serial.println(F(" speed (continuous run)")); Serial.println(F(" stop (stops motion, motors remain enabled for holding)")); Serial.println(F(" release (disables motors, releasing holding torque)")); // NEW COMMAND Serial.println(F(" home <1|2|all>")); Serial.println(F(" get_pos_steps <1|2>")); Serial.println(F(" get_pos_mm <1|2|all>")); Serial.println(F(" get_panel_pos_mm")); Serial.println(F(" move_panel_to ")); Serial.println(F(" set_steps_per_mm ")); Serial.println(F(" set_max_travel ")); Serial.println(F("--------------------------------------------------")); } void loop() { if (Serial.available()) { serialCommand = Serial.readStringUntil('\n'); serialCommand.trim(); if (serialCommand.length() > 0) { handleLine(serialCommand); } } // Motor 1 stepping logic if (motorRunning1) { digitalWrite(STEP_PIN_1, HIGH); delayMicroseconds(STEP_PULSE_WIDTH_US); digitalWrite(STEP_PIN_1, LOW); delayMicroseconds(currentStepPeriodUs1 - STEP_PULSE_WIDTH_US); if (M1_isHomed) { uint8_t currentDirPinState = digitalRead(DIR_PIN_1); bool physicallyMovingDown = (M1_DIRECTION_LOGIC_INVERTED) ? (currentDirPinState == LOW) : (currentDirPinState == HIGH); if (physicallyMovingDown) { M1_currentPositionSteps--; if (continuousRun1 && M1_currentPositionSteps <= 0) { M1_currentPositionSteps = 0; motorRunning1 = false; continuousRun1 = false; stepsToRun1 = 0; // digitalWrite(EN_PIN_1, HIGH); // REMOVED - Keep motor enabled for holding Serial.println(F("M1: Stopped at lower software limit (home). Holding position.")); } } else { M1_currentPositionSteps++; long max_steps_m1 = round(M1_MAX_TRAVEL_MM * M1_steps_per_mm); if (continuousRun1 && M1_currentPositionSteps >= max_steps_m1) { M1_currentPositionSteps = max_steps_m1; motorRunning1 = false; continuousRun1 = false; stepsToRun1 = 0; // digitalWrite(EN_PIN_1, HIGH); // REMOVED - Keep motor enabled for holding Serial.println(F("M1: Stopped at upper software limit. Holding position.")); } } } if (!continuousRun1 && --stepsToRun1 == 0) { motorRunning1 = false; // digitalWrite(EN_PIN_1, HIGH); // REMOVED - Keep motor enabled for holding Serial.println(F("M1: Finished move. Holding position.")); if (currentPanelMoveState == M1_MOVING) { if (panel_move_M2_target_steps_remaining != 0) { Serial.println(F("Panel: M1 finished, starting M2.")); digitalWrite(DIR_PIN_2, getDirPinStateForPhysicalDirection(2, panel_move_M2_direction_physical_down)); commandMotorSteps(2, labs(panel_move_M2_target_steps_remaining), panel_move_M2_period_us); currentPanelMoveState = M2_MOVING; } else { Serial.println(F("Panel: M1 finished, M2 requires no further movement. Sequence complete.")); currentPanelMoveState = IDLE; reportFinalPanelPosition(); } } } } // Motor 2 stepping logic if (motorRunning2) { digitalWrite(STEP_PIN_2, HIGH); delayMicroseconds(STEP_PULSE_WIDTH_US); digitalWrite(STEP_PIN_2, LOW); delayMicroseconds(currentStepPeriodUs2 - STEP_PULSE_WIDTH_US); if (M2_isHomed) { uint8_t currentDirPinState = digitalRead(DIR_PIN_2); bool physicallyMovingDown = (M2_DIRECTION_LOGIC_INVERTED) ? (currentDirPinState == LOW) : (currentDirPinState == HIGH); if (physicallyMovingDown) { M2_currentPositionSteps--; if (continuousRun2 && M2_currentPositionSteps <= 0) { M2_currentPositionSteps = 0; motorRunning2 = false; continuousRun2 = false; stepsToRun2 = 0; // digitalWrite(EN_PIN_2, HIGH); // REMOVED - Keep motor enabled for holding Serial.println(F("M2: Stopped at lower software limit (home). Holding position.")); } } else { M2_currentPositionSteps++; long max_steps_m2 = round(M2_MAX_TRAVEL_MM * M2_steps_per_mm); if (continuousRun2 && M2_currentPositionSteps >= max_steps_m2) { M2_currentPositionSteps = max_steps_m2; motorRunning2 = false; continuousRun2 = false; stepsToRun2 = 0; // digitalWrite(EN_PIN_2, HIGH); // REMOVED - Keep motor enabled for holding Serial.println(F("M2: Stopped at upper software limit. Holding position.")); } } } if (!continuousRun2 && --stepsToRun2 == 0) { motorRunning2 = false; // digitalWrite(EN_PIN_2, HIGH); // REMOVED - Keep motor enabled for holding Serial.println(F("M2: Finished move. Holding position.")); if (currentPanelMoveState == M2_MOVING) { Serial.println(F("Panel: M2 finished. Sequence complete.")); currentPanelMoveState = IDLE; reportFinalPanelPosition(); } } } } void homeMotor(int motorNum) { Serial.printf("Motor %d: Homing sequence started...\n", motorNum); uint8_t enPin, dirPin, stepPin, limitPin; volatile long* currentPositionStepsPtr; volatile bool* isHomedFlagPtr; uint16_t homingStepPeriod; long homingBackoffStepsVal; const char* motorNameStr; float current_max_travel_mm; if (motorNum == 1) { enPin = EN_PIN_1; dirPin = DIR_PIN_1; stepPin = STEP_PIN_1; limitPin = LIMIT_SWITCH_PIN_M1; currentPositionStepsPtr = &M1_currentPositionSteps; isHomedFlagPtr = &M1_isHomed; homingStepPeriod = HOMING_STEP_PERIOD_US_M1; homingBackoffStepsVal = HOMING_BACKOFF_STEPS_M1; motorNameStr = "M1"; current_max_travel_mm = M1_MAX_TRAVEL_MM; } else if (motorNum == 2) { enPin = EN_PIN_2; dirPin = DIR_PIN_2; stepPin = STEP_PIN_2; limitPin = LIMIT_SWITCH_PIN_M2; currentPositionStepsPtr = &M2_currentPositionSteps; isHomedFlagPtr = &M2_isHomed; homingStepPeriod = HOMING_STEP_PERIOD_US_M2; homingBackoffStepsVal = HOMING_BACKOFF_STEPS_M2; motorNameStr = "M2"; current_max_travel_mm = M2_MAX_TRAVEL_MM; } else { Serial.println(F("Invalid motor number for homing.")); return; } digitalWrite(enPin, LOW); // Enable motor for homing *isHomedFlagPtr = false; uint8_t dirPinStateForPhysicalUp = getDirPinStateForPhysicalDirection(motorNum, false); uint8_t dirPinStateForPhysicalDown = getDirPinStateForPhysicalDirection(motorNum, true); if (digitalRead(limitPin) == HIGH) { Serial.printf("%s: Already at limit. Moving UP slightly to clear switch.\n", motorNameStr); digitalWrite(dirPin, dirPinStateForPhysicalUp); for (int i = 0; i < homingBackoffStepsVal / 2; i++) { digitalWrite(stepPin, HIGH); delayMicroseconds(STEP_PULSE_WIDTH_US); digitalWrite(stepPin, LOW); delayMicroseconds(homingStepPeriod - STEP_PULSE_WIDTH_US); if (digitalRead(limitPin) == LOW) break; } if (digitalRead(limitPin) == HIGH) { Serial.printf("%s: ERROR - Could not move clear of limit switch. Homing aborted. Motor remains enabled.\n", motorNameStr); // digitalWrite(enPin, HIGH); // REMOVED - Keep enabled return; } } Serial.printf("%s: Moving physically DOWN towards limit switch...\n", motorNameStr); digitalWrite(dirPin, dirPinStateForPhysicalDown); unsigned long homingStartTime = millis(); unsigned long maxHomingDuration = 300000; while (digitalRead(limitPin) == LOW) { digitalWrite(stepPin, HIGH); delayMicroseconds(STEP_PULSE_WIDTH_US); digitalWrite(stepPin, LOW); delayMicroseconds(homingStepPeriod - STEP_PULSE_WIDTH_US); if (millis() - homingStartTime > maxHomingDuration) { Serial.printf("%s: ERROR - Homing timeout! Limit switch not found. Motor remains enabled.\n", motorNameStr); // digitalWrite(enPin, HIGH); // REMOVED - Keep enabled return; } } Serial.printf("%s: Limit switch HIT!\n", motorNameStr); *currentPositionStepsPtr = 0; Serial.printf("%s: Moving physically UP to back off from switch...\n", motorNameStr); digitalWrite(dirPin, dirPinStateForPhysicalUp); for (long i = 0; i < homingBackoffStepsVal; i++) { digitalWrite(stepPin, HIGH); delayMicroseconds(STEP_PULSE_WIDTH_US); digitalWrite(stepPin, LOW); delayMicroseconds(homingStepPeriod - STEP_PULSE_WIDTH_US); (*currentPositionStepsPtr)++; } *isHomedFlagPtr = true; // digitalWrite(enPin, HIGH); // REMOVED - Keep motor enabled for holding Serial.printf("%s: Homing complete. Current position: %ld steps. Max travel: %.2f mm. Motor enabled for holding.\n", motorNameStr, *currentPositionStepsPtr, current_max_travel_mm); } void homeMotor1() { homeMotor(1); } void homeMotor2() { if (!M1_isHomed) { Serial.println(F("M1 must be homed before M2. Homing M2 aborted.")); return; } if (motorRunning1) { Serial.println(F("M1 is currently moving. Cannot home M2. Stop M1 first.")); return; } homeMotor(2); } void homeAll() { Serial.println(F("Starting full homing sequence...")); if (motorRunning1 || motorRunning2) { Serial.println(F("Cannot home all while motors running. Stop motors first.")); return; } homeMotor1(); if (M1_isHomed) { Serial.println(F("M1 homed successfully. Proceeding to home M2.")); homeMotor2(); } else { Serial.println(F("M1 homing failed. M2 homing aborted.")); } if (M1_isHomed && M2_isHomed) Serial.println(F("All motors homed successfully. Motors enabled for holding.")); else Serial.println(F("Full homing sequence incomplete. Check individual motor status. Motors may be holding last position.")); } void commandMotorSteps(int motorNum, long stepsCount, uint32_t period_us) { if (stepsCount <= 0) { Serial.printf("M%d: No steps to move (count was %ld).\n", motorNum, stepsCount); return; } if (period_us < (STEP_PULSE_WIDTH_US * 2)) { Serial.printf("M%d: Invalid step period %lu us. Must be >= %duS.\n", motorNum, period_us, (STEP_PULSE_WIDTH_US * 2)); return; } long actualStepsToRun = stepsCount; uint8_t dirPin = (motorNum == 1) ? DIR_PIN_1 : DIR_PIN_2; uint8_t enPin = (motorNum == 1) ? EN_PIN_1 : EN_PIN_2; bool isPhysicallyMovingDown = (digitalRead(dirPin) == getDirPinStateForPhysicalDirection(motorNum, true)); if (motorNum == 1) { if (!M1_isHomed) Serial.println(F("Warning: M1 not homed. Moving relatively. Software limits not applied.")); else { long max_allowed_steps_m1 = round(M1_MAX_TRAVEL_MM * M1_steps_per_mm); if (isPhysicallyMovingDown) { if (M1_currentPositionSteps - actualStepsToRun < 0) { actualStepsToRun = M1_currentPositionSteps; Serial.printf("M1: Move truncated to %ld steps by lower software limit (home).\n", actualStepsToRun); } } else { if (M1_currentPositionSteps + actualStepsToRun > max_allowed_steps_m1) { actualStepsToRun = max_allowed_steps_m1 - M1_currentPositionSteps; Serial.printf("M1: Move truncated to %ld steps by upper software limit (%.2f mm).\n", actualStepsToRun, M1_MAX_TRAVEL_MM); } } if (actualStepsToRun <= 0) { Serial.printf("M1: Already at or beyond limit in the specified direction. No move executed. Motor remains in current enable state.\n"); // digitalWrite(EN_PIN_1, HIGH); // REMOVED - Do not disable if no move, keep previous state return; } } stepsToRun1 = actualStepsToRun; currentStepPeriodUs1 = period_us; continuousRun1 = false; motorRunning1 = true; digitalWrite(enPin, LOW); // Enable motor for movement Serial.printf("M1: Commanding %ld steps, period %lu us.\n", actualStepsToRun, period_us); } else if (motorNum == 2) { if (!M2_isHomed) Serial.println(F("Warning: M2 not homed. Moving relatively. Software limits not applied.")); else { long max_allowed_steps_m2 = round(M2_MAX_TRAVEL_MM * M2_steps_per_mm); if (isPhysicallyMovingDown) { if (M2_currentPositionSteps - actualStepsToRun < 0) { actualStepsToRun = M2_currentPositionSteps; Serial.printf("M2: Move truncated to %ld steps by lower software limit (home).\n", actualStepsToRun); } } else { if (M2_currentPositionSteps + actualStepsToRun > max_allowed_steps_m2) { actualStepsToRun = max_allowed_steps_m2 - M2_currentPositionSteps; Serial.printf("M2: Move truncated to %ld steps by upper software limit (%.2f mm).\n", actualStepsToRun, M2_MAX_TRAVEL_MM); } } if (actualStepsToRun <= 0) { Serial.printf("M2: Already at or beyond limit in the specified direction. No move executed. Motor remains in current enable state.\n"); // digitalWrite(EN_PIN_2, HIGH); // REMOVED return; } } stepsToRun2 = actualStepsToRun; currentStepPeriodUs2 = period_us; continuousRun2 = false; motorRunning2 = true; digitalWrite(enPin, LOW); // Enable motor for movement Serial.printf("M2: Commanding %ld steps, period %lu us.\n", actualStepsToRun, period_us); } } void reportFinalPanelPosition() { if (!M1_isHomed || !M2_isHomed) { Serial.println(F("Panel: Final position reporting skipped as one or both motors not homed.")); return; } float m1_final_mm = (float)M1_currentPositionSteps / M1_steps_per_mm; float m2_final_mm = (float)M2_currentPositionSteps / M2_steps_per_mm; Serial.printf("Panel Final Position: %.3f mm (M1: %.3f mm, M2: %.3f mm)\n", (m1_final_mm + m2_final_mm), m1_final_mm, m2_final_mm); } void handleLine(const String& cmd) { Serial.print(F(">> ")); Serial.println(cmd); int firstSpace = cmd.indexOf(' '); String commandWord = (firstSpace == -1) ? cmd : cmd.substring(0, firstSpace); String args = (firstSpace == -1) ? "" : cmd.substring(firstSpace + 1); args.trim(); if (commandWord.equalsIgnoreCase("steps")) { int secondSpace = args.indexOf(' '); if (secondSpace == -1) { Serial.println(F("ERROR: Bad 'steps' args. Usage: steps ")); return; } int motorNum = args.substring(0, secondSpace).toInt(); String remainingArgs = args.substring(secondSpace + 1); int thirdSpace = remainingArgs.indexOf(' '); if (thirdSpace == -1) { Serial.println(F("ERROR: Bad 'steps' args. Usage: steps ")); return; } long count = remainingArgs.substring(0, thirdSpace).toInt(); uint32_t period = remainingArgs.substring(thirdSpace + 1).toInt(); if (motorNum != 1 && motorNum != 2) { Serial.println(F("ERROR: Invalid motor for 'steps'. Use 1 or 2.")); return; } if (count <= 0) { Serial.println(F("ERROR: Step count must be positive for 'steps' command. Direction is set by 'dir'.")); return; } if (motorRunning1 || motorRunning2 || currentPanelMoveState != IDLE) { Serial.println(F("ERROR: Cannot start 'steps' while a motor is running or panel sequence active. Use 'stop' or wait.")); return; } commandMotorSteps(motorNum, count, period); } else if (commandWord.equalsIgnoreCase("speed")) { int secondSpace = args.indexOf(' '); if (secondSpace == -1) { Serial.println(F("ERROR: Bad 'speed' args. Usage: speed ")); return; } int motorNum = args.substring(0, secondSpace).toInt(); uint32_t period = args.substring(secondSpace + 1).toInt(); if (period < (STEP_PULSE_WIDTH_US * 2)) { Serial.printf("ERROR: Motor %d period %lu us is too short. Must be >= %duS.\n", motorNum, period, (STEP_PULSE_WIDTH_US * 2)); return; } if (motorNum != 1 && motorNum != 2) { Serial.println(F("ERROR: Invalid motor for 'speed'. Use 1 or 2.")); return; } if (motorRunning1 || motorRunning2 || currentPanelMoveState != IDLE) { Serial.println(F("ERROR: Cannot start 'speed' while a motor is running or panel sequence active. Use 'stop' or wait.")); return; } uint8_t enPinToUse = (motorNum == 1) ? EN_PIN_1 : EN_PIN_2; digitalWrite(enPinToUse, LOW); // Enable motor for continuous run if (motorNum == 1) { if (!M1_isHomed) Serial.println(F("Warning: M1 not homed. Continuous run will not respect software limits until homed.")); bool isMovingDown1 = digitalRead(DIR_PIN_1) == getDirPinStateForPhysicalDirection(1, true); if (M1_isHomed && ((isMovingDown1 && M1_currentPositionSteps <= 0) || (!isMovingDown1 && M1_currentPositionSteps >= round(M1_MAX_TRAVEL_MM * M1_steps_per_mm)))) { Serial.printf("M1: Already at %s limit. Cannot start continuous run.\n", isMovingDown1 ? "lower" : "upper"); return; } currentStepPeriodUs1 = period; continuousRun1 = true; motorRunning1 = true; Serial.printf("M1: Continuous run started, period %lu us (%.1f Hz).\n", period, 1e6/(double)period); } else { if (!M2_isHomed) Serial.println(F("Warning: M2 not homed. Continuous run will not respect software limits until homed.")); bool isMovingDown2 = digitalRead(DIR_PIN_2) == getDirPinStateForPhysicalDirection(2, true); if (M2_isHomed && ((isMovingDown2 && M2_currentPositionSteps <= 0) || (!isMovingDown2 && M2_currentPositionSteps >= round(M2_MAX_TRAVEL_MM * M2_steps_per_mm)))) { Serial.printf("M2: Already at %s limit. Cannot start continuous run.\n", isMovingDown2 ? "lower" : "upper"); return; } currentStepPeriodUs2 = period; continuousRun2 = true; motorRunning2 = true; Serial.printf("M2: Continuous run started, period %lu us (%.1f Hz).\n", period, 1e6/(double)period); } } else if (commandWord.equalsIgnoreCase("dir")) { if (motorRunning1 || motorRunning2 || currentPanelMoveState != IDLE) { Serial.println(F("ERROR: Cannot change direction while motors running or panel sequence active. Use 'stop' or wait.")); return; } int secondSpace = args.indexOf(' '); if (secondSpace == -1) { Serial.println(F("ERROR: Bad 'dir' args. Usage: dir <0|1>")); return; } int motorNum = args.substring(0, secondSpace).toInt(); int dirCmdState = args.substring(secondSpace + 1).toInt(); if (dirCmdState != 0 && dirCmdState != 1) { Serial.println(F("ERROR: Invalid direction cmd. Use 0 (UP) or 1 (DOWN).")); return; } if (motorNum != 1 && motorNum != 2) { Serial.println(F("ERROR: Invalid motor for 'dir'. Use 1 or 2.")); return; } bool physicalDown = (dirCmdState == 1); uint8_t targetDirPinState = getDirPinStateForPhysicalDirection(motorNum, physicalDown); uint8_t actualDirPin = (motorNum == 1) ? DIR_PIN_1 : DIR_PIN_2; digitalWrite(actualDirPin, targetDirPinState); Serial.printf("Motor %d: Physical direction set to %s. (DIR pin set to: %s)\n", motorNum, physicalDown ? "DOWN" : "UP", targetDirPinState == LOW ? "LOW" : "HIGH"); } else if (commandWord.equalsIgnoreCase("stop")) { // Stops motion, motors remain enabled if (args.equalsIgnoreCase("all")) { motorRunning1=false; continuousRun1=false; stepsToRun1=0; motorRunning2=false; continuousRun2=false; stepsToRun2=0; currentPanelMoveState = IDLE; Serial.println(F("All motors stopped. Motors remain enabled for holding. Panel sequence halted.")); } else { int motorNum = args.toInt(); if (motorNum == 1) { motorRunning1=false; continuousRun1=false; stepsToRun1=0; if(currentPanelMoveState == M1_MOVING) currentPanelMoveState = IDLE; Serial.println(F("M1 stopped. Motor remains enabled for holding.")); } else if (motorNum == 2) { motorRunning2=false; continuousRun2=false; stepsToRun2=0; if(currentPanelMoveState == M2_MOVING) currentPanelMoveState = IDLE; Serial.println(F("M2 stopped. Motor remains enabled for holding.")); } else { Serial.println(F("ERROR: Invalid target for 'stop'. Use <1|2|all>.")); } } } else if (commandWord.equalsIgnoreCase("release")) { // NEW: Disables motors if (args.equalsIgnoreCase("all")) { if (motorRunning1 || motorRunning2 || currentPanelMoveState != IDLE) { Serial.println(F("WARNING: Releasing motors while active. Stopping motion first.")); } motorRunning1=false; continuousRun1=false; stepsToRun1=0; digitalWrite(EN_PIN_1, HIGH); motorRunning2=false; continuousRun2=false; stepsToRun2=0; digitalWrite(EN_PIN_2, HIGH); currentPanelMoveState = IDLE; Serial.println(F("All motors released (disabled). Holding torque removed.")); } else { int motorNum = args.toInt(); if (motorNum == 1) { if (motorRunning1 || (currentPanelMoveState == M1_MOVING)) { Serial.println(F("WARNING: Releasing M1 while active. Stopping motion first.")); } motorRunning1=false; continuousRun1=false; stepsToRun1=0; digitalWrite(EN_PIN_1, HIGH); if(currentPanelMoveState == M1_MOVING) currentPanelMoveState = IDLE; Serial.println(F("M1 released (disabled). Holding torque removed.")); } else if (motorNum == 2) { if (motorRunning2 || (currentPanelMoveState == M2_MOVING)) { Serial.println(F("WARNING: Releasing M2 while active. Stopping motion first.")); } motorRunning2=false; continuousRun2=false; stepsToRun2=0; digitalWrite(EN_PIN_2, HIGH); if(currentPanelMoveState == M2_MOVING) currentPanelMoveState = IDLE; Serial.println(F("M2 released (disabled). Holding torque removed.")); } else { Serial.println(F("ERROR: Invalid target for 'release'. Use <1|2|all>.")); } } } else if (commandWord.equalsIgnoreCase("home")) { if (motorRunning1 || motorRunning2 || currentPanelMoveState != IDLE) { Serial.println(F("ERROR: Cannot home while motors running or panel sequence active. Use 'stop' or wait.")); return; } if (args.equalsIgnoreCase("1")) { homeMotor1(); } else if (args.equalsIgnoreCase("2")) { homeMotor2(); } else if (args.equalsIgnoreCase("all")) { homeAll(); } else { Serial.println(F("ERROR: Invalid 'home' argument. Usage: home <1|2|all>")); } } else if (commandWord.equalsIgnoreCase("get_pos_steps")) { int motorNum = args.toInt(); if (motorNum == 1) { if (M1_isHomed) Serial.printf("M1 Position: %ld steps from home.\n", M1_currentPositionSteps); else Serial.println(F("M1: Not homed.")); } else if (motorNum == 2) { if (M2_isHomed) Serial.printf("M2 Position: %ld steps from home.\n", M2_currentPositionSteps); else Serial.println(F("M2: Not homed.")); } else { Serial.println(F("ERROR: Usage: get_pos_steps <1|2>")); } } else if (commandWord.equalsIgnoreCase("get_pos_mm")) { if (args.equalsIgnoreCase("1")) { if (M1_isHomed) Serial.printf("M1 Position: %.3f mm (Max: %.2f mm).\n", (float)M1_currentPositionSteps / M1_steps_per_mm, M1_MAX_TRAVEL_MM); else Serial.println(F("M1: Not homed.")); } else if (args.equalsIgnoreCase("2")) { if (M2_isHomed) Serial.printf("M2 Position: %.3f mm (Max: %.2f mm).\n", (float)M2_currentPositionSteps / M2_steps_per_mm, M2_MAX_TRAVEL_MM); else Serial.println(F("M2: Not homed.")); } else if (args.equalsIgnoreCase("all")) { if (M1_isHomed) Serial.printf("M1: %.3f mm (%ld steps). Max: %.2f mm.\n", (float)M1_currentPositionSteps / M1_steps_per_mm, M1_currentPositionSteps, M1_MAX_TRAVEL_MM); else Serial.println(F("M1: Not homed.")); if (M2_isHomed) Serial.printf("M2: %.3f mm (%ld steps). Max: %.2f mm.\n", (float)M2_currentPositionSteps / M2_steps_per_mm, M2_currentPositionSteps, M2_MAX_TRAVEL_MM); else Serial.println(F("M2: Not homed.")); } else { Serial.println(F("ERROR: Usage: get_pos_mm <1|2|all>")); } } else if (commandWord.equalsIgnoreCase("get_panel_pos_mm")) { if (!M1_isHomed || !M2_isHomed) { Serial.println(F("Panel: Both motors must be homed to calculate panel position.")); if (!M1_isHomed) Serial.println(F(" M1: Not homed.")); if (!M2_isHomed) Serial.println(F(" M2: Not homed.")); return; } float m1_pos_mm = (float)M1_currentPositionSteps / M1_steps_per_mm; float m2_pos_mm = (float)M2_currentPositionSteps / M2_steps_per_mm; Serial.printf("Panel Absolute Position: %.3f mm (M1: %.3f mm + M2: %.3f mm)\n", (m1_pos_mm + m2_pos_mm), m1_pos_mm, m2_pos_mm); } else if (commandWord.equalsIgnoreCase("move_panel_to")) { if (currentPanelMoveState != IDLE) { Serial.println(F("Panel ERROR: Another panel move sequence is already in progress. Wait or use 'stop'.")); return; } if (motorRunning1 || motorRunning2) { Serial.println(F("Panel ERROR: Cannot move panel while motors are active individually. Use 'stop' first.")); return; } if (!M1_isHomed || !M2_isHomed) { Serial.println(F("Panel ERROR: Both M1 and M2 must be homed to use move_panel_to.")); return; } float target_abs_panel_mm = args.toFloat(); Serial.printf("Panel: Received command to move to absolute position: %.3f mm.\n", target_abs_panel_mm); float ideal_M2_target_mm = M2_MAX_TRAVEL_MM / 2.0f; float tentative_M1_target_mm = target_abs_panel_mm - ideal_M2_target_mm; float final_M1_target_mm = constrain(tentative_M1_target_mm, 0.0f, M1_MAX_TRAVEL_MM); float final_M2_target_mm = target_abs_panel_mm - final_M1_target_mm; final_M2_target_mm = constrain(final_M2_target_mm, 0.0f, M2_MAX_TRAVEL_MM); final_M1_target_mm = constrain(target_abs_panel_mm - final_M2_target_mm, 0.0f, M1_MAX_TRAVEL_MM); float achieved_panel_pos_mm = final_M1_target_mm + final_M2_target_mm; if (fabs(achieved_panel_pos_mm - target_abs_panel_mm) > 0.01) { Serial.printf("Panel Warning: Target %.3f mm unachievable. Moving to closest: %.3f mm (M1: %.3f, M2: %.3f).\n", target_abs_panel_mm, achieved_panel_pos_mm, final_M1_target_mm, final_M2_target_mm); } else { Serial.printf("Panel: Calculated motor targets - M1: %.3f mm, M2: %.3f mm.\n", final_M1_target_mm, final_M2_target_mm); } long target_M1_abs_steps = round(final_M1_target_mm * M1_steps_per_mm); long target_M2_abs_steps = round(final_M2_target_mm * M2_steps_per_mm); long delta_M1_steps = target_M1_abs_steps - M1_currentPositionSteps; long delta_M2_steps = target_M2_abs_steps - M2_currentPositionSteps; Serial.printf("Panel: M1 needs %ld steps. M2 needs %ld steps.\n", delta_M1_steps, delta_M2_steps); panel_move_M2_target_steps_remaining = delta_M2_steps; panel_move_M2_direction_physical_down = (delta_M2_steps < 0); panel_move_M2_period_us = currentStepPeriodUs2; if (delta_M1_steps != 0) { bool m1_move_physically_down = delta_M1_steps < 0; digitalWrite(DIR_PIN_1, getDirPinStateForPhysicalDirection(1, m1_move_physically_down)); Serial.printf("Panel: Initiating M1 move %s by %ld steps.\n", m1_move_physically_down ? "DOWN" : "UP", labs(delta_M1_steps)); commandMotorSteps(1, labs(delta_M1_steps), currentStepPeriodUs1); currentPanelMoveState = M1_MOVING; } else if (delta_M2_steps != 0) { Serial.println(F("Panel: M1 already at target. Initiating M2 move.")); digitalWrite(DIR_PIN_2, getDirPinStateForPhysicalDirection(2, panel_move_M2_direction_physical_down)); commandMotorSteps(2, labs(panel_move_M2_target_steps_remaining), panel_move_M2_period_us); currentPanelMoveState = M2_MOVING; } else { Serial.println(F("Panel: Already at target position. No movement needed.")); currentPanelMoveState = IDLE; reportFinalPanelPosition(); } } else if (commandWord.equalsIgnoreCase("set_steps_per_mm")) { int secondSpace = args.indexOf(' '); if (secondSpace == -1) { Serial.println(F("ERROR: Bad 'set_steps_per_mm' args. Usage: ")); return; } int motorNum = args.substring(0, secondSpace).toInt(); float val = args.substring(secondSpace + 1).toFloat(); if (val <= 0) { Serial.println(F("ERROR: Steps per mm value must be positive.")); return; } if (motorNum == 1) { M1_steps_per_mm = val; Serial.printf("M1 steps_per_mm set to: %.3f\n", M1_steps_per_mm); M1_isHomed = false; Serial.println(F("M1 marked NOT HOMED. Re-home.")); } else if (motorNum == 2) { M2_steps_per_mm = val; Serial.printf("M2 steps_per_mm set to: %.3f\n", M2_steps_per_mm); M2_isHomed = false; Serial.println(F("M2 marked NOT HOMED. Re-home.")); } else { Serial.println(F("ERROR: Invalid motor for 'set_steps_per_mm'. Use 1 or 2.")); } } else if (commandWord.equalsIgnoreCase("set_max_travel")) { int secondSpace = args.indexOf(' '); if (secondSpace == -1) { Serial.println(F("ERROR: Bad 'set_max_travel' args. Usage: ")); return; } int motorNum = args.substring(0, secondSpace).toInt(); float val = args.substring(secondSpace + 1).toFloat(); if (val <= 0) { Serial.println(F("ERROR: Max travel value must be positive (mm).")); return; } if (motorNum == 1) { M1_MAX_TRAVEL_MM = val; Serial.printf("M1_MAX_TRAVEL_MM set to: %.2f mm\n", M1_MAX_TRAVEL_MM); if (M1_isHomed && ((float)M1_currentPositionSteps / M1_steps_per_mm) > M1_MAX_TRAVEL_MM) Serial.println(F("Warning: M1 current position exceeds new max travel.")); } else if (motorNum == 2) { M2_MAX_TRAVEL_MM = val; Serial.printf("M2_MAX_TRAVEL_MM set to: %.2f mm\n", M2_MAX_TRAVEL_MM); if (M2_isHomed && ((float)M2_currentPositionSteps / M2_steps_per_mm) > M2_MAX_TRAVEL_MM) Serial.println(F("Warning: M2 current position exceeds new max travel.")); } else { Serial.println(F("ERROR: Invalid motor for 'set_max_travel'. Use 1 or 2.")); } } else { Serial.println(F("Unknown command.")); } }