diff --git a/Arduino-Satellite-Dish/src/main.cpp b/Arduino-Satellite-Dish/src/main.cpp index 0666176..d658b4d 100644 --- a/Arduino-Satellite-Dish/src/main.cpp +++ b/Arduino-Satellite-Dish/src/main.cpp @@ -1,18 +1,18 @@ /** - * @file IR_Tracker_Optimized_ProdReady.ino - * @brief Optimized Infrared Tracker using AccelStepper for Arduino Uno. - * @version 1.0 (Production Ready Candidate) - * @date 2025-04-28 + * @file Photoresistor_Tracker_Basic.ino + * @brief Basic Light Tracker using Photoresistors and AccelStepper for Arduino Uno. + * @version 0.5 (Basic Adaptation) + * @date 2025-04-28 (Original base date) * * @details This firmware controls a 2-axis stepper motor system (like a pan-tilt - * mechanism) to track an infrared (IR) source using five IR sensors (L, R, U, D, C). - * It includes homing, searching (sweep, local), tracking (P-control), and error handling. - * Optimized for performance on Arduino Uno using fixed-point math and direct port I/O. + * mechanism) to track a light source using five photoresistors (L, R, U, D, C). + * It includes homing, searching (sweep, basic local), tracking (P-control), and error handling. + * Simplified from an IR tracker, focusing on basic functionality. * * @warning CRITICAL: Verify all pin assignments in the configuration section match - * your hardware EXACTLY before uploading. Failure WILL cause malfunction. - * @warning CRITICAL: Pins A4 (18) and A5 (19) conflict with I2C. If using ANY - * I2C devices, you MUST change the limit switch pin assignments. + * your hardware EXACTLY before uploading. Limit switch pins have changed! + * @warning CRITICAL: Photoresistor pins A0-A4 are used. If hardware I2C (which often uses A4/A5) + * is required, photoresistor pin assignments MUST change, or use software I2C. */ #include @@ -24,7 +24,7 @@ // === SYSTEM CONFIGURATION & TUNING === // ========================================================================== // --- System Version --- -#define SYSTEM_VERSION "1.0" +#define SYSTEM_VERSION "0.5-Photoresistor-Basic" // --- Build Configuration --- // Comment out the next line for a production build (disables most Serial output for speed) @@ -43,112 +43,89 @@ // --- Pin Definitions --- // *** VERIFY ALL PINS AGAINST YOUR HARDWARE *** // Stepper Motors (using AccelStepper HALF4WIRE mode) -// Sequence: P1, P3, P2, P4 (as required by AccelStepper library) -// Recommend connecting ULN2003 IN1,IN2,IN3,IN4 to Arduino pins in sequence: P1,P2,P3,P4 -const uint8_t stepperX_pin1 = 8; // e.g., ULN2003 IN1 -> Arduino D8 (PB0) -const uint8_t stepperX_pin3 = 9; // e.g., ULN2003 IN3 -> Arduino D9 (PB1) -const uint8_t stepperX_pin2 = 10; // e.g., ULN2003 IN2 -> Arduino D10 (PB2) -const uint8_t stepperX_pin4 = 11; // e.g., ULN2003 IN4 -> Arduino D11 (PB3) +const uint8_t stepperX_pin1 = 8; +const uint8_t stepperX_pin3 = 9; +const uint8_t stepperX_pin2 = 10; +const uint8_t stepperX_pin4 = 11; -const uint8_t stepperY_pin1 = 4; // e.g., ULN2003 IN1 -> Arduino D4 (PD4) -const uint8_t stepperY_pin3 = 5; // e.g., ULN2003 IN3 -> Arduino D5 (PD5) -const uint8_t stepperY_pin2 = 6; // e.g., ULN2003 IN2 -> Arduino D6 (PD6) -const uint8_t stepperY_pin4 = 7; // e.g., ULN2003 IN4 -> Arduino D7 (PD7) +const uint8_t stepperY_pin1 = 4; +const uint8_t stepperY_pin3 = 5; +const uint8_t stepperY_pin2 = 6; +const uint8_t stepperY_pin4 = 7; -// IR Sensors (Active LOW assumed, e.g., TSOP receivers) -// Grouped by AVR Port for optimized reading where possible -const uint8_t sensorPinL = 2; // PD2 (INT0) -const uint8_t sensorPinR = 3; // PD3 (INT1) -const uint8_t sensorPinU = 12; // PB4 -const uint8_t sensorPinD = 13; // PB5 (SCK - also SPI) -const uint8_t sensorPinC = A0; // PC0 (ADC0) - Used as digital input +// Photoresistors (Analog Inputs) +// Assumes higher analogRead() value = more light. +const uint8_t photoresistorPinL = A0; // Left +const uint8_t photoresistorPinR = A1; // Right +const uint8_t photoresistorPinU = A2; // Up +const uint8_t photoresistorPinD = A3; // Down +const uint8_t photoresistorPinC = A4; // Center -// Limit Switches (Active LOW assumed) -// !!! WARNING: A4/A5 conflict with I2C. CHANGE IF USING I2C !!! -const uint8_t limitSwitchPinX = A4; // PC4 (SDA - I2C Data) -const uint8_t limitSwitchPinY = A5; // PC5 (SCL - I2C Clock) +// Limit Switches (Active LOW assumed) - MOVED TO DIGITAL PINS +const uint8_t limitSwitchPinX = 2; // PD2 (INT0) - Connect X-axis limit switch here +const uint8_t limitSwitchPinY = 3; // PD3 (INT1) - Connect Y-axis limit switch here -// Status LEDs (Indicate system state) -const uint8_t ledPinSearching = A1; // PC1 (ADC1) -const uint8_t ledPinTracking = A2; // PC2 (ADC2) -const uint8_t ledPinLost = A3; // PC3 (ADC3) +// --- Photoresistor & Tracking Parameters (CRITICAL TUNING REQUIRED) --- +const int PHOTO_LIGHT_THRESHOLD_GENERAL = 300; // Min analog reading to consider any sensor "active" (0-1023) +const int PHOTO_LIGHT_THRESHOLD_CENTER = 400; // Min analog reading for Center sensor to influence logic (if used beyond general detection) +// Defines how much raw analog difference translates into an "error unit" for Kp. +// Larger value = less sensitive to small differences. (e.g. (L_val - R_val) / THIS_FACTOR = error_int) +// Example: If L=600, R=400, diff=200. If factor is 50, error_int = 4. If factor is 100, error_int = 2. +const int ANALOG_DIFF_SCALING_FACTOR = 50; // Tune this based on your LDR sensitivity and light source! // --- Motor & Movement Parameters --- -const uint16_t stepsPerRevolution = 2038; // Steps for a full 360 rotation (for 28BYJ-48 in half-step) --- POSSIBLY ~4076 - -// Speeds & Acceleration (REQUIRES EMPIRICAL TUNING FOR YOUR HARDWARE) -// Start low and increase gradually. High values risk stalls/skipped steps. -const float homingSpeed = 200.0; // Speed during homing sequence (steps/sec) -const float homingAcceleration = 200.0; // Acceleration during homing (steps/sec^2) -const float sweepSpeedDefault = 400.0; // Speed during full sweep search (steps/sec) -const float sweepAccelerationDefault = 300.0; // Acceleration during full sweep (steps/sec^2) -const float trackSpeedDefault = 400.0; // Base speed during tracking/centering (steps/sec) -const float trackAccelerationDefault = 300.0; // Acceleration during tracking (steps/sec^2) -const float localSearchSpeed = 300.0; // Speed during local search patterns (steps/sec) -const float localSearchAccel = 200.0; // Acceleration during local search (steps/sec^2) +const uint16_t stepsPerRevolution = 2038; +const float homingSpeed = 200.0; +const float homingAcceleration = 200.0; +const float sweepSpeedDefault = 300.0; // Reduced default for initial basic setup +const float sweepAccelerationDefault = 200.0; +const float trackSpeedDefault = 300.0; +const float trackAccelerationDefault = 200.0; +const float localSearchSpeed = 250.0; +const float localSearchAccel = 150.0; // Search Range & Homing -const long searchRangeX_deg = 180; // Max X-axis travel during sweep (degrees) -const long searchRangeY_deg = 90; // Max Y-axis travel during sweep (degrees) -const int postHomingMoveOffSwitchSteps = 30; // Steps to move away from limit switch after homing -const uint8_t HOMING_OVERSHOOT_MULTIPLIER = 20; // Factor for initial homing move distance (ensure switch hit) +const long searchRangeX_deg = 180; +const long searchRangeY_deg = 90; +const int postHomingMoveOffSwitchSteps = 30; +const uint8_t HOMING_OVERSHOOT_MULTIPLIER = 20; // --- Tracking Control (Proportional Controller - REQUIRES CRITICAL TUNING) --- -// Tune Kp_float values first. Fixed-point values are derived automatically. -const float Kp_X_float = 6.0; // Proportional gain for X-axis. Higher = stronger/faster correction. Too high = oscillation. -const float Kp_Y_float = 6.0; // Proportional gain for Y-axis. -const int trackingDeadband_steps = 5; // Ignore movement commands smaller than this (steps) to prevent jitter when centered. +const float Kp_X_float = 10.0; // Proportional gain for X-axis. Start low (e.g., 5-20), tune carefully! +const float Kp_Y_float = 10.0; // Proportional gain for Y-axis. +const int trackingDeadband_steps = 8; // Ignore movement commands smaller than this (steps) -// Fixed Point Math Configuration (Internal - Do Not Change Without Understanding) -const int FIXED_POINT_SHIFT = 8; // Scale factor = 1 << FIXED_POINT_SHIFT (e.g., 8 -> 256) -const long FIXED_POINT_SCALE = 1L << FIXED_POINT_SHIFT; -const long Kp_X_scaled = (long)(Kp_X_float * FIXED_POINT_SCALE); // Auto-scaled Kp for internal fixed-point math -const long Kp_Y_scaled = (long)(Kp_Y_float * FIXED_POINT_SCALE); // Auto-scaled Kp for internal fixed-point math - -// Internal representation of tracking error magnitude based on sensor states. -// These determine the relative strength of the correction applied. -const int8_t ERROR_INT_FAR_NEG = -2; // Target far off in negative direction (e.g., Right only / Down only) -const int8_t ERROR_INT_NEAR_NEG = -1; // Target near center in negative direction (e.g., Center + Right / Center + Down) -const int8_t ERROR_INT_CENTERED = 0; // Target centered or balanced state -const int8_t ERROR_INT_NEAR_POS = 1; // Target near center in positive direction (e.g., Center + Left / Center + Up) -const int8_t ERROR_INT_FAR_POS = 2; // Target far off in positive direction (e.g., Left only / Up only) - -// --- Local Search Strategy Parameters (Tune based on sensor beam/spacing and Kp performance) --- -const int localSearchProbeSteps = 30; // Initial steps to move in the last known direction of the target. -const int localSearchSweepSteps = 100; // Steps for perpendicular sweeps if probe fails. -const int localSearchBoxStepSize = 30; // Step size for the fallback expanding box pattern. -const uint8_t localSearchMaxCycles = 3; // Max cycles (layers) of the fallback box pattern before giving up. -const uint8_t LOCAL_SEARCH_BOX_STEPS_PER_LAYER = 8; // Internal constant for box pattern logic. +// --- Local Search Strategy Parameters --- +const int localSearchProbeSteps = 25; +const int localSearchSweepSteps = 80; +const int localSearchBoxStepSize = 25; +const uint8_t localSearchMaxCycles = 2; // Reduced for basic +const uint8_t LOCAL_SEARCH_BOX_STEPS_PER_LAYER = 8; // --- Timing Parameters --- -const unsigned int debounceDelayMs = 50; // Sensor debounce time (ms) -const unsigned int limitSwitchDebounceMs = 25; // Limit switch debounce time (ms) -const unsigned int signalLostTimeoutMs = 500; // Time (ms) without signal in TRACKING before entering LOCAL_SEARCH -const unsigned int searchFailDelayMs = 3000; // Delay (ms) in SEARCH_FAILED state before retrying full sweep -const unsigned long stateTimeoutMs = 25000; // Generic safety timeout for states like SWEEP, CENTERING (ms) -const unsigned long homingTimeoutMs = 45000; // Safety timeout for HOMING states (ms) -const unsigned long localSearchTimeoutMs = 15000; // Overall safety timeout for the LOCAL_SEARCH state (ms) -const unsigned int telemetryIntervalMs = 750; // Interval for printing telemetry data (ms) (if ENABLE_TELEMETRY) -const unsigned int postHomingPauseMs = 500; // Pause (ms) after successful homing before starting search +const unsigned int debounceDelayMs = 50; // General sensor debounce (less critical for LDRs but kept for limit switches) +const unsigned int limitSwitchDebounceMs = 25; +const unsigned int signalLostTimeoutMs = 700; +const unsigned int searchFailDelayMs = 3000; +const unsigned long stateTimeoutMs = 25000; +const unsigned long homingTimeoutMs = 45000; +const unsigned long localSearchTimeoutMs = 15000; +const unsigned int telemetryIntervalMs = 750; +const unsigned int postHomingPauseMs = 500; -// --- Stall Detection Parameters (Safety Net - Tune Carefully) --- -// Frequent stalls indicate speeds/accel too high or mechanical issues. -const unsigned int stallCheckIntervalMs = 100; // How often to check for stalls (ms) -const unsigned int stallTimeoutThresholdMs = 1800; // Motor must be 'stuck' for this duration (ms) to trigger stall error -const uint8_t stallPositionTolerance = 2; // Minimum steps motor must move between checks to be considered 'not stuck' +// --- Stall Detection Parameters --- +const unsigned int stallCheckIntervalMs = 100; +const unsigned int stallTimeoutThresholdMs = 1800; +const uint8_t stallPositionTolerance = 2; // ========================================================================== // === INTERNAL DEFINITIONS & GLOBALS === // ========================================================================== - -// --- Calculated Constants --- -// Using float division for potentially better precision before casting to long const long searchMaxX_steps = (long)(((float)searchRangeX_deg * stepsPerRevolution) / 360.0f); const long searchMaxY_steps = (long)(((float)searchRangeY_deg * stepsPerRevolution) / 360.0f); -const long searchMinX_steps = 0; // Homing sets origin -const long searchMinY_steps = 0; // Homing sets origin +const long searchMinX_steps = 0; +const long searchMinY_steps = 0; -// --- Sensor Indices --- #define SENSOR_L 0 #define SENSOR_R 1 #define SENSOR_U 2 @@ -156,26 +133,16 @@ const long searchMinY_steps = 0; // Homing sets origin #define SENSOR_C 4 const int NUM_SENSORS = 5; -// --- Precomputed Bit Masks for Direct Port I/O --- -// Input Pins (Read from PINx registers) -const uint8_t sensorMaskL = (1 << PIND2); // PORTD -const uint8_t sensorMaskR = (1 << PIND3); // PORTD -const uint8_t sensorMaskU = (1 << PINB4); // PORTB -const uint8_t sensorMaskD = (1 << PINB5); // PORTB -const uint8_t sensorMaskC = (1 << PINC0); // PORTC -const uint8_t limitSwitchMaskX = (1 << PINC4); // PORTC -const uint8_t limitSwitchMaskY = (1 << PINC5); // PORTC +// Array for photoresistor pins for easier iteration +const uint8_t photoresistorPins[NUM_SENSORS] = { + photoresistorPinL, photoresistorPinR, photoresistorPinU, photoresistorPinD, photoresistorPinC}; -// Output Pins (Write to PORTx registers, configure with DDRx) -// Assuming LEDs are on PORTC as defined above -const uint8_t ledMaskSearching = (1 << PORTC1); -const uint8_t ledMaskTracking = (1 << PORTC2); -const uint8_t ledMaskLost = (1 << PORTC3); -const uint8_t ledMaskAll = ledMaskSearching | ledMaskTracking | ledMaskLost; +// Precomputed Bit Masks for Limit Switches (Direct Port I/O on PORTD) +const uint8_t limitSwitchMaskX = (1 << PIND2); // For limitSwitchPinX on D2 +const uint8_t limitSwitchMaskY = (1 << PIND3); // For limitSwitchPinY on D3 -// --- State Machine --- enum class State : uint8_t -{ // Use enum class for stronger typing +{ INITIALIZING, CHECK_PIN_CONFLICTS, HOMING_X, @@ -189,14 +156,14 @@ enum class State : uint8_t TRACKING, LOCAL_SEARCH, SEARCH_FAILED, - ERROR // Fatal halt + ERROR }; State currentState = State::INITIALIZING; -// State names in PROGMEM to save RAM const char state0[] PROGMEM = "INITIALIZING"; const char state1[] PROGMEM = "CHECK_PIN_CONFLICTS"; const char state2[] PROGMEM = "HOMING_X"; +// ... (rest of state names are the same as original, ensure array matches enum) const char state3[] PROGMEM = "HOMING_Y"; const char state4[] PROGMEM = "POST_HOMING_MOVE_OFF_X"; const char state5[] PROGMEM = "POST_HOMING_MOVE_OFF_Y"; @@ -209,95 +176,73 @@ const char state11[] PROGMEM = "LOCAL_SEARCH"; const char state12[] PROGMEM = "SEARCH_FAILED"; const char state13[] PROGMEM = "ERROR"; -const char *const stateNames[] PROGMEM = { // Must match enum order +const char *const stateNames[] PROGMEM = { state0, state1, state2, state3, state4, state5, state6, state7, state8, state9, state10, state11, state12, state13}; +char stateNameBuffer[25]; -// Buffer for reading state names from PROGMEM -char stateNameBuffer[25]; // Sufficient size for longest state name + null terminator - -// --- Stepper Instances --- AccelStepper stepperX(AccelStepper::HALF4WIRE, stepperX_pin1, stepperX_pin3, stepperX_pin2, stepperX_pin4); AccelStepper stepperY(AccelStepper::HALF4WIRE, stepperY_pin1, stepperY_pin3, stepperY_pin2, stepperY_pin4); -// --- Global State Variables --- -// Sensor & Limit Switch States (debounced) -bool sensorActive[NUM_SENSORS] = {false}; +// Sensor readings and states +int sensorReadings[NUM_SENSORS] = {0}; // Stores raw analogRead values +bool sensorActive[NUM_SENSORS] = {false}; // True if sensorReading > PHOTO_LIGHT_THRESHOLD_GENERAL bool limitSwitchActiveX = false; bool limitSwitchActiveY = false; -// Internal Debounce Tracking -uint8_t lastRawPinStatesB = 0; // Raw PORTB state for inputs -uint8_t lastRawPinStatesC = 0; // Raw PORTC state for inputs -uint8_t lastRawPinStatesD = 0; // Raw PORTD state for inputs -unsigned long lastDebounceTimeSensors = 0; +// Internal Debounce Tracking for Limit Switches +uint8_t lastRawPinStatesD = 0; // Raw PORTD state for limit switch inputs unsigned long lastDebounceTimeLimits = 0; -// Timing & State Tracking -unsigned long lastSignalTime = 0; // Time the IR signal was last detected -unsigned long stateEntryTime = 0; // Time the current state was entered +unsigned long lastSignalTime = 0; +unsigned long stateEntryTime = 0; unsigned long lastTelemetryTime = 0; -// Search State Variables -int8_t sweepDirectionX = 1; // 1 for positive, -1 for negative direction +int8_t sweepDirectionX = 1; int8_t sweepDirectionY = 1; -uint8_t sweepXCyclesCompleted = 0; // Counts legs (0, 1) of a full sweep cycle +uint8_t sweepXCyclesCompleted = 0; uint8_t sweepYCyclesCompleted = 0; -// Tracking & Local Search State -long lastKnownX = 0; // Last X position where signal was good -long lastKnownY = 0; // Last Y position where signal was good -int8_t lastErrorX_int = 0; // Last calculated integer error before signal loss or move -int8_t lastErrorY_int = 0; -uint8_t currentLocalSearchPhase = 0; // 0=Probe, 1=Sweep, 2=Box Pattern -uint8_t currentLocalSearchCycle = 0; // Cycle/step counter within local search phase +long lastKnownX = 0; +long lastKnownY = 0; +int lastErrorX_int = 0; // Stores scaled analog difference +int lastErrorY_int = 0; // Stores scaled analog difference +uint8_t currentLocalSearchPhase = 0; +uint8_t currentLocalSearchCycle = 0; -// Stall Detection State long lastKnownStepperXPosForStall = 0; long lastKnownStepperYPosForStall = 0; -unsigned long stallStartTimeX = 0; // Time potential stall started +unsigned long stallStartTimeX = 0; unsigned long stallStartTimeY = 0; unsigned long lastStallCheckTime = 0; -// Error Handling State -bool recoveryAttempted = false; // Flag to prevent infinite recovery loops +bool recoveryAttempted = false; // ========================================================================== // === FUNCTION PROTOTYPES === // ========================================================================== -// Setup & Initialization bool setupPinsAndCheckConflicts(); void printStartupMessage(); -void printTuningGuide(); // Conditional print - -// Core Logic -void readInputsOptimized(); +void printTuningGuide(); +void readPhotoresistorInputs(); // Changed name void handleStateMachine(); void changeState(State newState); -const char *getStateName(State state); // Helper for PROGMEM names -inline bool isAnySensorActive(); -inline bool isCentered(); // Added missing prototype - -// State Handlers +const char *getStateName(State state); +inline bool isAnySensorGenerallyActive(); // Changed name +inline bool isCenterSensorGenerallyActive(); // Changed name void handleHomingStates(); -void handleTrackingLogicFixedPoint(); +void handleTrackingLogic(); // Changed name void handleLocalSearch(); void startSweepX(); void startSweepY(); - -// Safety & Monitoring -void updateStatusLEDsOptimized(); +// void updateStatusLEDsOptimized(); // REMOVED void checkStateTimeouts(); void checkMotorStall(); void handleSystemError(const __FlashStringHelper *reason); void indicateFatalError(const __FlashStringHelper *message); - -// Utilities inline unsigned long safeMillisSubtract(unsigned long current, unsigned long previous); inline long clamp(long val, long minVal, long maxVal); void setMotorSpeeds(float speedX, float accelX, float speedY, float accelY); - -// Telemetry (Conditional) void printTelemetry(); // ========================================================================== @@ -307,14 +252,10 @@ void setup() { #if defined(ENABLE_DEBUG_PRINTING) || defined(ENABLE_TELEMETRY) Serial.begin(115200); - // Allow time for Serial Monitor connection if debugging enabled - // Keep this delay for robustness during development/debugging delay(2000); #endif - printStartupMessage(); // Print version and config info - - // Transition to pin checking state + printStartupMessage(); changeState(State::CHECK_PIN_CONFLICTS); } @@ -323,39 +264,23 @@ void setup() // ========================================================================== void loop() { - unsigned long now = millis(); // Cache current time for this loop iteration + unsigned long now = millis(); - // --- Essential Operations --- - // 1. Process stepper motor movements (critical for non-blocking operation) - // AccelStepper requires run() to be called frequently. stepperX.run(); stepperY.run(); - // 2. Read inputs (sensors, limit switches) with debouncing - readInputsOptimized(); + readPhotoresistorInputs(); // Reads photoresistors and limit switches - // --- State-Dependent Logic --- - // 3. Update tracking variables (only if tracking/centering and signal present) - if ((currentState == State::TRACKING || currentState == State::CENTERING) && isAnySensorActive()) + if ((currentState == State::TRACKING || currentState == State::CENTERING) && isAnySensorGenerallyActive()) { - lastSignalTime = now; // Keep track of the last time signal was seen + lastSignalTime = now; } - // 4. Execute main state machine logic handleStateMachine(); - - // --- Monitoring & Safety --- - // 5. Update status LEDs to reflect current state - updateStatusLEDsOptimized(); - - // 6. Check for motor stalls (runs checks at configured interval) + // updateStatusLEDsOptimized(); // REMOVED checkMotorStall(); - - // 7. Check for state timeouts (safety net against deadlocks) checkStateTimeouts(); -// --- Telemetry (Conditional) --- -// 8. Print periodic status information if enabled #ifdef ENABLE_TELEMETRY if (currentState > State::CHECK_PIN_CONFLICTS && currentState != State::ERROR && safeMillisSubtract(now, lastTelemetryTime) >= telemetryIntervalMs) @@ -369,38 +294,30 @@ void loop() // ========================================================================== // === STATE MACHINE IMPLEMENTATION === // ========================================================================== - -/** - * @brief Main state machine router. Calls appropriate handlers based on `currentState`. - */ void handleStateMachine() { - // Delegate Homing and Post-Homing states first for efficiency if (currentState >= State::HOMING_X && currentState <= State::POST_HOMING_DELAY) { handleHomingStates(); - return; // Don't process further in this iteration + return; } - // Cache sensor state (already debounced) - bool anySensor = isAnySensorActive(); - unsigned long now = millis(); // Re-cache time if needed + bool anySensor = isAnySensorGenerallyActive(); + unsigned long now = millis(); switch (currentState) { case State::INITIALIZING: - // Should transition immediately in setup() changeState(State::CHECK_PIN_CONFLICTS); break; case State::CHECK_PIN_CONFLICTS: if (!setupPinsAndCheckConflicts()) { - // Error handled within the function (halts) while (1) - ; // Should not be reached + ; } - printTuningGuide(); // Print only if debug enabled + printTuningGuide(); #ifdef ENABLE_DEBUG_PRINTING Serial.println(F("Pin checks complete. Starting Homing sequence...")); #endif @@ -414,32 +331,32 @@ void handleStateMachine() #ifdef ENABLE_DEBUG_PRINTING Serial.println(F("Signal Found (X Sweep) -> Centering")); #endif - stepperX.stop(); // Stop the sweep smoothly + stepperX.stop(); recoveryAttempted = false; - sweepXCyclesCompleted = 0; // Reset for next search + sweepXCyclesCompleted = 0; changeState(State::CENTERING); } else if (stepperX.distanceToGo() == 0) - { // Move complete? + { sweepXCyclesCompleted++; if (sweepXCyclesCompleted >= 2) - { // Full out-and-back done + { #ifdef ENABLE_DEBUG_PRINTING Serial.println(F("Full X Sweep Cycle Complete (No Signal) -> Sweeping Y")); #endif sweepXCyclesCompleted = 0; - sweepDirectionY = 1; // Reset for next sweep type + sweepDirectionY = 1; changeState(State::SWEEP_Y); } else - { // First leg done, reverse + { #ifdef ENABLE_DEBUG_PRINTING Serial.print(F("X Sweep Leg Complete (")); Serial.print(sweepXCyclesCompleted); Serial.println(F("/2) -> Reversing X Sweep")); #endif sweepDirectionX *= -1; - startSweepX(); // Start the return move + startSweepX(); } } break; @@ -450,16 +367,16 @@ void handleStateMachine() #ifdef ENABLE_DEBUG_PRINTING Serial.println(F("Signal Found (Y Sweep) -> Centering")); #endif - stepperY.stop(); // Stop the sweep smoothly + stepperY.stop(); recoveryAttempted = false; - sweepYCyclesCompleted = 0; // Reset for next search + sweepYCyclesCompleted = 0; changeState(State::CENTERING); } else if (stepperY.distanceToGo() == 0) - { // Move complete? + { sweepYCyclesCompleted++; if (sweepYCyclesCompleted >= 2) - { // Full up-and-down done + { #ifdef ENABLE_DEBUG_PRINTING Serial.println(F("Full Y Sweep Cycle Complete (No Signal) -> Search FAILED")); #endif @@ -467,42 +384,38 @@ void handleStateMachine() changeState(State::SEARCH_FAILED); } else - { // First leg done, reverse + { #ifdef ENABLE_DEBUG_PRINTING Serial.print(F("Y Sweep Leg Complete (")); Serial.print(sweepYCyclesCompleted); Serial.println(F("/2) -> Reversing Y Sweep")); #endif sweepDirectionY *= -1; - startSweepY(); // Start the return move + startSweepY(); } } break; case State::CENTERING: - // Perform P-control logic to move towards center - handleTrackingLogicFixedPoint(); - - // Check if centering move is complete (both axes stopped at target) + handleTrackingLogic(); if (stepperX.distanceToGo() == 0 && stepperY.distanceToGo() == 0) { #ifdef ENABLE_DEBUG_PRINTING Serial.println(F("Centering Move Complete. Re-evaluating...")); #endif recoveryAttempted = false; - if (isAnySensorActive()) - { // Still see signal after stopping? + if (isAnySensorGenerallyActive()) + { #ifdef ENABLE_DEBUG_PRINTING Serial.println(F("Signal Active -> Tracking")); #endif changeState(State::TRACKING); } else - { // Lost signal during final centering move or right after stopping + { #ifdef ENABLE_DEBUG_PRINTING Serial.println(F("Signal LOST during/after Centering -> Local Search")); #endif - // lastErrorX/Y_int holds info from just before stopping changeState(State::LOCAL_SEARCH); } } @@ -511,66 +424,53 @@ void handleStateMachine() case State::TRACKING: if (anySensor) { - // Update last known good position before calculating move lastKnownX = stepperX.currentPosition(); lastKnownY = stepperY.currentPosition(); - // Perform P-control logic - handleTrackingLogicFixedPoint(); // Updates lastErrorX/Y_int + handleTrackingLogic(); recoveryAttempted = false; - // lastSignalTime updated in loop() } else { - // No sensors active, check timeout for signal loss if (safeMillisSubtract(now, lastSignalTime) > signalLostTimeoutMs) { #ifdef ENABLE_DEBUG_PRINTING Serial.println(F("Signal LOST (Timeout) -> Local Search")); #endif stepperX.stop(); - stepperY.stop(); // Command smooth stop - // lastErrorX/Y_int holds info from before loss + stepperY.stop(); changeState(State::LOCAL_SEARCH); } - // Else: Wait briefly for signal recovery, motors continue last command via run() } break; case State::LOCAL_SEARCH: - handleLocalSearch(); // Contains probe, sweep, box pattern logic + handleLocalSearch(); break; case State::SEARCH_FAILED: - // Signal not found after full search, wait then retry if (safeMillisSubtract(now, stateEntryTime) > searchFailDelayMs) { #ifdef ENABLE_DEBUG_PRINTING Serial.println(F("Search Failed Timeout -> Retrying Full Search (Sweep X)")); #endif - sweepDirectionX = 1; // Ensure search starts in positive X direction + sweepDirectionX = 1; changeState(State::SWEEP_X); } break; case State::ERROR: - // Fatal halt state. indicateFatalError handles actions. Do nothing here. break; default: - // Should be unreachable handleSystemError(F("FATAL: Reached Unknown State!")); break; } } -/** - * @brief Handles state transitions, logging, and state entry actions. - * @param newState The target state to transition to. - */ void changeState(State newState) { if (newState == currentState) - return; // No change needed + return; #ifdef ENABLE_DEBUG_PRINTING Serial.print(getStateName(currentState)); @@ -581,36 +481,30 @@ void changeState(State newState) currentState = newState; stateEntryTime = millis(); - // --- State Entry Actions --- - // Reset stall detection when entering any state involving motion commands if (newState >= State::HOMING_X && newState <= State::LOCAL_SEARCH) { stallStartTimeX = 0; stallStartTimeY = 0; - lastKnownStepperXPosForStall = stepperX.currentPosition(); // Use current pos as baseline + lastKnownStepperXPosForStall = stepperX.currentPosition(); lastKnownStepperYPosForStall = stepperY.currentPosition(); - lastStallCheckTime = stateEntryTime; // Start checking immediately + lastStallCheckTime = stateEntryTime; } - // Configure motors and initiate actions specific to the NEW state switch (currentState) { case State::HOMING_X: setMotorSpeeds(homingSpeed, homingAcceleration, homingSpeed, homingAcceleration); - // Move significantly in negative direction to ensure hitting the limit switch - // FIX: Added cast to long for HOMING_OVERSHOOT_MULTIPLIER to prevent potential overflow warning stepperX.moveTo(-((long)stepsPerRevolution * (long)HOMING_OVERSHOOT_MULTIPLIER)); - stepperY.stop(); // Ensure Y is stationary + stepperY.stop(); break; case State::HOMING_Y: setMotorSpeeds(homingSpeed, homingAcceleration, homingSpeed, homingAcceleration); - // FIX: Added cast to long for HOMING_OVERSHOOT_MULTIPLIER to prevent potential overflow warning stepperY.moveTo(-((long)stepsPerRevolution * (long)HOMING_OVERSHOOT_MULTIPLIER)); - stepperX.stop(); // Ensure X is stationary + stepperX.stop(); break; case State::POST_HOMING_MOVE_OFF_X: setMotorSpeeds(homingSpeed, homingAcceleration, homingSpeed, homingAcceleration); - stepperX.move(postHomingMoveOffSwitchSteps); // Small relative move away from switch + stepperX.move(postHomingMoveOffSwitchSteps); stepperY.stop(); break; case State::POST_HOMING_MOVE_OFF_Y: @@ -620,26 +514,25 @@ void changeState(State newState) break; case State::POST_HOMING_DELAY: stepperX.stop(); - stepperY.stop(); // Ensure motors are stopped during pause + stepperY.stop(); break; case State::SWEEP_X: setMotorSpeeds(sweepSpeedDefault, sweepAccelerationDefault, sweepSpeedDefault, sweepAccelerationDefault); - sweepXCyclesCompleted = 0; // Reset sweep progress - startSweepX(); // Initiate the first X sweep leg + sweepXCyclesCompleted = 0; + startSweepX(); break; case State::SWEEP_Y: setMotorSpeeds(sweepSpeedDefault, sweepAccelerationDefault, sweepSpeedDefault, sweepAccelerationDefault); - sweepYCyclesCompleted = 0; // Reset sweep progress - startSweepY(); // Initiate the first Y sweep leg + sweepYCyclesCompleted = 0; + startSweepY(); break; - case State::CENTERING: // Fall-through intentional + case State::CENTERING: case State::TRACKING: - // Set tracking speed/accel; actual movement command is in handleTrackingLogicFixedPoint setMotorSpeeds(trackSpeedDefault, trackAccelerationDefault, trackSpeedDefault, trackAccelerationDefault); break; case State::LOCAL_SEARCH: setMotorSpeeds(localSearchSpeed, localSearchAccel, localSearchSpeed, localSearchAccel); - currentLocalSearchPhase = 0; // Start with phase 0 (Probe) + currentLocalSearchPhase = 0; currentLocalSearchCycle = 0; #ifdef ENABLE_DEBUG_PRINTING Serial.print(F("Entering Local Search. Last Error Int X:")); @@ -647,52 +540,45 @@ void changeState(State newState) Serial.print(F(", Y:")); Serial.println(lastErrorY_int); #endif - // Initial move command is handled within handleLocalSearch() based on phase 0 logic break; - case State::SEARCH_FAILED: // Fall-through intentional + case State::SEARCH_FAILED: case State::ERROR: stepperX.stop(); - stepperY.stop(); // Ensure motors stop smoothly if possible - // In ERROR state, indicateFatalError() will attempt to disable outputs + stepperY.stop(); break; - case State::INITIALIZING: // Fall-through intentional + case State::INITIALIZING: case State::CHECK_PIN_CONFLICTS: - // No motor actions required break; } - updateStatusLEDsOptimized(); // Update LEDs immediately to reflect the new state + // updateStatusLEDsOptimized(); // REMOVED } // ========================================================================== // === STATE HANDLERS === // ========================================================================== - -/** - * @brief Manages the sequence of states related to homing the X and Y axes. - */ -void handleHomingStates() +void handleHomingStates() // Logic remains same as original { switch (currentState) { case State::HOMING_X: if (limitSwitchActiveX) - { // Check debounced state + { #ifdef ENABLE_DEBUG_PRINTING Serial.println(F("Homing X: Limit HIT")); #endif stepperX.stop(); - stepperX.setCurrentPosition(searchMinX_steps); // Define current position as 0 + stepperX.setCurrentPosition(searchMinX_steps); changeState(State::POST_HOMING_MOVE_OFF_X); } else if (!stepperX.isRunning()) - { // Should not happen unless stalled/timeout + { handleSystemError(F("Homing X Error: Motor stopped before limit switch.")); } break; case State::POST_HOMING_MOVE_OFF_X: if (stepperX.distanceToGo() == 0) - { // Check if small move is complete + { #ifdef ENABLE_DEBUG_PRINTING Serial.println(F("Homing X: Moved off switch -> Homing Y")); #endif @@ -702,23 +588,23 @@ void handleHomingStates() case State::HOMING_Y: if (limitSwitchActiveY) - { // Check debounced state + { #ifdef ENABLE_DEBUG_PRINTING Serial.println(F("Homing Y: Limit HIT")); #endif stepperY.stop(); - stepperY.setCurrentPosition(searchMinY_steps); // Define current position as 0 + stepperY.setCurrentPosition(searchMinY_steps); changeState(State::POST_HOMING_MOVE_OFF_Y); } else if (!stepperY.isRunning()) - { // Should not happen unless stalled/timeout + { handleSystemError(F("Homing Y Error: Motor stopped before limit switch.")); } break; case State::POST_HOMING_MOVE_OFF_Y: if (stepperY.distanceToGo() == 0) - { // Check if small move is complete + { #ifdef ENABLE_DEBUG_PRINTING Serial.println(F("Homing Y: Moved off switch -> Post Homing Delay")); #endif @@ -727,91 +613,65 @@ void handleHomingStates() break; case State::POST_HOMING_DELAY: - // Wait for specified pause duration if (safeMillisSubtract(millis(), stateEntryTime) > postHomingPauseMs) { #ifdef ENABLE_DEBUG_PRINTING Serial.println(F("Homing Complete -> Starting Full Search (Sweep X)")); #endif - sweepDirectionX = 1; // Ensure search starts in positive X direction + sweepDirectionX = 1; changeState(State::SWEEP_X); } break; default: - break; // Should not happen + break; } } /** - * @brief Calculates tracking error using integer logic and commands motor movement - * using fixed-point proportional control. Updates `lastErrorX_int`, `lastErrorY_int`. + * @brief Calculates tracking error using photoresistor differences and commands motor movement + * using proportional control. Updates `lastErrorX_int`, `lastErrorY_int`. */ -void handleTrackingLogicFixedPoint() +void handleTrackingLogic() { - // Sensor states (already debounced and read into sensorActive[]) - bool L = sensorActive[SENSOR_L], R = sensorActive[SENSOR_R]; - bool U = sensorActive[SENSOR_U], D = sensorActive[SENSOR_D]; - bool C = sensorActive[SENSOR_C]; + // sensorReadings[] are populated by readPhotoresistorInputs() + // Higher analogRead value = more light. Light source attracts. + int L_val = sensorReadings[SENSOR_L]; + int R_val = sensorReadings[SENSOR_R]; + int U_val = sensorReadings[SENSOR_U]; + int D_val = sensorReadings[SENSOR_D]; + // int C_val = sensorReadings[SENSOR_C]; // Available if needed for more complex logic - int8_t currentErrorX_int = ERROR_INT_CENTERED; - int8_t currentErrorY_int = ERROR_INT_CENTERED; + int currentErrorX_int = 0; + int currentErrorY_int = 0; - // --- Calculate X Error (Integer Representation) --- - // Goal: Determine the direction and relative magnitude of correction needed. - // Positive error = target is Left, need to move Right (+steps) - // Negative error = target is Right, need to move Left (-steps) - if (C) - { // Center ON: Target is close to center axis. Apply smaller correction. - if (L && !R) - currentErrorX_int = ERROR_INT_NEAR_POS; // Center+Left -> Small move Right - else if (!L && R) - currentErrorX_int = ERROR_INT_NEAR_NEG; // Center+Right -> Small move Left - // C && L && R => Centered or wide beam, treat as centered (0). - // C && !L && !R => Perfectly centered (0). + // --- Calculate Raw Difference (Positive error means target is Left/Up) --- + // X-axis: Positive diff (L > R) means light is to the Left, want to move stepper Right (+steps) + int x_diff = L_val - R_val; + // Y-axis: Positive diff (U > D) means light is Up, want to move stepper Down (+steps) + int y_diff = U_val - D_val; + + // --- Scale Difference to Error Integer --- + // Tune ANALOG_DIFF_SCALING_FACTOR carefully! + if (ANALOG_DIFF_SCALING_FACTOR != 0) + { // Prevent division by zero + currentErrorX_int = x_diff / ANALOG_DIFF_SCALING_FACTOR; + currentErrorY_int = y_diff / ANALOG_DIFF_SCALING_FACTOR; } else - { // Center OFF: Target is further off-axis. Apply larger correction. - if (L && !R) - currentErrorX_int = ERROR_INT_FAR_POS; // Left only -> Medium move Right - else if (!L && R) - currentErrorX_int = ERROR_INT_FAR_NEG; // Right only -> Medium move Left - // !C && L && R => Gap in middle or wide beam overlapping L/R? Treat as centered (0) for now. - // !C && !L && !R => No signal detected, error remains 0. State machine handles loss via timeout. + { // Should not happen if configured, but safety + currentErrorX_int = (x_diff > 0) ? 1 : ((x_diff < 0) ? -1 : 0); + currentErrorY_int = (y_diff > 0) ? 1 : ((y_diff < 0) ? -1 : 0); } - // --- Calculate Y Error (Integer Representation) --- - // Positive error = target is Up, need to move Down (+steps) - // Negative error = target is Down, need to move Up (-steps) - if (C) - { // Center ON: Smaller correction. - if (U && !D) - currentErrorY_int = ERROR_INT_NEAR_POS; // Center+Up -> Small move Down - else if (!U && D) - currentErrorY_int = ERROR_INT_NEAR_NEG; // Center+Down -> Small move Up - } - else - { // Center OFF: Larger correction. - if (U && !D) - currentErrorY_int = ERROR_INT_FAR_POS; // Up only -> Medium move Down - else if (!U && D) - currentErrorY_int = ERROR_INT_FAR_NEG; // Down only -> Medium move Up - // !C && U && D => Gap or wide beam, treat as centered (0). - } - - // --- Store Last Error --- - // Store the calculated integer error before applying gain/deadband. - // Used by Local Search if signal is lost immediately after this calculation. + // --- Store Last Error (for Local Search) --- lastErrorX_int = currentErrorX_int; lastErrorY_int = currentErrorY_int; - // --- Apply Proportional Gain (Fixed Point) --- - // steps = (error_int * Kp_scaled) / SCALE = (error_int * Kp_float * SCALE) / SCALE - // Use right shift for efficient division by power-of-2 SCALE. - long stepsToMoveX = ((long)currentErrorX_int * Kp_X_scaled) >> FIXED_POINT_SHIFT; - long stepsToMoveY = ((long)currentErrorY_int * Kp_Y_scaled) >> FIXED_POINT_SHIFT; + // --- Apply Proportional Gain (Floating Point) --- + long stepsToMoveX = (long)((float)currentErrorX_int * Kp_X_float); + long stepsToMoveY = (long)((float)currentErrorY_int * Kp_Y_float); // --- Apply Deadband --- - // If calculated move is smaller than the deadband, ignore it to prevent jitter. if (abs(stepsToMoveX) < trackingDeadband_steps) stepsToMoveX = 0; if (abs(stepsToMoveY) < trackingDeadband_steps) @@ -822,47 +682,32 @@ void handleTrackingLogicFixedPoint() { long currentX = stepperX.currentPosition(); long currentY = stepperY.currentPosition(); - // Calculate absolute target position, clamped within defined search limits. long targetX = clamp(currentX + stepsToMoveX, searchMinX_steps, searchMaxX_steps); long targetY = clamp(currentY + stepsToMoveY, searchMinY_steps, searchMaxY_steps); - // Only issue moveTo command if the target has actually changed. - // Avoids redundant commands to AccelStepper if already moving to that target or if move is 0. if (targetX != stepperX.targetPosition()) - { stepperX.moveTo(targetX); - } if (targetY != stepperY.targetPosition()) - { stepperY.moveTo(targetY); - } } - // If stepsToMoveX/Y are both 0, no new moveTo command is issued. - // Motors will continue to their previous target or remain stopped, handled by stepper.run(). } -/** - * @brief Implements the local search strategy when the IR signal is lost during tracking. - * Cycles through: Probe -> Perpendicular Sweep -> Expanding Box Pattern. - */ -void handleLocalSearch() +void handleLocalSearch() // Logic remains similar to original, uses lastErrorX/Y_int { unsigned long now = millis(); - // --- Check for Signal Re-acquisition --- - if (isAnySensorActive()) + if (isAnySensorGenerallyActive()) { #ifdef ENABLE_DEBUG_PRINTING Serial.println(F("Signal RE-ACQUIRED (Local Search) -> Centering")); #endif stepperX.stop(); stepperY.stop(); - recoveryAttempted = false; // Reset flag as we recovered + recoveryAttempted = false; changeState(State::CENTERING); return; } - // --- Check Overall Timeout --- if (safeMillisSubtract(now, stateEntryTime) > localSearchTimeoutMs) { #ifdef ENABLE_DEBUG_PRINTING @@ -870,20 +715,18 @@ void handleLocalSearch() #endif stepperX.stop(); stepperY.stop(); - sweepDirectionX = 1; // Reset sweep + sweepDirectionX = 1; changeState(State::SWEEP_X); return; } - // --- Execute Search Pattern Logic (Only if Motors Stopped) --- - // Proceed only if the previous local search move is complete. if (stepperX.distanceToGo() == 0 && stepperY.distanceToGo() == 0) { long currentX = stepperX.currentPosition(); long currentY = stepperY.currentPosition(); long targetX = currentX; long targetY = currentY; - bool commandMove = false; // Flag to indicate if a new move should be commanded + bool commandMove = false; #ifdef ENABLE_DEBUG_PRINTING Serial.print(F("Local Search Phase: ")); @@ -894,68 +737,57 @@ void handleLocalSearch() switch (currentLocalSearchPhase) { - // Phase 0: Probe - Make a small move in the direction the target was last seen. - case 0: - { // FIX: Added curly braces for scope + case 0: // Probe + { #ifdef ENABLE_DEBUG_PRINTING Serial.println(F(" Phase 0: Probing Last Direction")); #endif - // Determine probe direction based on the sign of the last recorded integer error. - // Positive error X -> target was Left, probe Right (+steps) - // Positive error Y -> target was Up, probe Down (+steps) + // lastErrorX_int > 0 means target was Left (L>R), so probe Right (+steps) + // lastErrorY_int > 0 means target was Up (U>D), so probe Down (+steps) targetX = currentX + (lastErrorX_int > 0 ? 1 : (lastErrorX_int < 0 ? -1 : 0)) * localSearchProbeSteps; targetY = currentY + (lastErrorY_int > 0 ? 1 : (lastErrorY_int < 0 ? -1 : 0)) * localSearchProbeSteps; - // Clamp probe move to physical limits and also relative to last known position to prevent excessive drift. targetX = clamp(targetX, searchMinX_steps, searchMaxX_steps); targetY = clamp(targetY, searchMinY_steps, searchMaxY_steps); - targetX = clamp(targetX, lastKnownX - localSearchSweepSteps, lastKnownX + localSearchSweepSteps); // Limit probe range + targetX = clamp(targetX, lastKnownX - localSearchSweepSteps, lastKnownX + localSearchSweepSteps); targetY = clamp(targetY, lastKnownY - localSearchSweepSteps, lastKnownY + localSearchSweepSteps); - commandMove = true; - currentLocalSearchPhase = 1; // Advance to Sweep phase next - currentLocalSearchCycle = 0; // Reset cycle counter for Sweep phase + currentLocalSearchPhase = 1; + currentLocalSearchCycle = 0; break; - } // FIX: Closed curly brace - - // Phase 1: Sweep - Perform sweeps perpendicular to the last known direction (simple cross pattern). - case 1: - { // FIX: Added curly braces for scope + } + case 1: // Perpendicular Sweep + { #ifdef ENABLE_DEBUG_PRINTING Serial.println(F(" Phase 1: Perpendicular Sweep")); #endif - // Cycle 0: Sweep X+ ; Cycle 1: Sweep X- ; Cycle 2: Sweep Y+ ; Cycle 3: Sweep Y- - // Uses lastKnownX/Y as the center of the sweep cross. - int sweepDir = (currentLocalSearchCycle % 2 == 0) ? 1 : -1; // Alternate positive/negative direction + int sweepDir = (currentLocalSearchCycle % 2 == 0) ? 1 : -1; if (currentLocalSearchCycle < 2) - { // Sweep X axis (Cycles 0, 1) + { // Sweep X targetX = lastKnownX + sweepDir * localSearchSweepSteps; + targetY = lastKnownY; // Stay on last known Y for X sweep } else - { // Sweep Y axis (Cycles 2, 3) + { // Sweep Y targetY = lastKnownY + sweepDir * localSearchSweepSteps; + targetX = lastKnownX; // Stay on last known X for Y sweep } - targetX = clamp(targetX, searchMinX_steps, searchMaxX_steps); // Clamp to absolute limits + targetX = clamp(targetX, searchMinX_steps, searchMaxX_steps); targetY = clamp(targetY, searchMinY_steps, searchMaxY_steps); - commandMove = true; currentLocalSearchCycle++; if (currentLocalSearchCycle >= 4) - { // Completed all 4 legs of the cross? - currentLocalSearchPhase = 2; // Advance to Box Pattern phase - currentLocalSearchCycle = 0; // Reset cycle counter for Box phase + { + currentLocalSearchPhase = 2; + currentLocalSearchCycle = 0; } break; - } // FIX: Closed curly brace - - // Phase 2: Box Pattern - Fallback expanding square spiral search. - case 2: - { // FIX: Added curly braces for scope + } + case 2: // Box Pattern + { #ifdef ENABLE_DEBUG_PRINTING Serial.println(F(" Phase 2: Expanding Box Pattern")); #endif - // Check if max search cycles (box layers) exceeded - // FIX: Cast currentLocalSearchCycle to uint16_t to avoid signed/unsigned comparison warning if ((uint16_t)currentLocalSearchCycle >= ((uint16_t)localSearchMaxCycles * LOCAL_SEARCH_BOX_STEPS_PER_LAYER)) { #ifdef ENABLE_DEBUG_PRINTING @@ -963,51 +795,45 @@ void handleLocalSearch() #endif sweepDirectionX = 1; changeState(State::SWEEP_X); - return; // Give up local search + return; } - long dX = 0, dY = 0; - int moveScale = (currentLocalSearchCycle / LOCAL_SEARCH_BOX_STEPS_PER_LAYER) + 1; // Expansion scale (layer number) - // Determine movement vector based on step number within the current box layer + int moveScale = (currentLocalSearchCycle / LOCAL_SEARCH_BOX_STEPS_PER_LAYER) + 1; switch (currentLocalSearchCycle % LOCAL_SEARCH_BOX_STEPS_PER_LAYER) { - // Simple square pattern: R, U, L, L, D, D, R, R (relative to current position) - // Adjust number of steps per side if needed. case 0: dX = (long)localSearchBoxStepSize * moveScale; - break; // Right - Added cast just in case + break; // R case 1: dY = -(long)localSearchBoxStepSize * moveScale; - break; // Up (Negative Y) - Added cast + break; // U (photoresistor Y-axis is inverted relative to motor Y for "Up") case 2: dX = -(long)localSearchBoxStepSize * moveScale; - break; // Left - Added cast + break; // L case 3: dX = -(long)localSearchBoxStepSize * moveScale; - break; // Left - Added cast + break; // L case 4: dY = (long)localSearchBoxStepSize * moveScale; - break; // Down (Positive Y) - Added cast + break; // D case 5: dY = (long)localSearchBoxStepSize * moveScale; - break; // Down - Added cast + break; // D case 6: dX = (long)localSearchBoxStepSize * moveScale; - break; // Right - Added cast + break; // R case 7: dX = (long)localSearchBoxStepSize * moveScale; - break; // Right (completes layer) - Added cast + break; // R } targetX = clamp(currentX + dX, searchMinX_steps, searchMaxX_steps); targetY = clamp(currentY + dY, searchMinY_steps, searchMaxY_steps); - commandMove = true; - currentLocalSearchCycle++; // Increment box step counter + currentLocalSearchCycle++; break; } - } + } // end switch - // --- Command Move (If Calculated) --- if (commandMove && (targetX != currentX || targetY != currentY)) { #ifdef ENABLE_DEBUG_PRINTING @@ -1016,7 +842,6 @@ void handleLocalSearch() Serial.print(F(", Y:")); Serial.println(targetY); #endif - // Only issue moveTo if target changed if (targetX != stepperX.targetPosition()) stepperX.moveTo(targetX); if (targetY != stepperY.targetPosition()) @@ -1024,42 +849,24 @@ void handleLocalSearch() } else if (commandMove) { -// Move was calculated but resulted in no change (e.g., clamped at limit). -// Need to ensure the state machine progresses. #ifdef ENABLE_DEBUG_PRINTING - Serial.println(F(" Move resulted in no change (at limit or target=current). Forcing phase/cycle advance.")); + Serial.println(F(" Local search move resulted in no change.")); #endif - // Force advancement logic (simplified): If a move was intended but didn't happen, - // immediately try the next step/phase in the next loop iteration by incrementing counters/phases. - // This logic assumes the phase/cycle increments done above are sufficient. - // If getting stuck here, may need more explicit advancement logic. E.g.: - // if (currentLocalSearchPhase == 0) { currentLocalSearchPhase=1; currentLocalSearchCycle=0; } - // else if (currentLocalSearchPhase == 1) { /* cycle incremented above */ if(currentLocalSearchCycle>=4) { currentLocalSearchPhase=2; currentLocalSearchCycle=0;} } - // else if (currentLocalSearchPhase == 2) { /* cycle incremented above */ if(currentLocalSearchCycle >= ...) { changeState(State::SWEEP_X); } } } } - // Else: Motors are still running the previous local search move; wait for completion. } -/** - * @brief Initiates an X-axis sweep move towards the min or max limit. - */ -void startSweepX() +void startSweepX() // Logic remains same { long targetX = (sweepDirectionX > 0) ? searchMaxX_steps : searchMinX_steps; #ifdef ENABLE_DEBUG_PRINTING Serial.print(F("Starting X Sweep Leg -> Target: ")); Serial.println(targetX); #endif - // Clamp target just in case limits calculation was slightly off. stepperX.moveTo(clamp(targetX, searchMinX_steps, searchMaxX_steps)); - // stepperY.stop(); // Optional: Explicitly stop Y if needed, but usually handled by state transitions. } -/** - * @brief Initiates a Y-axis sweep move towards the min or max limit. - */ -void startSweepY() +void startSweepY() // Logic remains same { long targetY = (sweepDirectionY > 0) ? searchMaxY_steps : searchMinY_steps; #ifdef ENABLE_DEBUG_PRINTING @@ -1067,69 +874,37 @@ void startSweepY() Serial.println(targetY); #endif stepperY.moveTo(clamp(targetY, searchMinY_steps, searchMaxY_steps)); - // stepperX.stop(); // Optional: Explicitly stop X. } // ========================================================================== // === SETUP & UTILITY FUNCTIONS === // ========================================================================== - -/** - * @brief Initializes pin modes, performs critical pin conflict checks, and prints warnings. - * @return True if setup is successful, False if a fatal conflict is found (should halt). - */ bool setupPinsAndCheckConflicts() { #ifdef ENABLE_DEBUG_PRINTING Serial.println(F("--- Initializing Pins & Checking Conflicts ---")); #endif + // Photoresistor pins are A0-A4 (INPUT by default for analogRead, no explicit pinMode needed) + // Limit Switches: Input with internal pull-up. MOVED TO D2, D3. + pinMode(limitSwitchPinX, INPUT_PULLUP); // D2 + pinMode(limitSwitchPinY, INPUT_PULLUP); // D3 - // --- Configure Pin Modes --- - // Sensors & Limit Switches: Input with internal pull-up enabled. - pinMode(sensorPinL, INPUT_PULLUP); - pinMode(sensorPinR, INPUT_PULLUP); - pinMode(sensorPinU, INPUT_PULLUP); - pinMode(sensorPinD, INPUT_PULLUP); - pinMode(sensorPinC, INPUT_PULLUP); // Analog pin used as digital input - pinMode(limitSwitchPinX, INPUT_PULLUP); - pinMode(limitSwitchPinY, INPUT_PULLUP); + // LEDs REMOVED - // LEDs: Output, initially LOW. Using direct port manipulation for efficiency. - // Set DDRC bits for LED pins (PC1, PC2, PC3) to 1 (Output). - DDRC |= ledMaskAll; - // Set PORTC bits for LED pins to 0 (LOW). - PORTC &= ~ledMaskAll; - - // Stepper pins are configured by the AccelStepper library constructor. - - // --- Critical Conflict Checks --- - - // Internal Pin Overlaps (Add more checks if custom pins are used) - // Basic check: Ensure limit switches don't overlap LEDs on PORTC - if (((limitSwitchMaskX | limitSwitchMaskY) & ledMaskAll) != 0) - { - indicateFatalError(F("FATAL PIN CONFIG: Limit switch pin conflicts with LED pin on PORTC!")); - return false; // Should not be reached - } - // Add checks for overlaps between sensors, steppers etc. if pins were changed significantly. - // Example: if (sensorPinL == stepperX_pin1) { indicateFatalError(F("...")); } + // Stepper pins are configured by AccelStepper. #ifdef ENABLE_DEBUG_PRINTING - Serial.println(F("--- Pin Conflict Check PASSED (Review I2C Warning!) ---")); - Serial.println(F("--- Pin Initialization Complete ---")); + Serial.println(F("--- Pin Initialization Complete (Review I2C/Analog Pin Warning!) ---")); #endif - return true; // Setup successful + return true; } -/** - * @brief Prints the initial startup message with version info. - */ -void printStartupMessage() +void printStartupMessage() // Logic remains same { #if defined(ENABLE_DEBUG_PRINTING) || defined(ENABLE_TELEMETRY) - Serial.println(F("")); // Newline + Serial.println(F("")); Serial.println(F("-------------------------------------")); - Serial.print(F(" IR Tracker System Initializing v")); + Serial.print(F(" Photoresistor Tracker System Initializing v")); Serial.println(F(SYSTEM_VERSION)); #ifdef DEBUG_BUILD Serial.println(F(" Build Mode: DEBUG (Serial Enabled)")); @@ -1137,27 +912,34 @@ void printStartupMessage() Serial.println(F(" Build Mode: PRODUCTION (Serial Disabled)")); #endif Serial.println(F("-------------------------------------")); - Serial.flush(); // Ensure message is printed before potentially complex setup + Serial.flush(); #endif } -/** - * @brief Prints detailed tuning guidance (only if Serial output is enabled). - */ -void printTuningGuide() +void printTuningGuide() // Updated for photoresistors { #if defined(ENABLE_DEBUG_PRINTING) || defined(ENABLE_TELEMETRY) - Serial.println(F("\n--- TUNING REQUIRED - READ CAREFULLY ---")); - Serial.println(F("This system WILL NOT WORK reliably without tuning to YOUR specific hardware.")); - Serial.println(F("Defaults are intentionally conservative/starting points ONLY.")); - Serial.println(F("METHODICAL TUNING IS ESSENTIAL. Test ONE parameter change at a time.")); + Serial.println(F("\n--- TUNING REQUIRED - READ CAREFULLY (PHOTORESISTOR VERSION) ---")); + Serial.println(F("This system WILL NOT WORK reliably without tuning to YOUR specific hardware & light conditions.")); + Serial.println(F("Assumes photoresistors give HIGHER analogRead() value for MORE light.")); - Serial.println(F("\n1. SPEEDS & ACCELERATION (Start Here):")); - Serial.println(F(" - Goal: Find the fastest reliable speed/accel WITHOUT stalling or skipping steps.")); - Serial.println(F(" - Test: Command long moves (e.g., manually trigger SWEEP state). Listen/watch for issues.")); - Serial.println(F(" - Start LOW (e.g., 300/300) and increase gradually. Find max reliable homing/sweep speeds first.")); - Serial.println(F(" - Track speed can be lower if needed for smoother tracking.")); - Serial.print(F(" - CURRENT DEFAULTS (Likely need tuning): H=")); + Serial.println(F("\n1. PHOTORESISTOR PARAMETERS (Start Here):")); + Serial.println(F(" - Goal: Reliable light detection and differential error calculation.")); + Serial.println(F(" - Use Serial Plotter to observe sensorReadings[] values under various light conditions.")); + Serial.print(F(" - PHOTO_LIGHT_THRESHOLD_GENERAL (Currently: ")); + Serial.print(PHOTO_LIGHT_THRESHOLD_GENERAL); + Serial.println(F("): Min reading for 'isAnySensorGenerallyActive'. Set above ambient noise, below target light.")); + Serial.print(F(" - PHOTO_LIGHT_THRESHOLD_CENTER (Currently: ")); + Serial.print(PHOTO_LIGHT_THRESHOLD_CENTER); + Serial.println(F("): Similar, for center sensor if specific logic added (currently only for general detection).")); + Serial.print(F(" - ANALOG_DIFF_SCALING_FACTOR (Currently: ")); + Serial.print(ANALOG_DIFF_SCALING_FACTOR); + Serial.println(F("): Divides (L-R) or (U-D) difference. Tune so error_int is in a reasonable range (e.g., -20 to +20) for Kp.")); + Serial.println(F(" Too small factor = large error_int = Kp makes huge jumps. Too large factor = small error_int = sluggish.")); + + Serial.println(F("\n2. SPEEDS & ACCELERATION:")); + Serial.println(F(" - Goal: Find fastest reliable speed/accel WITHOUT stalling. Start LOW.")); + Serial.print(F(" - CURRENT DEFAULTS: H=")); Serial.print(homingSpeed); Serial.print(F(" Sw=")); Serial.print(sweepSpeedDefault); @@ -1166,224 +948,85 @@ void printTuningGuide() Serial.print(F(" Accel ~")); Serial.println(trackAccelerationDefault); - Serial.println(F("\n2. PROPORTIONAL CONTROL (Kp_X_float, Kp_Y_float, trackingDeadband_steps) - MOST CRITICAL:")); - Serial.println(F(" - Kp (Gain): Tune the Kp_X_float / Kp_Y_float values. Determines reaction strength. START LOW (e.g., 5.0-10.0).")); - Serial.println(F(" - TOO LOW: Sluggish, slow to center, may lose target easily. -> INCREASE Kp_float slowly.")); - Serial.println(F(" - TOO HIGH: Overshoots, oscillates (shakes back and forth), unstable. -> DECREASE Kp_float.")); - Serial.println(F(" - Deadband: Zone (in steps) around center where no correction occurs.")); - Serial.println(F(" - TOO LOW: Jitters/hunts when centered (if Kp is high enough). -> INCREASE Deadband.")); - Serial.println(F(" - TOO HIGH: Target needs to be far off-center before correction starts. -> DECREASE Deadband.")); - Serial.println(F(" - Test: Manually move IR source slowly across sensors in TRACKING state. Observe stability, overshoot, centering accuracy.")); - Serial.print(F(" - CURRENT DEFAULTS (MUST TUNE): Kp_X_float=")); + Serial.println(F("\n3. PROPORTIONAL CONTROL (Kp_X_float, Kp_Y_float, trackingDeadband_steps):")); + Serial.println(F(" - Kp (Gain): Tune Kp_X_float / Kp_Y_float. Determines reaction strength. START LOW (e.g., 5.0-20.0).")); + Serial.println(F(" - TOO LOW: Sluggish. -> INCREASE Kp_float.")); + Serial.println(F(" - TOO HIGH: Overshoots, oscillates. -> DECREASE Kp_float.")); + Serial.println(F(" - Deadband: Zone (steps) around center where no correction occurs.")); + Serial.print(F(" - CURRENT DEFAULTS: Kp_X_float=")); Serial.print(Kp_X_float); Serial.print(F(", Kp_Y_float=")); Serial.print(Kp_Y_float); Serial.print(F(", Deadband=")); Serial.println(trackingDeadband_steps); - Serial.println(F("\n3. STALL DETECTION (Safety Net Tuning):")); - Serial.println(F(" - Goal: Detect real stalls without triggering falsely during normal operation.")); - Serial.println(F(" - Timeout: How long motor must be 'stuck' before erroring.")); - Serial.println(F(" - Interval: How often position is checked.")); - Serial.println(F(" - Tolerance: Min steps moved between checks to be considered 'not stuck'.")); - Serial.println(F(" - If false stalls: INCREASE Timeout, or slightly INCREASE Interval/Tolerance.")); - Serial.println(F(" - If real stalls missed: DECREASE Timeout. Ensure speeds aren't too high (Step 1).")); - Serial.print(F(" - CURRENT DEFAULTS: Interval=")); - Serial.print(stallCheckIntervalMs); - Serial.print(F("ms, Timeout=")); - Serial.print(stallTimeoutThresholdMs); - Serial.print(F("ms, Tolerance=")); - Serial.println(stallPositionTolerance); - - Serial.println(F("\n4. OTHER TIMINGS (signalLostTimeoutMs, etc.):")); - Serial.println(F(" - signalLostTimeoutMs: Adjust based on expected signal dropouts vs. true loss.")); - Serial.println(F(" - localSearch params: Tune probe/sweep steps based on beam width and Kp performance.")); + Serial.println(F("\n4. STALL DETECTION & OTHER TIMINGS: Review if issues arise.")); Serial.println(F("\n--- End Tuning Guidance ---")); Serial.flush(); #endif } /** - * @brief Reads sensor and limit switch states using direct port reads and debounces them. - * Updates global `sensorActive[]`, `limitSwitchActiveX`, `limitSwitchActiveY`. + * @brief Reads photoresistor values and limit switch states. + * Updates global `sensorReadings[]`, `sensorActive[]`, `limitSwitchActiveX/Y`. */ -void readInputsOptimized() +void readPhotoresistorInputs() { unsigned long now = millis(); - // --- Read Raw Port States --- - // Read only the ports where relevant input pins are located - uint8_t currentPinStatesB = PINB; // For Sensor U (PB4), Sensor D (PB5) - uint8_t currentPinStatesC = PINC; // For Sensor C (PC0), Limit X (PC4), Limit Y (PC5) - uint8_t currentPinStatesD = PIND; // For Sensor L (PD2), Sensor R (PD3) - - // --- Check for Changes & Reset Debounce Timers --- - // Define masks for the input pins we care about on each port - const uint8_t relevantMaskB_Sensors = sensorMaskU | sensorMaskD; - const uint8_t relevantMaskC_Sensors = sensorMaskC; - const uint8_t relevantMaskD_Sensors = sensorMaskL | sensorMaskR; - const uint8_t relevantMaskC_Limits = limitSwitchMaskX | limitSwitchMaskY; - - bool sensorsChanged = ((lastRawPinStatesB ^ currentPinStatesB) & relevantMaskB_Sensors) || - ((lastRawPinStatesC ^ currentPinStatesC) & relevantMaskC_Sensors) || - ((lastRawPinStatesD ^ currentPinStatesD) & relevantMaskD_Sensors); - bool limitsChanged = ((lastRawPinStatesC ^ currentPinStatesC) & relevantMaskC_Limits); - - if (sensorsChanged) + // --- Read Photoresistors --- + for (int i = 0; i < NUM_SENSORS; i++) { - lastDebounceTimeSensors = now; + sensorReadings[i] = analogRead(photoresistorPins[i]); + sensorActive[i] = (sensorReadings[i] > PHOTO_LIGHT_THRESHOLD_GENERAL); } + + // --- Read Limit Switches (on PORTD: D2, D3) --- + uint8_t currentPinStatesD_limits = PIND; // Read PORTD for limit switches + + // Check for Changes & Reset Debounce Timers for Limit Switches + bool limitsChanged = ((lastRawPinStatesD ^ currentPinStatesD_limits) & (limitSwitchMaskX | limitSwitchMaskY)); + if (limitsChanged) { lastDebounceTimeLimits = now; } + lastRawPinStatesD = currentPinStatesD_limits; // Update last raw state FOR LIMITS - // Update last raw states AFTER comparison - lastRawPinStatesB = currentPinStatesB; - lastRawPinStatesC = currentPinStatesC; - lastRawPinStatesD = currentPinStatesD; - - // --- Apply Debounce Logic --- - // Update active states only if enough time has passed since the last bounce. - if (safeMillisSubtract(now, lastDebounceTimeSensors) > debounceDelayMs) - { - // Active LOW logic: Sensor is active (true) if the corresponding PIN bit is 0. - sensorActive[SENSOR_L] = !(currentPinStatesD & sensorMaskL); - sensorActive[SENSOR_R] = !(currentPinStatesD & sensorMaskR); - sensorActive[SENSOR_U] = !(currentPinStatesB & sensorMaskU); - sensorActive[SENSOR_D] = !(currentPinStatesB & sensorMaskD); - sensorActive[SENSOR_C] = !(currentPinStatesC & sensorMaskC); - } - + // Apply Debounce Logic for Limit Switches if (safeMillisSubtract(now, lastDebounceTimeLimits) > limitSwitchDebounceMs) { - // Active LOW logic for limit switches. - limitSwitchActiveX = !(currentPinStatesC & limitSwitchMaskX); - limitSwitchActiveY = !(currentPinStatesC & limitSwitchMaskY); + limitSwitchActiveX = !(currentPinStatesD_limits & limitSwitchMaskX); + limitSwitchActiveY = !(currentPinStatesD_limits & limitSwitchMaskY); } } /** - * @brief Simple check if any sensor is currently reporting an active signal. - * Uses debounced sensor states. - * @return True if at least one sensor is active, False otherwise. + * @brief Checks if any photoresistor is reading above the general threshold. */ -inline bool isAnySensorActive() +inline bool isAnySensorGenerallyActive() { - // Check the debounced values in the global array for (uint8_t i = 0; i < NUM_SENSORS; i++) { if (sensorActive[i]) - return true; + return true; // Uses the pre-calculated sensorActive based on threshold } return false; } /** - * @brief Checks if the center sensor is currently active. - * @return True if the center sensor is active, False otherwise. + * @brief Checks if the center photoresistor is reading above the general threshold. */ -inline bool isCentered() +inline bool isCenterSensorGenerallyActive() { - // Check the debounced state of the center sensor - return sensorActive[SENSOR_C]; + return sensorActive[SENSOR_C]; // Uses the pre-calculated sensorActive } -/** - * @brief Updates status LEDs using direct port manipulation for efficiency. - */ -void updateStatusLEDsOptimized() -{ - uint8_t ledStateMask = 0; // Bitmask of LEDs to turn ON (0 = off) - unsigned int blinkInterval = 0; // Blink cycle duration (ms), 0 = solid ON +// updateStatusLEDsOptimized() REMOVED - // Determine LED pattern based on current state - switch (currentState) - { - // Blinking Yellow (Searching LED) - case State::HOMING_X: - case State::HOMING_Y: - ledStateMask = ledMaskSearching; - blinkInterval = 600; - break; - case State::CENTERING: - ledStateMask = ledMaskSearching; - blinkInterval = 300; - break; // Faster blink - case State::LOCAL_SEARCH: - ledStateMask = ledMaskSearching; - blinkInterval = 400; - break; // Medium blink - - // Solid Yellow (Searching LED) - case State::POST_HOMING_MOVE_OFF_X: // Fall-through - case State::POST_HOMING_MOVE_OFF_Y: // Fall-through - case State::POST_HOMING_DELAY: // Fall-through - case State::SWEEP_X: - case State::SWEEP_Y: - ledStateMask = ledMaskSearching; - break; - - // Green (Tracking LED) - case State::TRACKING: - // Solid Green only if centered AND stopped moving. Blinking Green otherwise. - if (sensorActive[SENSOR_C] && stepperX.distanceToGo() == 0 && stepperY.distanceToGo() == 0) - { - ledStateMask = ledMaskTracking; // Solid Green - } - else - { - ledStateMask = ledMaskTracking; - blinkInterval = 500; // Blinking Green - } - break; - - // Red / Blinking Red (Lost LED) - case State::SEARCH_FAILED: - ledStateMask = ledMaskLost; - blinkInterval = 1000; - break; // Slow blink Red - case State::ERROR: - ledStateMask = ledMaskLost; - blinkInterval = 250; - break; // Fast blink Red - - // All Off - case State::INITIALIZING: // Fall-through - case State::CHECK_PIN_CONFLICTS: - ledStateMask = 0; - break; - } - - // --- Apply Blinking Logic --- - if (blinkInterval > 0) - { - // Simple time-based blink: Check if we are in the "ON" half of the cycle. - // (millis() / (half_interval)) % 2 == 0 -> ON period - // (millis() / (half_interval)) % 2 == 1 -> OFF period - bool isOnPeriod = ((millis() / (blinkInterval / 2)) % 2) == 0; - if (!isOnPeriod) - { - ledStateMask = 0; // Force OFF during the off-period of the blink cycle - } - } - - // --- Update LEDs via Direct Port Manipulation (PORTC) --- - // Read current PORTC state, mask out all LED bits, then OR in the desired state. - uint8_t portC_val = PORTC; - portC_val &= ~ledMaskAll; // Clear all LED bits - portC_val |= ledStateMask; // Set bits for the LEDs that should be ON - PORTC = portC_val; // Write the updated value back to the port -} - -/** - * @brief Checks if any relevant state has exceeded its safety timeout duration. - * Calls handleSystemError if a timeout occurs. - */ -void checkStateTimeouts() +void checkStateTimeouts() // Logic remains same { unsigned long currentTimeout = 0; - // Assign timeout duration based on the current potentially blocking state switch (currentState) { case State::HOMING_X: @@ -1397,13 +1040,9 @@ void checkStateTimeouts() case State::CENTERING: currentTimeout = stateTimeoutMs; break; - // LOCAL_SEARCH has its own specific timeout handled within its function. - // Other states (TRACKING, SEARCH_FAILED, ERROR, IDLE) don't require this generic timeout. default: - return; // No timeout check needed for this state + return; } - - // Check if the timeout has been exceeded if (currentTimeout > 0 && safeMillisSubtract(millis(), stateEntryTime) > currentTimeout) { #ifdef ENABLE_DEBUG_PRINTING @@ -1413,150 +1052,110 @@ void checkStateTimeouts() Serial.print(currentTimeout); Serial.println(F(" ms")); #endif - // Use F() macro for the error reason string handleSystemError(F("Safety Timeout: State duration exceeded limit.")); } } -/** - * @brief Checks if either motor appears to be stalled (commanded to move but position isn't changing). - * Runs check periodically based on `stallCheckIntervalMs`. Calls handleSystemError if stall detected. - */ -void checkMotorStall() +void checkMotorStall() // Logic remains same { unsigned long now = millis(); - // Only perform check at the specified interval if (safeMillisSubtract(now, lastStallCheckTime) < stallCheckIntervalMs) - { return; - } - lastStallCheckTime = now; // Update last check time + lastStallCheckTime = now; bool stallDetected = false; const __FlashStringHelper *stallReason = nullptr; - // --- Check X Motor --- if (stepperX.isRunning()) - { // Only check if motor is supposed to be moving + { long currentXPos = stepperX.currentPosition(); - // Calculate position change since last check (use manual abs for long) long deltaX = currentXPos - lastKnownStepperXPosForStall; if (deltaX < 0) deltaX = -deltaX; - if (deltaX < stallPositionTolerance) - { // Position changed less than tolerance? Potential stall. + { if (stallStartTimeX == 0) - { // If timer not started, start it now stallStartTimeX = now; - } else if (safeMillisSubtract(now, stallStartTimeX) > stallTimeoutThresholdMs) - { // Stuck for too long? + { stallDetected = true; stallReason = F("Stall Detected: Motor X stuck."); } } else - { // Motor moved sufficiently - stallStartTimeX = 0; // Reset stall timer - lastKnownStepperXPosForStall = currentXPos; // Update known position only when moving + { + stallStartTimeX = 0; + lastKnownStepperXPosForStall = currentXPos; } } else - { // Motor is not running (stopped normally or finished move) - stallStartTimeX = 0; // Reset stall timer - lastKnownStepperXPosForStall = stepperX.currentPosition(); // Keep known position updated + { + stallStartTimeX = 0; + lastKnownStepperXPosForStall = stepperX.currentPosition(); } - // --- Check Y Motor (only if X didn't stall) --- if (!stallDetected && stepperY.isRunning()) { long currentYPos = stepperY.currentPosition(); long deltaY = currentYPos - lastKnownStepperYPosForStall; if (deltaY < 0) deltaY = -deltaY; - if (deltaY < stallPositionTolerance) - { // Potential stall + { if (stallStartTimeY == 0) - { stallStartTimeY = now; - } else if (safeMillisSubtract(now, stallStartTimeY) > stallTimeoutThresholdMs) - { // Stuck too long? + { stallDetected = true; stallReason = F("Stall Detected: Motor Y stuck."); } } else - { // Moved sufficiently - stallStartTimeY = 0; // Reset timer - lastKnownStepperYPosForStall = currentYPos; // Update position + { + stallStartTimeY = 0; + lastKnownStepperYPosForStall = currentYPos; } } else - { // Motor not running - stallStartTimeY = 0; // Reset timer - lastKnownStepperYPosForStall = stepperY.currentPosition(); // Update position + { + stallStartTimeY = 0; + lastKnownStepperYPosForStall = stepperY.currentPosition(); } - // --- Handle Stall Detection --- if (stallDetected) - { handleSystemError(stallReason); - } } -/** - * @brief Central error handler. Logs the error, attempts recovery once if possible, - * otherwise triggers a fatal error halt. - * @param reason A pointer to a flash string (F() macro) describing the error. - */ -void handleSystemError(const __FlashStringHelper *reason) +void handleSystemError(const __FlashStringHelper *reason) // Logic remains same { #if defined(ENABLE_DEBUG_PRINTING) || defined(ENABLE_TELEMETRY) Serial.println(F("--- System Error Detected ---")); Serial.println(reason); #endif - - // Check if recovery is feasible (in an operational state) and hasn't been attempted yet bool canAttemptRecovery = (currentState >= State::SWEEP_X && currentState <= State::LOCAL_SEARCH); - if (!recoveryAttempted && canAttemptRecovery) { #if defined(ENABLE_DEBUG_PRINTING) || defined(ENABLE_TELEMETRY) Serial.println(F("Attempting recovery: Stopping motors and restarting full search...")); #endif - recoveryAttempted = true; // Set flag to prevent infinite loops + recoveryAttempted = true; stepperX.stop(); - stepperY.stop(); // Command smooth stop - sweepDirectionX = 1; // Reset search parameters - changeState(State::SWEEP_X); // Attempt to restart the search process - return; // Exit handler, let the state machine continue from SWEEP_X + stepperY.stop(); + sweepDirectionX = 1; + changeState(State::SWEEP_X); + return; } - -// Recovery not possible, already attempted, or failed. Trigger fatal error. #if defined(ENABLE_DEBUG_PRINTING) || defined(ENABLE_TELEMETRY) if (recoveryAttempted && canAttemptRecovery) - { Serial.println(F("Recovery already attempted but failed. Proceeding to fatal error.")); - } else if (!canAttemptRecovery) - { Serial.println(F("Error occurred during non-recoverable state. Proceeding to fatal error.")); - } #endif - indicateFatalError(reason); // Halt the system + indicateFatalError(reason); } -/** - * @brief Enters fatal error state. Prints diagnostics (if Serial enabled), disables outputs, - * and halts execution with fast blinking red LED. - * @param message A pointer to a flash string (F() macro) describing the fatal error. - */ -void indicateFatalError(const __FlashStringHelper *message) +void indicateFatalError(const __FlashStringHelper *message) // Modified for no LEDs { -// Print diagnostics if Serial is enabled #if defined(ENABLE_DEBUG_PRINTING) || defined(ENABLE_TELEMETRY) Serial.println(F("\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")); Serial.println(F(" >>> FATAL SYSTEM ERROR <<<")); @@ -1571,76 +1170,41 @@ void indicateFatalError(const __FlashStringHelper *message) Serial.print(stepperX.currentPosition()); Serial.print(F(" Position Y: ")); Serial.println(stepperY.currentPosition()); - // Print Sensor/Limit states (use cached debounced values) - Serial.print(F("Sensors [LRCUD]: ")); - Serial.print(sensorActive[SENSOR_L] ? '1' : '0'); - Serial.print(sensorActive[SENSOR_R] ? '1' : '0'); - Serial.print(sensorActive[SENSOR_C] ? '1' : '0'); - Serial.print(sensorActive[SENSOR_U] ? '1' : '0'); - Serial.println(sensorActive[SENSOR_D] ? '1' : '0'); - Serial.print(F("Limits [X Y]: ")); + Serial.print(F("Photoresistors Raw [L R U D C]: ")); + for (int i = 0; i < NUM_SENSORS; i++) + { + Serial.print(sensorReadings[i]); + Serial.print(i == NUM_SENSORS - 1 ? "" : " "); + } + Serial.println(); + Serial.print(F("Limits Active [X Y]: ")); Serial.print(limitSwitchActiveX ? '1' : '0'); Serial.print(" "); Serial.println(limitSwitchActiveY ? '1' : '0'); Serial.println(F("System Halted. Reset required.")); Serial.println(F("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n")); - Serial.flush(); // Attempt to ensure all messages are sent before halting + Serial.flush(); #endif - - // --- Critical Halt Sequence --- - // 1. Disable interrupts to prevent further AccelStepper processing or other ISRs interfering. - noInterrupts(); // cli() - - // 2. Set final state variable (prevents potential recursion if error occurs during stop) + noInterrupts(); currentState = State::ERROR; - - // 3. Attempt to disable stepper outputs directly (might not fully stop immediately but cuts power) stepperX.disableOutputs(); stepperY.disableOutputs(); - - // 4. Set LED to indicate fatal error (fast blink red = Lost LED) - // Manually control LED here as loop() is stopped. - // Ensure LED pin is output (already done in setup, but safe to redo DDR) - DDRC |= ledMaskLost; // Make sure Lost LED pin is output - - // 5. Infinite loop to halt CPU, manually blinking the error LED. while (true) { - PORTC |= ledMaskLost; // Turn ON Lost LED (PC3) - // FIX: Use a value that fits unsigned int for delayMicroseconds to avoid overflow warning - delayMicroseconds(50000); // 50ms ON - PORTC &= ~ledMaskLost; // Turn OFF Lost LED - delayMicroseconds(50000); // 50ms OFF (100ms total period) + // Halt CPU. No LEDs to blink. + delay(1000); // Simple delay in halt loop } } -/** - * @brief Safely calculates the difference between two millis() values, handling rollover. - * @param current The current millis() value. - * @param previous The previous millis() value. - * @return The difference (current - previous), correctly handling potential rollover. - */ -inline unsigned long safeMillisSubtract(unsigned long current, unsigned long previous) +inline unsigned long safeMillisSubtract(unsigned long current, unsigned long previous) // Logic remains same { if (current >= previous) - { return current - previous; - } else - { - // Rollover occurred return (ULONG_MAX - previous) + current + 1; - } } -/** - * @brief Clamps a long value to be within a specified minimum and maximum range. - * @param val The value to clamp. - * @param minVal The minimum allowed value. - * @param maxVal The maximum allowed value. - * @return The clamped value. - */ -inline long clamp(long val, long minVal, long maxVal) +inline long clamp(long val, long minVal, long maxVal) // Logic remains same { if (val < minVal) return minVal; @@ -1649,14 +1213,7 @@ inline long clamp(long val, long minVal, long maxVal) return val; } -/** - * @brief Sets the maximum speed and acceleration for both stepper motors. - * @param speedX Max speed for X axis (steps/sec). - * @param accelX Acceleration for X axis (steps/sec^2). - * @param speedY Max speed for Y axis (steps/sec). - * @param accelY Acceleration for Y axis (steps/sec^2). - */ -void setMotorSpeeds(float speedX, float accelX, float speedY, float accelY) +void setMotorSpeeds(float speedX, float accelX, float speedY, float accelY) // Logic remains same { stepperX.setMaxSpeed(speedX); stepperX.setAcceleration(accelX); @@ -1664,22 +1221,13 @@ void setMotorSpeeds(float speedX, float accelX, float speedY, float accelY) stepperY.setAcceleration(accelY); } -/** - * @brief Helper function to retrieve state name string from PROGMEM. - * @param state The State enum value. - * @return Pointer to the state name string in the global buffer `stateNameBuffer`. - */ -const char *getStateName(State state) +const char *getStateName(State state) // Logic remains same { - // Copy the string from PROGMEM into the buffer strcpy_P(stateNameBuffer, (char *)pgm_read_word(&(stateNames[static_cast(state)]))); return stateNameBuffer; } -/** - * @brief Prints periodic telemetry data to the Serial Monitor (if enabled). - */ -void printTelemetry() +void printTelemetry() // Updated for photoresistor active state { #ifdef ENABLE_TELEMETRY Serial.print(F("TLM: St:")); @@ -1689,23 +1237,14 @@ void printTelemetry() Serial.print(F(", Y:")); Serial.print(stepperY.currentPosition()); Serial.print(F("]")); - Serial.print(F(" S:[L")); - Serial.print(sensorActive[SENSOR_L] ? '1' : '0'); - Serial.print(F("R")); + Serial.print(F(" PhRS Actv[LRCUD]: ")); // PhotoResistor Active + Serial.print(sensorActive[SENSOR_L] ? '1' : '0'); // Using the boolean sensorActive array Serial.print(sensorActive[SENSOR_R] ? '1' : '0'); - Serial.print(F("U")); + Serial.print(sensorActive[SENSOR_C] ? '1' : '0'); // Note: C is in middle of L R U D for this print Serial.print(sensorActive[SENSOR_U] ? '1' : '0'); - Serial.print(F("D")); Serial.print(sensorActive[SENSOR_D] ? '1' : '0'); - Serial.print(F("C")); - Serial.print(sensorActive[SENSOR_C] ? '1' : '0'); - Serial.print(F("]")); - // Optional: Add DtG (Distance to Go) if useful for debugging movement states - // if (stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0) { - // Serial.print(F(" DtG:[X:")); Serial.print(stepperX.distanceToGo()); - // Serial.print(F(", Y:")); Serial.print(stepperY.distanceToGo()); Serial.print(F("]")); - // } + Serial.print(F("] PhRS Raw[C]:")); + Serial.print(sensorReadings[SENSOR_C]); // Example raw value Serial.println(); -// No Serial.flush() needed here, let buffer handle periodic output. -#endif // ENABLE_TELEMETRY +#endif } \ No newline at end of file