diff --git a/Arduino-Satellite-Dish/src/README b/Arduino-Satellite-Dish/src/README
new file mode 100644
index 0000000..6683caf
--- /dev/null
+++ b/Arduino-Satellite-Dish/src/README
@@ -0,0 +1,433 @@
+# Arduino IR Tracker with Stepper Motors (Optimized)
+
+**Version:** 2.2.2 (Production Ready Candidate)
+
+[](https://opensource.org/licenses/MIT)
+
+**CRITICAL WARNINGS:**
+
+> **1. HARDWARE VERIFICATION:** Failure to verify **ALL** pin assignments in the code's `SYSTEM CONFIGURATION & TUNING` section against your physical hardware **WILL** lead to malfunction or system halt. Default pins are provided but **MUST** be confirmed.
+>
+> **2. I2C PIN CONFLICT:** The default pins for the limit switches (`limitSwitchPinX = A4`, `limitSwitchPinY = A5`) directly conflict with the Arduino Uno's I2C pins (SDA/SCL). If you plan to use **ANY** I2C devices (displays, sensors, etc.) with this project, you **MUST** change `limitSwitchPinX` and `limitSwitchPinY` to other unused digital or analog pins in the configuration block.
+
+---
+
+## Table of Contents
+
+1. [Overview](#overview)
+2. [Features](#features)
+3. [Hardware Requirements](#hardware-requirements)
+4. [Wiring Instructions](#wiring-instructions)
+ - [Stepper Motors & Drivers](#stepper-motors--drivers)
+ - [IR Sensors](#ir-sensors)
+ - [Limit Switches](#limit-switches)
+ - [Status LEDs](#status-leds)
+ - [Power Supply](#power-supply)
+5. [Software Setup](#software-setup)
+ - [Dependencies](#dependencies)
+ - [Flashing](#flashing)
+6. [Code Configuration](#code-configuration)
+ - [Build Mode](#build-mode)
+ - [Pin Definitions](#pin-definitions)
+ - [Motor & Movement Parameters](#motor--movement-parameters)
+ - [Tracking Control (P-Controller)](#tracking-control-p-controller)
+ - [Local Search Parameters](#local-search-parameters)
+ - [Timing Parameters](#timing-parameters)
+ - [Stall Detection Parameters](#stall-detection-parameters)
+7. [Software Overview](#software-overview)
+ - [Core Loop](#core-loop)
+ - [State Machine](#state-machine)
+ - [Key Algorithms](#key-algorithms)
+ - [Optimizations](#optimizations)
+ - [Coordinate System](#coordinate-system)
+8. [**CRITICAL: Tuning Guide**](#critical-tuning-guide)
+ - [Methodology](#methodology)
+ - [Tuning Order](#tuning-order)
+ - [1. Speeds & Acceleration](#1-speeds--acceleration)
+ - [2. Proportional Control (Kp & Deadband)](#2-proportional-control-kp--deadband)
+ - [3. Stall Detection](#3-stall-detection)
+ - [4. Timings & Local Search](#4-timings--local-search)
+9. [Troubleshooting](#troubleshooting)
+10. [Future Improvements](#future-improvements)
+11. [License](#license)
+
+---
+
+## Overview
+
+This project implements firmware for an Arduino Uno to control a 2-axis (Pan/Tilt) platform equipped with stepper motors. Its primary goal is to automatically track a moving infrared (IR) light source using an array of five IR sensors (Left, Right, Up, Down, Center).
+
+The system performs automatic homing using limit switches, searches for the IR signal using sweep patterns, tracks the signal using Proportional (P) control logic, and attempts to re-acquire the signal using a local search pattern if it's lost. The code is optimized for performance on the resource-constrained Arduino Uno, utilizing direct port I/O for faster input reading and LED updates, and fixed-point arithmetic for the control calculations.
+
+This firmware is designed for platforms using 28BYJ-48 stepper motors driven by ULN2003 driver boards and common active-LOW IR receiver modules (like TSOP series).
+
+---
+
+## Features
+
+- **2-Axis Stepper Control:** Drives two stepper motors (e.g., 28BYJ-48) via ULN2003 drivers using the `AccelStepper` library for smooth acceleration and non-blocking operation.
+- **Automatic Homing:** Uses limit switches on both axes to establish a known starting position (0,0).
+- **IR Signal Tracking:** Employs 5 active-LOW IR sensors (L, R, U, D, C) to detect the direction of an IR source.
+- **Proportional (P) Control:** Uses fixed-point P-control logic to smoothly move the platform towards the detected IR signal for tracking.
+- **Sweep Search:** Performs a configurable wide-angle sweep pattern across both axes if the signal is not initially detected.
+- **Local Search:** Initiates a multi-phase local search pattern (probe -> perpendicular sweep -> expanding box) if the signal is lost during tracking, attempting re-acquisition.
+- **Stall Detection:** Includes a safety feature to detect if a motor is commanded to move but fails to do so (indicating a stall or obstruction), triggering an error state.
+- **State Machine Logic:** Organizes functionality into distinct states (Homing, Sweeping, Centering, Tracking, Local Search, Error, etc.) for robust control flow.
+- **Status LEDs:** Provides visual feedback on the current system state (Searching, Tracking, Lost/Error) via LEDs.
+- **Performance Optimizations:** Utilizes direct port manipulation for faster I/O and fixed-point math for control calculations, crucial for Arduino Uno performance.
+- **Highly Configurable:** Allows tuning of speeds, acceleration, P-control gains, deadbands, search patterns, timings, and pin assignments via constants in the code.
+- **Debug/Production Builds:** Supports conditional compilation to enable/disable Serial output for debugging or maximum performance.
+
+---
+
+## Hardware Requirements
+
+1. **Microcontroller:** Arduino Uno R3 (or compatible ATmega328P-based board).
+2. **Stepper Motors:** 2 x 28BYJ-48 5V Stepper Motors (or similar, ensure `stepsPerRevolution` is adjusted if different).
+3. **Stepper Motor Drivers:** 2 x ULN2003 Driver Boards (or equivalent darlington array driver).
+4. **IR Sensors:** 5 x Active-LOW IR Receiver Modules (e.g., TSOP38238, TSOP4838, VS1838B). Must output LOW when IR is detected. Ensure they operate at the correct voltage (usually 3.3V or 5V).
+5. **Limit Switches:** 2 x Mechanical Limit Switches (Microswitches preferred). Configuration assumes Normally Open (NO) wiring connected to GND and Signal pins (Active LOW when pressed).
+6. **Status LEDs:** 3 x Standard LEDs (e.g., Red, Yellow, Green) with appropriate current-limiting resistors (e.g., 220-330 Ohm for 5V).
+7. **Power Supply:**
+ - **CRITICAL:** A stable **external 5V power supply** capable of providing sufficient current for **both** stepper motors running simultaneously (e.g., 1A-2A minimum, depending on motor draw). **Do not** attempt to power the motors directly from the Arduino's 5V pin or USB, as this will cause instability and potentially damage the Arduino.
+ - The Arduino itself can be powered via USB or a separate 7-12V supply into the VIN pin/barrel jack.
+8. **Jumper Wires & Breadboard (Optional):** For connections.
+9. **Pan/Tilt Mechanism:** A physical structure to mount the motors, sensors, and Arduino.
+
+---
+
+## Wiring Instructions
+
+**IMPORTANT:** Double-check all connections before applying power. Refer to the `Pin Definitions` section in the code configuration and verify these defaults match your wiring.
+
+### Stepper Motors & Drivers
+
+- Connect the 28BYJ-48 motor connector to the output connector on its corresponding ULN2003 board.
+- Connect the ULN2003 board's VCC/+ and GND/- pins to your **external 5V power supply**.
+- Connect the ULN2003 board's IN1, IN2, IN3, IN4 pins to the Arduino pins defined for the respective motor (X or Y).
+ - **X-Axis Motor (Defaults):**
+ - ULN2003 IN1 -> Arduino D8 (`stepperX_pin1`)
+ - ULN2003 IN2 -> Arduino D10 (`stepperX_pin2`)
+ - ULN2003 IN3 -> Arduino D9 (`stepperX_pin3`)
+ - ULN2003 IN4 -> Arduino D11 (`stepperX_pin4`)
+ - _(Note the AccelStepper library sequence P1,P3,P2,P4)_
+ - **Y-Axis Motor (Defaults):**
+ - ULN2003 IN1 -> Arduino D4 (`stepperY_pin1`)
+ - ULN2003 IN2 -> Arduino D6 (`stepperY_pin2`)
+ - ULN2003 IN3 -> Arduino D5 (`stepperY_pin3`)
+ - ULN2003 IN4 -> Arduino D7 (`stepperY_pin4`)
+ - _(Note the AccelStepper library sequence P1,P3,P2,P4)_
+
+### IR Sensors
+
+- Connect the VCC pin of each sensor to Arduino 5V (or 3.3V if required by the sensor module).
+- Connect the GND pin of each sensor to Arduino GND.
+- Connect the OUT/Signal pin of each sensor to the corresponding Arduino digital input pin:
+ - Left Sensor OUT -> Arduino D2 (`sensorPinL`)
+ - Right Sensor OUT -> Arduino D3 (`sensorPinR`)
+ - Up Sensor OUT -> Arduino D12 (`sensorPinU`)
+ - Down Sensor OUT -> Arduino D13 (`sensorPinD`)
+ - Center Sensor OUT -> Arduino A0 (`sensorPinC`)
+
+### Limit Switches
+
+- The code assumes Active LOW switches connected between the Arduino pin and GND.
+- Connect one terminal of the switch (usually COM - Common) to Arduino GND.
+- Connect the **Normally Open (NO)** terminal of the switch to the corresponding Arduino digital input pin. (The internal pull-up resistor on the Arduino pin will keep it HIGH until the switch is pressed, pulling it LOW).
+ - X-Axis Limit Switch (NO) -> Arduino A4 (`limitSwitchPinX`) **(!!! I2C CONFLICT !!!)**
+ - Y-Axis Limit Switch (NO) -> Arduino A5 (`limitSwitchPinY`) **(!!! I2C CONFLICT !!!)**
+ - _(If using Normally Closed (NC) contacts, you'll need to connect COM to 5V, NC to the pin, and potentially disable the internal pull-up and invert the logic in `readInputsOptimized`)_.
+
+### Status LEDs
+
+- Connect the **Anode** (longer lead) of each LED to the corresponding Arduino digital output pin _through_ its current-limiting resistor (e.g., 220 Ohm).
+- Connect the **Cathode** (shorter lead, flat side) of each LED to Arduino GND.
+ - Searching LED Anode (+ resistor) -> Arduino A1 (`ledPinSearching`)
+ - Tracking LED Anode (+ resistor) -> Arduino A2 (`ledPinTracking`)
+ - Lost/Error LED Anode (+ resistor) -> Arduino A3 (`ledPinLost`)
+
+### Power Supply
+
+- Connect the **External 5V Power Supply Positive (+)** output to the VCC/+ pins of **both** ULN2003 driver boards.
+- Connect the **External 5V Power Supply Ground (-)** output to the GND/- pins of **both** ULN2003 driver boards **AND** to one of the Arduino GND pins. **This common ground connection is essential!**
+- Power the Arduino board itself via its USB port or a separate 7-12V supply connected to the VIN pin or barrel jack.
+
+---
+
+## Software Setup
+
+### Dependencies
+
+1. **Arduino IDE** or **PlatformIO:** Install your preferred development environment.
+2. **AccelStepper Library:** This library is required.
+ - **Arduino IDE:** Go to `Sketch` -> `Include Library` -> `Manage Libraries...`. Search for "AccelStepper" by Mike McCauley and install it.
+ - **PlatformIO:** Add `AccelStepper` to the `lib_deps` list in your `platformio.ini` file (e.g., `lib_deps = mikemccauley/AccelStepper @ ^1.61`). PlatformIO will usually install it automatically on build.
+
+### Flashing
+
+1. **Download/Clone:** Get the project code (`IR_Tracker_Optimized_ProdReady.ino` or similar).
+2. **Configure:** **CRITICALLY REVIEW AND EDIT** the parameters in the `SYSTEM CONFIGURATION & TUNING` section of the code, especially the `Pin Definitions`, to match your exact hardware wiring. Resolve the I2C conflict if necessary.
+3. **Open:** Load the `.ino` file in your Arduino IDE or open the project folder in PlatformIO.
+4. **Select Board & Port:**
+ - **Arduino IDE:** Go to `Tools` -> `Board` and select "Arduino Uno". Go to `Tools` -> `Port` and select the correct serial port for your connected Arduino.
+ - **PlatformIO:** Ensure your `platformio.ini` specifies `board = uno`. PlatformIO usually auto-detects the port.
+5. **Upload:**
+ - **Arduino IDE:** Click the "Upload" button (right arrow icon).
+ - **PlatformIO:** Use the "Upload" task (e.g., click the alien head icon in the VS Code status bar and select `PlatformIO: Upload`).
+6. **Monitor (Optional):** If `DEBUG_BUILD` is defined in the code, open the Serial Monitor (Arduino IDE: `Tools` -> `Serial Monitor`; PlatformIO: `PlatformIO: Serial Monitor` task) and set the baud rate to **115200**. You should see startup messages and telemetry data.
+
+---
+
+## Code Configuration
+
+All user-configurable parameters are located at the top of the `.ino` file within the `SYSTEM CONFIGURATION & TUNING` section. **Tuning these values is essential for proper operation.**
+
+### Build Mode
+
+- `#define DEBUG_BUILD`: Uncomment this line to enable detailed `Serial` output for debugging, state changes, and telemetry. Comment it out for a production build (disables most `Serial` calls for maximum performance and reduced code size).
+
+### Pin Definitions
+
+- `stepperX_pin1`, `stepperX_pin3`, `stepperX_pin2`, `stepperX_pin4`: Arduino pins connected to the X-axis ULN2003 driver (IN1, IN3, IN2, IN4 respectively due to AccelStepper).
+- `stepperY_pin1`, `stepperY_pin3`, `stepperY_pin2`, `stepperY_pin4`: Arduino pins connected to the Y-axis ULN2003 driver.
+- `sensorPinL`, `sensorPinR`, `sensorPinU`, `sensorPinD`, `sensorPinC`: Arduino pins connected to the respective IR sensor output pins.
+- `limitSwitchPinX`, `limitSwitchPinY`: Arduino pins connected to the limit switches. **WARNING: Defaults A4/A5 conflict with I2C!**
+- `ledPinSearching`, `ledPinTracking`, `ledPinLost`: Arduino pins connected to the status LEDs (via resistors).
+
+### Motor & Movement Parameters
+
+- `stepsPerRevolution`: Total steps for one full 360° motor rotation (2038 for 28BYJ-48 in half-step mode).
+- `homingSpeed`, `homingAcceleration`: Speed/acceleration during the homing sequence. Must be reliable enough to trigger switches without stalling.
+- `sweepSpeedDefault`, `sweepAccelerationDefault`: Speed/acceleration during the wide-angle sweep search. Can be faster than homing/tracking if the motors handle it.
+- `trackSpeedDefault`, `trackAccelerationDefault`: Base speed/acceleration used during centering and tracking adjustments. Higher values react faster but may overshoot.
+- `localSearchSpeed`, `localSearchAccel`: Speed/acceleration during the local search patterns. Often slower for more precise small movements.
+- `searchRangeX_deg`, `searchRangeY_deg`: Maximum range (in degrees) the system will sweep during the initial search.
+- `postHomingMoveOffSwitchSteps`: Small number of steps to move away from the limit switch immediately after it's triggered during homing. Prevents sitting directly on the switch.
+- `HOMING_OVERSHOOT_MULTIPLIER`: Used internally to calculate the initial large move distance during homing to ensure the switch is definitely hit.
+
+### Tracking Control (P-Controller)
+
+- `Kp_X_float`, `Kp_Y_float`: **CRITICAL TUNING PARAMETERS.** Proportional gain for each axis. Determines how strongly the system reacts to tracking errors. Start low (e.g., 5.0-10.0) and increase carefully. Too high causes oscillation/overshoot; too low causes sluggish response/loss of target.
+- `trackingDeadband_steps`: **CRITICAL TUNING PARAMETER.** A zone (in steps) around the target position where small errors are ignored. Prevents jitter/hunting when centered. Too high makes centering less precise; too low causes constant small adjustments.
+
+### Local Search Parameters
+
+- `localSearchProbeSteps`: Initial distance (steps) to move in the last known direction of the target when signal is lost. Tune based on sensor beam width and expected target speed.
+- `localSearchSweepSteps`: Distance (steps) for the perpendicular sweeps in the local search cross pattern.
+- `localSearchBoxStepSize`: Step size for each leg of the expanding box pattern.
+- `localSearchMaxCycles`: Number of full box layers to attempt before giving up local search and reverting to a full sweep.
+
+### Timing Parameters
+
+- `debounceDelayMs`, `limitSwitchDebounceMs`: Software debounce time for sensors and switches to filter out noise/bouncing. Increase if inputs seem flaky.
+- `signalLostTimeoutMs`: Duration (ms) the system will wait in TRACKING state without any sensor input before declaring the signal lost and entering LOCAL_SEARCH.
+- `searchFailDelayMs`: Delay (ms) before retrying a full sweep after completing one without finding the signal (`SEARCH_FAILED` state).
+- `stateTimeoutMs`, `homingTimeoutMs`, `localSearchTimeoutMs`: Safety timeouts (ms) for specific states to prevent the system from getting stuck indefinitely. Triggers an error if exceeded.
+- `telemetryIntervalMs`: How often (ms) telemetry data is printed to Serial (if `ENABLE_TELEMETRY` is active).
+- `postHomingPauseMs`: Brief pause (ms) after successful homing before starting the first search sweep.
+
+### Stall Detection Parameters
+
+- `stallCheckIntervalMs`: How frequently (ms) the code checks for potential motor stalls.
+- `stallTimeoutThresholdMs`: Duration (ms) a motor must appear stuck (not moving despite being commanded) before a stall error is triggered.
+- `stallPositionTolerance`: Minimum number of steps the motor position must change between checks to be considered _not_ stalled. Increase slightly if getting false positives during slow moves or vibration.
+
+---
+
+## Software Overview
+
+### Core Loop
+
+The `loop()` function performs the essential tasks repeatedly:
+
+1. **`stepperX.run()` / `stepperY.run()`:** Calls the AccelStepper library functions to manage motor stepping, acceleration, and non-blocking movement based on current targets. **This is critical and must be called frequently.**
+2. **`readInputsOptimized()`:** Reads the state of all IR sensors and limit switches using direct port I/O and applies software debouncing. Updates global state variables (`sensorActive[]`, `limitSwitchActiveX/Y`).
+3. **Update `lastSignalTime`:** If in TRACKING or CENTERING state and a sensor is active, updates the timestamp of the last known signal detection.
+4. **`handleStateMachine()`:** The core logic router. Based on the `currentState`, it calls the appropriate function or performs actions for that state (e.g., `handleHomingStates`, `handleTrackingLogicFixedPoint`, `handleLocalSearch`).
+5. **`updateStatusLEDsOptimized()`:** Sets the status LEDs according to the `currentState` using direct port I/O.
+6. **`checkMotorStall()`:** Periodically checks if motors appear stalled.
+7. **`checkStateTimeouts()`:** Checks if certain states have run longer than their allowed safety timeout.
+8. **`printTelemetry()`:** (Conditional) Prints status information to the Serial Monitor at regular intervals if `ENABLE_TELEMETRY` is defined.
+
+### State Machine
+
+The system operates based on a finite state machine (`enum class State`). This ensures predictable behavior and handles different operational modes cleanly. The primary states and transitions are:
+
+- **INITIALIZING -> CHECK_PIN_CONFLICTS:** Initial power-up state.
+- **CHECK_PIN_CONFLICTS -> HOMING_X:** After setup and pin checks.
+- **HOMING_X -> POST_HOMING_MOVE_OFF_X -> HOMING_Y -> POST_HOMING_MOVE_OFF_Y -> POST_HOMING_DELAY:** Sequence to home both axes using limit switches.
+- **POST_HOMING_DELAY -> SWEEP_X:** Homing complete, start searching.
+- **SWEEP_X:** Sweeps the X-axis back and forth.
+ - If signal found -> **CENTERING**
+ - If full X sweep complete (no signal) -> **SWEEP_Y**
+- **SWEEP_Y:** Sweeps the Y-axis back and forth.
+ - If signal found -> **CENTERING**
+ - If full Y sweep complete (no signal) -> **SEARCH_FAILED**
+- **CENTERING:** Signal detected, performing fine adjustments to center the target using tracking logic.
+ - If move completes and signal still present -> **TRACKING**
+ - If move completes and signal lost -> **LOCAL_SEARCH**
+- **TRACKING:** Continuously adjusts position to keep the signal centered using tracking logic.
+ - If signal lost (timeout) -> **LOCAL_SEARCH**
+- **LOCAL_SEARCH:** Signal lost during tracking, executing probe/sweep/box patterns to re-acquire.
+ - If signal re-acquired -> **CENTERING**
+ - If local search times out or exhausts patterns -> **SWEEP_X** (restart full search)
+- **SEARCH_FAILED:** Full sweep completed without finding signal. Pauses, then restarts search -> **SWEEP_X**.
+- **(Any State) -> ERROR:** Triggered by fatal errors (stall detected, safety timeout, config error). System halts.
+
+### Key Algorithms
+
+- **Fixed-Point P-Control (`handleTrackingLogicFixedPoint`)**: Calculates an error value based on which IR sensors are active. This error (represented as a small integer) is multiplied by a pre-scaled gain (`Kp_X/Y_scaled`) using integer math. The result is scaled back down (using bit-shifting) to determine the number of steps to move. A deadband ignores very small calculated moves.
+- **Local Search (`handleLocalSearch`)**: A multi-phase strategy:
+ 1. _Probe:_ Moves a short distance in the direction the target was last seen.
+ 2. _Sweep:_ Performs a cross-pattern sweep perpendicular to the last known direction.
+ 3. _Box:_ If the above fail, executes an expanding square spiral pattern.
+- **Direct Port I/O (`readInputsOptimized`, `updateStatusLEDsOptimized`)**: Reads multiple input pins or writes multiple output pins belonging to the same AVR port (PORTB, PORTC, PORTD) in a single operation using bit masks, significantly faster than multiple `digitalRead`/`digitalWrite` calls.
+
+### Optimizations
+
+- **Non-Blocking Stepper Control:** `AccelStepper` library handles movement without blocking the main loop.
+- **Direct Port I/O:** Faster reading of sensors/switches and writing to LEDs.
+- **Fixed-Point Math:** Avoids slow floating-point calculations in the time-critical tracking loop.
+- **Conditional Serial Output:** `Serial` printing is computationally expensive; disabling it via `DEBUG_BUILD` provides a significant performance boost.
+- **`F()` Macro / PROGMEM:** Stores constant strings in Flash memory instead of RAM.
+- **Optimized Data Types:** Uses smallest appropriate data types (`uint8_t`, etc.) where possible.
+- **Inlining:** Suggests inlining for small utility functions.
+
+### Coordinate System
+
+- **Origin (0, 0):** Defined by the limit switches during homing. Typically the bottom-left or top-left corner depending on switch placement and motor direction.
+- **X-Axis:** Positive steps generally move the platform Right. Negative steps move Left.
+- **Y-Axis:** Positive steps generally move the platform Down. Negative steps move Up.
+ _(Note: This depends on motor wiring and mounting. If movement is inverted, you might need to reverse the motor direction logic or simply invert the sign in the P-control calculations if that's easier.)_
+
+---
+
+## **CRITICAL: Tuning Guide**
+
+**This system WILL NOT WORK correctly or reliably without careful tuning.** Default values are starting points ONLY and are unlikely to be optimal for your specific hardware (motors, power supply, mechanism friction, sensor spacing, IR source characteristics).
+
+### Methodology
+
+1. **Patience:** Tuning takes time and observation.
+2. **One Parameter at a Time:** Change only _one_ value, upload, test its effect, then decide on the next change. Changing multiple values makes it impossible to know what caused an improvement or regression.
+3. **Observe & Listen:** Watch the physical movement. Is it smooth? Jerky? Does it overshoot? Listen to the motors. Do they sound strained or skip steps (clicking noise)?
+4. **Use Serial Monitor (with `DEBUG_BUILD`):** Enable `DEBUG_BUILD` during tuning. The telemetry output (`TLM: ...`) provides valuable information about the current state, position, and sensor readings.
+5. **Start Conservatively:** Begin with lower speeds, accelerations, and Kp values, then gradually increase them.
+
+### Tuning Order
+
+Follow this general order for best results:
+
+1. **Speeds & Acceleration:** Find the physical limits of your motors and mechanism.
+2. **Proportional Control (Kp & Deadband):** Tune the tracking responsiveness and stability. This is the most critical part for tracking performance.
+3. **Stall Detection:** Fine-tune the safety net once movement is generally reliable.
+4. **Timings & Local Search:** Adjust timeouts and search patterns based on observed performance.
+
+### 1. Speeds & Acceleration
+
+- **Goal:** Find the highest `...Speed...` and `...Acceleration...` values your motors can reliably handle without stalling (buzzing without turning) or skipping steps under load.
+- **Parameters:** `homingSpeed`, `homingAcceleration`, `sweepSpeedDefault`, `sweepAccelerationDefault`, `trackSpeedDefault`, `trackAccelerationDefault`, `localSearchSpeed`, `localSearchAccel`.
+- **Testing:**
+ - Start with low values (e.g., Speed=300, Accel=300).
+ - Trigger a long move (like the initial sweep after homing). Listen carefully.
+ - Gradually increase Speed first. If it stalls, reduce speed and try increasing Acceleration. Sometimes higher acceleration helps overcome initial friction, but too high also causes stalls.
+ - Find the maximum reliable values for sweep/homing first. `trackSpeed` can often be slightly lower for smoother fine adjustments. `localSearchSpeed` is usually slower still.
+- **Symptoms:**
+ - _Too High:_ Motors stall, buzz, vibrate heavily, skip steps (lose position accuracy).
+ - _Too Low:_ Movement is unnecessarily slow.
+
+### 2. Proportional Control (Kp & Deadband)
+
+- **Goal:** Achieve smooth, responsive tracking that centers accurately without oscillation (shaking) or excessive jitter.
+- **Parameters:** `Kp_X_float`, `Kp_Y_float`, `trackingDeadband_steps`.
+- **Testing:**
+ - Get the system into the `TRACKING` state (have it find and lock onto the IR source).
+ - Slowly move the IR source side-to-side and up-and-down across the sensors.
+ - Observe how the platform reacts.
+ - **Start with `Kp` LOW (e.g., 5.0-10.0)** and `trackingDeadband_steps` relatively low (e.g., 2-5).
+- **Symptoms & Adjustments:**
+ - **Oscillation/Shaking:** Target bounces back and forth past the center. -> **Decrease `Kp_float`** for the relevant axis.
+ - **Overshooting:** Moves quickly past the center, then corrects back. -> **Decrease `Kp_float`**. Possibly increase `trackAccelerationDefault` if the overshoot happens _during_ deceleration (less common).
+ - **Sluggish/Slow Response:** Takes a long time to center, might lose target easily if it moves quickly. -> **Increase `Kp_float`** slowly. Ensure `trackSpeedDefault` is adequate.
+ - **Jitter/Hunting when Centered:** Constantly making tiny movements back and forth even when the target seems centered. -> **Increase `trackingDeadband_steps`**. `Kp_float` might also be slightly too high.
+ - **Large Dead Zone:** Target has to move significantly off-center before the platform reacts. -> **Decrease `trackingDeadband_steps`**.
+- **Note:** Kp and Deadband interact. You may need to adjust both iteratively. Tuning Kp is usually the primary focus.
+
+### 3. Stall Detection
+
+- **Goal:** Reliably detect genuine motor stalls without triggering falsely during normal, possibly slow or jerky, operation.
+- **Parameters:** `stallCheckIntervalMs`, `stallTimeoutThresholdMs`, `stallPositionTolerance`.
+- **Testing:**
+ - Operate the system normally after tuning speeds and P-control.
+ - If you get unexpected "Stall Detected" errors during normal moves:
+ - Try increasing `stallTimeoutThresholdMs` (gives it more time to recover).
+ - Try increasing `stallPositionTolerance` slightly (allows for more vibration/tiny movements).
+ - Less likely, slightly increase `stallCheckIntervalMs`.
+ - If you physically stall a motor (carefully!) and it's _not_ detected:
+ - Decrease `stallTimeoutThresholdMs`.
+ - Ensure `stallPositionTolerance` is not too high.
+ - Verify your `...Speed...` and `...Acceleration...` values aren't simply too high, causing the stall in the first place (see Step 1).
+- **Note:** This is a safety net. Frequent stalls usually mean speeds/accel are too high or there's a mechanical issue.
+
+### 4. Timings & Local Search
+
+- **Goal:** Optimize how the system handles signal loss and searching based on your environment and IR source.
+- **Parameters:** `signalLostTimeoutMs`, `localSearch...` parameters, `searchFailDelayMs`.
+- **Testing:**
+ - Observe behavior when the IR signal is temporarily blocked or moves out of range.
+ - If it enters `LOCAL_SEARCH` too quickly for brief dropouts -> Increase `signalLostTimeoutMs`.
+ - If `LOCAL_SEARCH` fails to re-acquire a nearby target -> Adjust `localSearchProbeSteps`, `localSearchSweepSteps`, `localSearchBoxStepSize`. Make steps larger if the target might have moved further; smaller for finer searching. Consider increasing `localSearchMaxCycles`.
+ - If `LOCAL_SEARCH` takes too long before giving up -> Decrease `localSearchTimeoutMs` or `localSearchMaxCycles`.
+ - Adjust `searchFailDelayMs` based on how long you want to wait before a full retry after a failed search.
+
+---
+
+## Troubleshooting
+
+- **No Serial Output:**
+ - Ensure `DEBUG_BUILD` is uncommented in the code.
+ - Verify Serial Monitor baud rate is set to 115200.
+ - Check Arduino connection, port selection in IDE/PlatformIO.
+ - Check Arduino power.
+- **System Doesn't Start / No LEDs:**
+ - Check Arduino power supply (USB or VIN).
+ - Check wiring, especially power and ground connections.
+- **Motors Buzz/Vibrate, Don't Turn:**
+ - **Insufficient Power:** External 5V supply cannot provide enough current for the motors. Use a higher current rating supply.
+ - **Wiring Error:** Double-check ULN2003 IN1-4 connections to Arduino pins match the code _exactly_. Order matters! Check motor connector is fully seated.
+ - **Speed/Acceleration Too High:** Lower the `...Speed...` and `...Acceleration...` values significantly and try again.
+ - **Mechanical Binding:** Ensure the platform moves freely by hand.
+- **Homing Fails:**
+ - **Switch Not Triggered:** Check limit switch wiring (GND to COM, Arduino Pin to NO). Check physical actuation. Decrease `homingSpeed` if moving too fast past switch. Increase `HOMING_OVERSHOOT_MULTIPLIER` move if it stops short. Check `homingTimeoutMs`.
+ - **Doesn't Move Off Switch:** Increase `postHomingMoveOffSwitchSteps`. Check switch isn't stuck. Verify switch logic is Active LOW in code (`readInputsOptimized`).
+- **Tracking Issues (Oscillation, Sluggish, Jitter):**
+ - See the **Proportional Control (Kp & Deadband)** tuning section above. This requires careful adjustment.
+- **False Stall Detection:**
+ - See the **Stall Detection** tuning section above. Adjust timeouts and tolerance. Check speeds/mechanics.
+- **Stall Not Detected:**
+ - See the **Stall Detection** tuning section. Decrease timeout. Verify speeds aren't too high.
+- **I2C Devices Not Working:**
+ - **PIN CONFLICT!** You are likely using limit switches on A4/A5. You **MUST** change the `limitSwitchPinX` and `limitSwitchPinY` definitions in the code to unused pins and rewire accordingly.
+- **Movement Direction Reversed:**
+ - Easiest fix: In `handleTrackingLogicFixedPoint`, invert the sign of the calculated `stepsToMoveX` or `stepsToMoveY` if only one axis is reversed. Alternatively, physically reverse the connection order for the problematic motor on the ULN2003 board (e.g., swap IN1<->IN4, IN2<->IN3) or reverse the motor connector itself if possible.
+
+---
+
+## Future Improvements
+
+- **Smoother Acceleration Profiles:** Implement S-curve acceleration instead of linear acceleration (AccelStepper might support this or require modification/alternative library).
+- **PID Control:** Replace the P-controller with a full PID (Proportional-Integral-Derivative) controller for potentially better handling of overshoot and steady-state error, though tuning becomes more complex.
+- **Adaptive Tuning:** Implement logic to adjust Kp or other parameters dynamically based on performance.
+- **Improved Sensor Filtering:** Use more advanced digital filtering techniques on sensor readings if noise is an issue.
+- **Web Interface/Remote Control:** Add an ESP8266 or ESP32 for Wi-Fi connectivity, allowing remote monitoring, control, and tuning via a web interface.
+- **Support for Different Hardware:** Abstract hardware interactions (pin definitions, sensor reading logic) to make porting to different microcontrollers or sensors easier.
+- **Obstacle Avoidance:** Integrate distance sensors to prevent collisions.
+- **Target Prediction:** Implement basic prediction algorithms (e.g., Kalman filter) if tracking fast-moving targets.
+
+---
+
+## License
+
+This project is licensed under the MIT License. See the [LICENSE](LICENSE) file (if included) or the link below for details.
+
+[https://opensource.org/licenses/MIT](https://opensource.org/licenses/MIT)
diff --git a/Arduino-Satellite-Dish/src/main.cpp b/Arduino-Satellite-Dish/src/main.cpp
index 109fb86..0666176 100644
--- a/Arduino-Satellite-Dish/src/main.cpp
+++ b/Arduino-Satellite-Dish/src/main.cpp
@@ -1,520 +1,1711 @@
-#include <Stepper.h>
+/**
+ * @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
+ *
+ * @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.
+ *
+ * @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.
+ */
+
#include <Arduino.h>
-#include <avr/wdt.h>
+#include <AccelStepper.h>
+#include <limits.h>
+#include <avr/pgmspace.h>
-//---------------------------------------------------------
-// Stepper Motor Configuration and Global Constants
-//---------------------------------------------------------
-const int stepsPerRevolution = 4096;
-const float stepsPerDegree = 4096.0 / 360.0; // More precise calculation
+// ==========================================================================
+// === SYSTEM CONFIGURATION & TUNING ===
+// ==========================================================================
+// --- System Version ---
+#define SYSTEM_VERSION "1.0"
-// Motor Enable Pins (adjust these if needed)
-const int ENABLE_X = 2; // Active LOW enable
-const int ENABLE_Y = 3;
+// --- Build Configuration ---
+// Comment out the next line for a production build (disables most Serial output for speed)
+#define DEBUG_BUILD
-// Light sensor pins
-const int LDR_TOP_LEFT = A0; // Top-left sensor
-const int LDR_TOP_RIGHT = A1; // Top-right sensor
-const int LDR_BOTTOM_LEFT = A2; // Bottom-left sensor
-const int LDR_BOTTOM_RIGHT = A3; // Bottom-right sensor
+// Derived Conditional Compilation
+#ifdef DEBUG_BUILD
+#define ENABLE_TELEMETRY // Enable periodic status messages via Serial
+#define ENABLE_DEBUG_PRINTING // Enable state change and event messages via Serial
+#else
+// Production build: Telemetry and Debug prints are disabled below
+#undef ENABLE_TELEMETRY
+#undef ENABLE_DEBUG_PRINTING
+#endif
-// Tracking parameters and thresholds
-const int LIGHT_THRESHOLD = 100; // Minimum change to trigger movement
-const int LIGHT_LOCK_THRESHOLD = 50; // When target is considered "locked on"
-const int MAX_SEARCH_ITERATIONS =
- 3; // Maximum number of search patterns before entering idle
-const int MIN_LIGHT_LEVEL = 200; // Minimum overall light to consider valid
+// --- 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)
-bool targetLocked = false;
+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)
-// Motor speed settings (in RPM, for Stepper.setSpeed)
-const int SEARCH_SPEED = 5; // Slower speed for search mode
-const int TRACK_SPEED = 10; // Faster speed for tracking mode
-const int MAX_STEP_SIZE = 5; // Maximum degree movement per adjustment
+// 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
-//---------------------------------------------------------
-// System state and global variables
-//---------------------------------------------------------
-enum SystemState
-{
+// 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)
+
+// 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)
+
+// --- 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)
+
+// 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)
+
+// --- 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.
+
+// 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.
+
+// --- 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
+
+// --- 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'
+
+// ==========================================================================
+// === 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
+
+// --- Sensor Indices ---
+#define SENSOR_L 0
+#define SENSOR_R 1
+#define SENSOR_U 2
+#define SENSOR_D 3
+#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
+
+// 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;
+
+// --- State Machine ---
+enum class State : uint8_t
+{ // Use enum class for stronger typing
INITIALIZING,
- SEARCHING,
+ CHECK_PIN_CONFLICTS,
+ HOMING_X,
+ HOMING_Y,
+ POST_HOMING_MOVE_OFF_X,
+ POST_HOMING_MOVE_OFF_Y,
+ POST_HOMING_DELAY,
+ SWEEP_X,
+ SWEEP_Y,
+ CENTERING,
TRACKING,
- IDLE
+ LOCAL_SEARCH,
+ SEARCH_FAILED,
+ ERROR // Fatal halt
};
+State currentState = State::INITIALIZING;
-SystemState currentState = INITIALIZING;
-unsigned long lastActionTime = 0;
-int searchIteration = 0;
+// 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";
+const char state3[] PROGMEM = "HOMING_Y";
+const char state4[] PROGMEM = "POST_HOMING_MOVE_OFF_X";
+const char state5[] PROGMEM = "POST_HOMING_MOVE_OFF_Y";
+const char state6[] PROGMEM = "POST_HOMING_DELAY";
+const char state7[] PROGMEM = "SWEEP_X";
+const char state8[] PROGMEM = "SWEEP_Y";
+const char state9[] PROGMEM = "CENTERING";
+const char state10[] PROGMEM = "TRACKING";
+const char state11[] PROGMEM = "LOCAL_SEARCH";
+const char state12[] PROGMEM = "SEARCH_FAILED";
+const char state13[] PROGMEM = "ERROR";
-// Calibration offsets for each light sensor
-int offsetTL = 0, offsetTR = 0, offsetBL = 0, offsetBR = 0;
+const char *const stateNames[] PROGMEM = { // Must match enum order
+ state0, state1, state2, state3, state4, state5, state6, state7,
+ state8, state9, state10, state11, state12, state13};
-//---------------------------------------------------------
-// Create stepper objects (wiring order may need to be verified)
-//---------------------------------------------------------
-Stepper stepperX(stepsPerRevolution, 8, 10, 9, 11);
-Stepper stepperY(stepsPerRevolution, 4, 5, 6, 7);
+// Buffer for reading state names from PROGMEM
+char stateNameBuffer[25]; // Sufficient size for longest state name + null terminator
-//---------------------------------------------------------
-// PID Control Variables for Tracking
-//---------------------------------------------------------
-float lastXError = 0, lastYError = 0;
-float xIntegral = 0, yIntegral = 0;
-const float kP = 0.2; // Proportional constant
-const float kI = 0.01; // Integral constant
-const float kD = 0.05; // Derivative constant
+// --- 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);
-//---------------------------------------------------------
-// Motor Power Management Functions
-//---------------------------------------------------------
-void enableMotors()
+// --- Global State Variables ---
+// Sensor & Limit Switch States (debounced)
+bool sensorActive[NUM_SENSORS] = {false};
+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;
+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 lastTelemetryTime = 0;
+
+// Search State Variables
+int8_t sweepDirectionX = 1; // 1 for positive, -1 for negative direction
+int8_t sweepDirectionY = 1;
+uint8_t sweepXCyclesCompleted = 0; // Counts legs (0, 1) of a full sweep cycle
+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
+
+// Stall Detection State
+long lastKnownStepperXPosForStall = 0;
+long lastKnownStepperYPosForStall = 0;
+unsigned long stallStartTimeX = 0; // Time potential stall started
+unsigned long stallStartTimeY = 0;
+unsigned long lastStallCheckTime = 0;
+
+// Error Handling State
+bool recoveryAttempted = false; // Flag to prevent infinite recovery loops
+
+// ==========================================================================
+// === FUNCTION PROTOTYPES ===
+// ==========================================================================
+// Setup & Initialization
+bool setupPinsAndCheckConflicts();
+void printStartupMessage();
+void printTuningGuide(); // Conditional print
+
+// Core Logic
+void readInputsOptimized();
+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
+void handleHomingStates();
+void handleTrackingLogicFixedPoint();
+void handleLocalSearch();
+void startSweepX();
+void startSweepY();
+
+// Safety & Monitoring
+void updateStatusLEDsOptimized();
+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();
+
+// ==========================================================================
+// === SETUP ===
+// ==========================================================================
+void setup()
{
- // For many motor drivers, setting the enable pin LOW activates the driver.
- digitalWrite(ENABLE_X, LOW);
- digitalWrite(ENABLE_Y, LOW);
+#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
+ changeState(State::CHECK_PIN_CONFLICTS);
}
-void disableMotors()
+// ==========================================================================
+// === MAIN LOOP ===
+// ==========================================================================
+void loop()
{
- // Setting enable to HIGH disables the motor drivers.
- digitalWrite(ENABLE_X, HIGH);
- digitalWrite(ENABLE_Y, HIGH);
-}
+ unsigned long now = millis(); // Cache current time for this loop iteration
-//---------------------------------------------------------
-// Movement Function with Motor Enable/Disable and Speed Management
-//---------------------------------------------------------
-void turnDegrees(Stepper &stepper, float deg)
-{
- // Skip very small movements
- if (fabs(deg) < 0.1)
+ // --- 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();
+
+ // --- State-Dependent Logic ---
+ // 3. Update tracking variables (only if tracking/centering and signal present)
+ if ((currentState == State::TRACKING || currentState == State::CENTERING) && isAnySensorActive())
{
- return;
+ lastSignalTime = now; // Keep track of the last time signal was seen
}
- enableMotors();
+ // 4. Execute main state machine logic
+ handleStateMachine();
- // Choose speed based on system state
- int targetSpeed =
- (currentState == SEARCHING) ? SEARCH_SPEED : TRACK_SPEED;
- stepper.setSpeed(targetSpeed);
+ // --- Monitoring & Safety ---
+ // 5. Update status LEDs to reflect current state
+ updateStatusLEDsOptimized();
- // Convert degrees to steps
- int steps = round(deg * stepsPerDegree);
- stepper.step(steps);
+ // 6. Check for motor stalls (runs checks at configured interval)
+ checkMotorStall();
- lastActionTime = millis();
+ // 7. Check for state timeouts (safety net against deadlocks)
+ checkStateTimeouts();
- // Optionally disable motors when idle to save power
- if (currentState == IDLE)
+// --- 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)
{
- disableMotors();
+ printTelemetry();
+ lastTelemetryTime = now;
}
+#endif
}
-//---------------------------------------------------------
-// Filtered Light Sensor Reading (averaging several samples)
-//---------------------------------------------------------
-void readLightSensors(int &topLeft, int &topRight, int &bottomLeft,
- int &bottomRight)
-{
- const int samples = 5;
- topLeft = 0;
- topRight = 0;
- bottomLeft = 0;
- bottomRight = 0;
+// ==========================================================================
+// === STATE MACHINE IMPLEMENTATION ===
+// ==========================================================================
- for (int i = 0; i < samples; i++)
+/**
+ * @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)
{
- topLeft += analogRead(LDR_TOP_LEFT);
- topRight += analogRead(LDR_TOP_RIGHT);
- bottomLeft += analogRead(LDR_BOTTOM_LEFT);
- bottomRight += analogRead(LDR_BOTTOM_RIGHT);
- delay(2);
+ handleHomingStates();
+ return; // Don't process further in this iteration
}
- topLeft /= samples;
- topRight /= samples;
- bottomLeft /= samples;
- bottomRight /= samples;
+ // Cache sensor state (already debounced)
+ bool anySensor = isAnySensorActive();
+ unsigned long now = millis(); // Re-cache time if needed
- // Debug output for sensor values
- Serial.print("Light levels - TL: ");
- Serial.print(topLeft);
- Serial.print(", TR: ");
- Serial.print(topRight);
- Serial.print(", BL: ");
- Serial.print(bottomLeft);
- Serial.print(", BR: ");
- Serial.println(bottomRight);
-}
-
-//---------------------------------------------------------
-// Get Average Light Level
-//---------------------------------------------------------
-int getAverageLightLevel()
-{
- int tl, tr, bl, br;
- readLightSensors(tl, tr, bl, br);
- return (tl + tr + bl + br) / 4;
-}
-
-//---------------------------------------------------------
-// Sensor Calibration Routine
-//---------------------------------------------------------
-void calibrateSensors()
-{
- Serial.println("Starting sensor calibration...");
- Serial.println("Cover sensors evenly with ambient light.");
- delay(3000);
-
- int tl = 0, tr = 0, bl = 0, br = 0;
- const int samples = 10;
-
- for (int i = 0; i < samples; i++)
+ switch (currentState)
{
- tl += analogRead(LDR_TOP_LEFT);
- tr += analogRead(LDR_TOP_RIGHT);
- bl += analogRead(LDR_BOTTOM_LEFT);
- br += analogRead(LDR_BOTTOM_RIGHT);
- delay(100);
- }
-
- tl /= samples;
- tr /= samples;
- bl /= samples;
- br /= samples;
-
- // Find maximum value from the sensors
- int maxVal = max(max(tl, tr), max(bl, br));
-
- // Calculate offsets to normalize sensor responses
- offsetTL = maxVal - tl;
- offsetTR = maxVal - tr;
- offsetBL = maxVal - bl;
- offsetBR = maxVal - br;
-
- Serial.println("Calibration complete!");
- Serial.print("Offsets - TL: ");
- Serial.print(offsetTL);
- Serial.print(", TR: ");
- Serial.print(offsetTR);
- Serial.print(", BL: ");
- Serial.print(offsetBL);
- Serial.print(", BR: ");
- Serial.println(offsetBR);
-}
-
-//---------------------------------------------------------
-// Search Routine with Various Search Patterns
-//---------------------------------------------------------
-void search()
-{
- currentState = SEARCHING;
- targetLocked = false;
-
- // Set stepper speeds for search
- stepperX.setSpeed(SEARCH_SPEED);
- stepperY.setSpeed(SEARCH_SPEED);
-
- switch (searchIteration % 3)
- {
- case 0: // Horizontal sweep
- Serial.println("Executing horizontal sweep search pattern");
- turnDegrees(stepperX, 180);
- delay(50);
- turnDegrees(stepperX, -180);
+ case State::INITIALIZING:
+ // Should transition immediately in setup()
+ changeState(State::CHECK_PIN_CONFLICTS);
break;
- case 1: // Grid search
- Serial.println("Executing grid search pattern");
- for (int y = -45; y <= 45; y += 30)
+
+ case State::CHECK_PIN_CONFLICTS:
+ if (!setupPinsAndCheckConflicts())
{
- turnDegrees(stepperY, 30);
- delay(50);
- // Alternate X direction each row
- if (((y / 30) & 1) == 0)
- {
- turnDegrees(stepperX, 180);
+ // Error handled within the function (halts)
+ while (1)
+ ; // Should not be reached
+ }
+ printTuningGuide(); // Print only if debug enabled
+#ifdef ENABLE_DEBUG_PRINTING
+ Serial.println(F("Pin checks complete. Starting Homing sequence..."));
+#endif
+ recoveryAttempted = false;
+ changeState(State::HOMING_X);
+ break;
+
+ case State::SWEEP_X:
+ if (anySensor)
+ {
+#ifdef ENABLE_DEBUG_PRINTING
+ Serial.println(F("Signal Found (X Sweep) -> Centering"));
+#endif
+ stepperX.stop(); // Stop the sweep smoothly
+ recoveryAttempted = false;
+ sweepXCyclesCompleted = 0; // Reset for next search
+ 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
+ changeState(State::SWEEP_Y);
}
else
- {
- turnDegrees(stepperX, -180);
+ { // 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
}
- delay(50);
- }
- // Reset Y position
- turnDegrees(stepperY, -45);
- break;
- case 2: // Spiral search
- Serial.println("Executing spiral search pattern");
- for (int i = 0; i < 4; i++)
- {
- int stepSize = 15 + (i * 15);
- turnDegrees(stepperX, stepSize);
- delay(50);
- turnDegrees(stepperY, stepSize);
- delay(50);
- turnDegrees(stepperX, -stepSize);
- delay(50);
- turnDegrees(stepperY, -stepSize);
- delay(50);
}
break;
- }
- searchIteration++;
- if (searchIteration >= MAX_SEARCH_ITERATIONS)
- {
- Serial.println("Max search iterations reached. Returning to idle state.");
- currentState = IDLE;
- searchIteration = 0;
+ case State::SWEEP_Y:
+ if (anySensor)
+ {
+#ifdef ENABLE_DEBUG_PRINTING
+ Serial.println(F("Signal Found (Y Sweep) -> Centering"));
+#endif
+ stepperY.stop(); // Stop the sweep smoothly
+ recoveryAttempted = false;
+ sweepYCyclesCompleted = 0; // Reset for next search
+ 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
+ sweepYCyclesCompleted = 0;
+ 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
+ }
+ }
+ break;
+
+ case State::CENTERING:
+ // Perform P-control logic to move towards center
+ handleTrackingLogicFixedPoint();
+
+ // Check if centering move is complete (both axes stopped at target)
+ 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?
+#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);
+ }
+ }
+ break;
+
+ 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
+ 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
+ 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
+ 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
+ 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;
}
}
-//---------------------------------------------------------
-// PID-Based Tracking Routine
-//---------------------------------------------------------
-void track()
+/**
+ * @brief Handles state transitions, logging, and state entry actions.
+ * @param newState The target state to transition to.
+ */
+void changeState(State newState)
{
- currentState = TRACKING;
+ if (newState == currentState)
+ return; // No change needed
- // Use faster speeds when tracking
- stepperX.setSpeed(TRACK_SPEED);
- stepperY.setSpeed(TRACK_SPEED);
+#ifdef ENABLE_DEBUG_PRINTING
+ Serial.print(getStateName(currentState));
+ Serial.print(F(" -> "));
+ Serial.println(getStateName(newState));
+#endif
- // Read sensor values with filtering
- int topLeft, topRight, bottomLeft, bottomRight;
- readLightSensors(topLeft, topRight, bottomLeft, bottomRight);
+ currentState = newState;
+ stateEntryTime = millis();
- // Apply calibration offsets to even out sensor responses
- topLeft += offsetTL;
- topRight += offsetTR;
- bottomLeft += offsetBL;
- bottomRight += offsetBR;
-
- int avgLight = (topLeft + topRight + bottomLeft + bottomRight) / 4;
- if (avgLight < MIN_LIGHT_LEVEL)
+ // --- State Entry Actions ---
+ // Reset stall detection when entering any state involving motion commands
+ if (newState >= State::HOMING_X && newState <= State::LOCAL_SEARCH)
{
- Serial.println("Light level too low for tracking. Reverting to search mode.");
- currentState = SEARCHING;
- targetLocked = false;
- return;
+ stallStartTimeX = 0;
+ stallStartTimeY = 0;
+ lastKnownStepperXPosForStall = stepperX.currentPosition(); // Use current pos as baseline
+ lastKnownStepperYPosForStall = stepperY.currentPosition();
+ lastStallCheckTime = stateEntryTime; // Start checking immediately
}
- // Calculate differences (error signals) for X and Y
- int xDiff = ((topRight + bottomRight) - (topLeft + bottomLeft)) / 2;
- int yDiff =
- ((bottomLeft + bottomRight) - (topLeft + topRight)) / 2;
-
- // Check if target is centered enough
- if (abs(xDiff) < LIGHT_LOCK_THRESHOLD &&
- abs(yDiff) < LIGHT_LOCK_THRESHOLD)
+ // Configure motors and initiate actions specific to the NEW state
+ switch (currentState)
{
- if (!targetLocked)
- {
- Serial.println("Target locked!");
- targetLocked = true;
+ 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
+ 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
+ break;
+ case State::POST_HOMING_MOVE_OFF_X:
+ setMotorSpeeds(homingSpeed, homingAcceleration, homingSpeed, homingAcceleration);
+ stepperX.move(postHomingMoveOffSwitchSteps); // Small relative move away from switch
+ stepperY.stop();
+ break;
+ case State::POST_HOMING_MOVE_OFF_Y:
+ setMotorSpeeds(homingSpeed, homingAcceleration, homingSpeed, homingAcceleration);
+ stepperY.move(postHomingMoveOffSwitchSteps);
+ stepperX.stop();
+ break;
+ case State::POST_HOMING_DELAY:
+ stepperX.stop();
+ stepperY.stop(); // Ensure motors are stopped during pause
+ break;
+ case State::SWEEP_X:
+ setMotorSpeeds(sweepSpeedDefault, sweepAccelerationDefault, sweepSpeedDefault, sweepAccelerationDefault);
+ sweepXCyclesCompleted = 0; // Reset sweep progress
+ startSweepX(); // Initiate the first X sweep leg
+ break;
+ case State::SWEEP_Y:
+ setMotorSpeeds(sweepSpeedDefault, sweepAccelerationDefault, sweepSpeedDefault, sweepAccelerationDefault);
+ sweepYCyclesCompleted = 0; // Reset sweep progress
+ startSweepY(); // Initiate the first Y sweep leg
+ break;
+ case State::CENTERING: // Fall-through intentional
+ 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)
+ currentLocalSearchCycle = 0;
+#ifdef ENABLE_DEBUG_PRINTING
+ Serial.print(F("Entering Local Search. Last Error Int X:"));
+ Serial.print(lastErrorX_int);
+ 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::ERROR:
+ stepperX.stop();
+ stepperY.stop(); // Ensure motors stop smoothly if possible
+ // In ERROR state, indicateFatalError() will attempt to disable outputs
+ break;
+ case State::INITIALIZING: // Fall-through intentional
+ case State::CHECK_PIN_CONFLICTS:
+ // No motor actions required
+ break;
+ }
+ updateStatusLEDsOptimized(); // Update LEDs immediately to reflect the new state
+}
+
+// ==========================================================================
+// === STATE HANDLERS ===
+// ==========================================================================
+
+/**
+ * @brief Manages the sequence of states related to homing the X and Y axes.
+ */
+void handleHomingStates()
+{
+ 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
+ changeState(State::POST_HOMING_MOVE_OFF_X);
}
- return;
- }
+ else if (!stepperX.isRunning())
+ { // Should not happen unless stalled/timeout
+ handleSystemError(F("Homing X Error: Motor stopped before limit switch."));
+ }
+ break;
- // PID control for the X axis
- float xError = xDiff;
- xIntegral = constrain(xIntegral + xError, -500, 500);
- float xDerivative = xError - lastXError;
- float xOutput = (kP * xError + kI * xIntegral + kD * xDerivative) / 100.0;
- lastXError = xError;
+ 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
+ changeState(State::HOMING_Y);
+ }
+ break;
- // PID control for the Y axis
- float yError = yDiff;
- yIntegral = constrain(yIntegral + yError, -500, 500);
- float yDerivative = yError - lastYError;
- float yOutput = (kP * yError + kI * yIntegral + kD * yDerivative) / 100.0;
- lastYError = yError;
+ 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
+ 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;
- // Convert PID outputs to degree movements (limit by max step size)
- float xDegrees = constrain(xOutput, -MAX_STEP_SIZE, MAX_STEP_SIZE);
- float yDegrees = constrain(yOutput, -MAX_STEP_SIZE, MAX_STEP_SIZE);
+ 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
+ changeState(State::POST_HOMING_DELAY);
+ }
+ break;
- if (abs(xDegrees) > 0.1)
- {
- Serial.print("Moving X: ");
- Serial.println(xDegrees);
- turnDegrees(stepperX, xDegrees);
- }
-
- delay(20);
-
- if (abs(yDegrees) > 0.1)
- {
- Serial.print("Moving Y: ");
- Serial.println(yDegrees);
- turnDegrees(stepperY, yDegrees);
+ 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
+ changeState(State::SWEEP_X);
+ }
+ break;
+ default:
+ break; // Should not happen
}
}
-//---------------------------------------------------------
-// Process Serial Commands for Manual Control and Status
-//---------------------------------------------------------
-void processCommand(String command)
+/**
+ * @brief Calculates tracking error using integer logic and commands motor movement
+ * using fixed-point proportional control. Updates `lastErrorX_int`, `lastErrorY_int`.
+ */
+void handleTrackingLogicFixedPoint()
{
- command.trim();
+ // 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];
- if (command.startsWith("move"))
+ int8_t currentErrorX_int = ERROR_INT_CENTERED;
+ int8_t currentErrorY_int = ERROR_INT_CENTERED;
+
+ // --- 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).
+ }
+ 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.
+ }
+
+ // --- 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.
+ 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 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)
+ stepsToMoveY = 0;
+
+ // --- Command Movement ---
+ if (stepsToMoveX != 0 || stepsToMoveY != 0)
{
- int spaceIndex1 = command.indexOf(' ');
- int spaceIndex2 = command.indexOf(' ', spaceIndex1 + 1);
- if (spaceIndex1 != -1 && spaceIndex2 != -1)
+ 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())
{
- String xStr = command.substring(spaceIndex1 + 1, spaceIndex2);
- String yStr = command.substring(spaceIndex2 + 1);
- float xDeg = xStr.toFloat();
- float yDeg = yStr.toFloat();
-
- Serial.print("Moving X: ");
- Serial.print(xDeg);
- Serial.print(", Y: ");
- Serial.println(yDeg);
-
- turnDegrees(stepperX, xDeg);
- turnDegrees(stepperY, yDeg);
+ stepperX.moveTo(targetX);
+ }
+ if (targetY != stepperY.targetPosition())
+ {
+ stepperY.moveTo(targetY);
}
}
- else if (command.equals("search"))
+ // 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()
+{
+ unsigned long now = millis();
+
+ // --- Check for Signal Re-acquisition ---
+ if (isAnySensorActive())
{
- Serial.println("Starting search mode");
- currentState = SEARCHING;
- searchIteration = 0;
+#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
+ changeState(State::CENTERING);
+ return;
}
- else if (command.equals("track"))
+
+ // --- Check Overall Timeout ---
+ if (safeMillisSubtract(now, stateEntryTime) > localSearchTimeoutMs)
{
- Serial.println("Starting tracking mode");
- currentState = TRACKING;
+#ifdef ENABLE_DEBUG_PRINTING
+ Serial.println(F("TIMEOUT: Local Search Failed -> Full Search (Sweep X)"));
+#endif
+ stepperX.stop();
+ stepperY.stop();
+ sweepDirectionX = 1; // Reset sweep
+ changeState(State::SWEEP_X);
+ return;
}
- else if (command.equals("calibrate"))
+
+ // --- 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)
{
- calibrateSensors();
- }
- else if (command.equals("stop") || command.equals("idle"))
- {
- Serial.println("Stopping and entering idle mode");
- currentState = IDLE;
- }
- else if (command.equals("status"))
- {
- Serial.println("=== System Status ===");
- Serial.print("State: ");
- switch (currentState)
+ 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
+
+#ifdef ENABLE_DEBUG_PRINTING
+ Serial.print(F("Local Search Phase: "));
+ Serial.print(currentLocalSearchPhase);
+ Serial.print(F(", Cycle/Step: "));
+ Serial.println(currentLocalSearchCycle);
+#endif
+
+ switch (currentLocalSearchPhase)
{
- case INITIALIZING:
- Serial.println("Initializing");
+ // Phase 0: Probe - Make a small move in the direction the target was last seen.
+ case 0:
+ { // FIX: Added curly braces for scope
+#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)
+ 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
+ targetY = clamp(targetY, lastKnownY - localSearchSweepSteps, lastKnownY + localSearchSweepSteps);
+
+ commandMove = true;
+ currentLocalSearchPhase = 1; // Advance to Sweep phase next
+ currentLocalSearchCycle = 0; // Reset cycle counter for Sweep phase
break;
- case SEARCHING:
- Serial.println("Searching");
+ } // 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
+#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
+ if (currentLocalSearchCycle < 2)
+ { // Sweep X axis (Cycles 0, 1)
+ targetX = lastKnownX + sweepDir * localSearchSweepSteps;
+ }
+ else
+ { // Sweep Y axis (Cycles 2, 3)
+ targetY = lastKnownY + sweepDir * localSearchSweepSteps;
+ }
+ targetX = clamp(targetX, searchMinX_steps, searchMaxX_steps); // Clamp to absolute limits
+ 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
+ }
break;
- case TRACKING:
- Serial.println("Tracking");
- break;
- case IDLE:
- Serial.println("Idle");
+ } // FIX: Closed curly brace
+
+ // Phase 2: Box Pattern - Fallback expanding square spiral search.
+ case 2:
+ { // FIX: Added curly braces for scope
+#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
+ Serial.println(F("Local Search Box Pattern Max Cycles Reached -> Full Search"));
+#endif
+ sweepDirectionX = 1;
+ changeState(State::SWEEP_X);
+ return; // Give up local search
+ }
+
+ 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
+ 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
+ case 1:
+ dY = -(long)localSearchBoxStepSize * moveScale;
+ break; // Up (Negative Y) - Added cast
+ case 2:
+ dX = -(long)localSearchBoxStepSize * moveScale;
+ break; // Left - Added cast
+ case 3:
+ dX = -(long)localSearchBoxStepSize * moveScale;
+ break; // Left - Added cast
+ case 4:
+ dY = (long)localSearchBoxStepSize * moveScale;
+ break; // Down (Positive Y) - Added cast
+ case 5:
+ dY = (long)localSearchBoxStepSize * moveScale;
+ break; // Down - Added cast
+ case 6:
+ dX = (long)localSearchBoxStepSize * moveScale;
+ break; // Right - Added cast
+ case 7:
+ dX = (long)localSearchBoxStepSize * moveScale;
+ break; // Right (completes layer) - Added cast
+ }
+ targetX = clamp(currentX + dX, searchMinX_steps, searchMaxX_steps);
+ targetY = clamp(currentY + dY, searchMinY_steps, searchMaxY_steps);
+
+ commandMove = true;
+ currentLocalSearchCycle++; // Increment box step counter
break;
}
- Serial.print("Target locked: ");
- Serial.println(targetLocked ? "Yes" : "No");
- int tl, tr, bl, br;
- readLightSensors(tl, tr, bl, br);
- Serial.print("Average light level: ");
- Serial.println((tl + tr + bl + br) / 4);
- Serial.println("====================");
+ }
+
+ // --- Command Move (If Calculated) ---
+ if (commandMove && (targetX != currentX || targetY != currentY))
+ {
+#ifdef ENABLE_DEBUG_PRINTING
+ Serial.print(F(" Cmd Move -> X:"));
+ Serial.print(targetX);
+ 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())
+ stepperY.moveTo(targetY);
+ }
+ 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."));
+#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()
+{
+ 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()
+{
+ long targetY = (sweepDirectionY > 0) ? searchMaxY_steps : searchMinY_steps;
+#ifdef ENABLE_DEBUG_PRINTING
+ Serial.print(F("Starting Y Sweep Leg -> Target: "));
+ 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
+
+ // --- 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: 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("...")); }
+
+#ifdef ENABLE_DEBUG_PRINTING
+ Serial.println(F("--- Pin Conflict Check PASSED (Review I2C Warning!) ---"));
+ Serial.println(F("--- Pin Initialization Complete ---"));
+#endif
+ return true; // Setup successful
+}
+
+/**
+ * @brief Prints the initial startup message with version info.
+ */
+void printStartupMessage()
+{
+#if defined(ENABLE_DEBUG_PRINTING) || defined(ENABLE_TELEMETRY)
+ Serial.println(F("")); // Newline
+ Serial.println(F("-------------------------------------"));
+ Serial.print(F(" IR Tracker System Initializing v"));
+ Serial.println(F(SYSTEM_VERSION));
+#ifdef DEBUG_BUILD
+ Serial.println(F(" Build Mode: DEBUG (Serial Enabled)"));
+#else
+ Serial.println(F(" Build Mode: PRODUCTION (Serial Disabled)"));
+#endif
+ Serial.println(F("-------------------------------------"));
+ Serial.flush(); // Ensure message is printed before potentially complex setup
+#endif
+}
+
+/**
+ * @brief Prints detailed tuning guidance (only if Serial output is enabled).
+ */
+void printTuningGuide()
+{
+#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("\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.print(homingSpeed);
+ Serial.print(F(" Sw="));
+ Serial.print(sweepSpeedDefault);
+ Serial.print(F(" Tr="));
+ Serial.print(trackSpeedDefault);
+ 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.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("\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`.
+ */
+void readInputsOptimized()
+{
+ 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)
+ {
+ lastDebounceTimeSensors = now;
+ }
+ if (limitsChanged)
+ {
+ lastDebounceTimeLimits = now;
+ }
+
+ // 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);
+ }
+
+ if (safeMillisSubtract(now, lastDebounceTimeLimits) > limitSwitchDebounceMs)
+ {
+ // Active LOW logic for limit switches.
+ limitSwitchActiveX = !(currentPinStatesC & limitSwitchMaskX);
+ limitSwitchActiveY = !(currentPinStatesC & 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.
+ */
+inline bool isAnySensorActive()
+{
+ // Check the debounced values in the global array
+ for (uint8_t i = 0; i < NUM_SENSORS; i++)
+ {
+ if (sensorActive[i])
+ return true;
+ }
+ return false;
+}
+
+/**
+ * @brief Checks if the center sensor is currently active.
+ * @return True if the center sensor is active, False otherwise.
+ */
+inline bool isCentered()
+{
+ // Check the debounced state of the center sensor
+ return sensorActive[SENSOR_C];
+}
+
+/**
+ * @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
+
+ // 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()
+{
+ unsigned long currentTimeout = 0;
+ // Assign timeout duration based on the current potentially blocking state
+ switch (currentState)
+ {
+ case State::HOMING_X:
+ case State::HOMING_Y:
+ case State::POST_HOMING_MOVE_OFF_X:
+ case State::POST_HOMING_MOVE_OFF_Y:
+ currentTimeout = homingTimeoutMs;
+ break;
+ case State::SWEEP_X:
+ case State::SWEEP_Y:
+ 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
+ }
+
+ // Check if the timeout has been exceeded
+ if (currentTimeout > 0 && safeMillisSubtract(millis(), stateEntryTime) > currentTimeout)
+ {
+#ifdef ENABLE_DEBUG_PRINTING
+ Serial.print(F("TIMEOUT! State: "));
+ Serial.print(getStateName(currentState));
+ Serial.print(F(" exceeded "));
+ 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()
+{
+ unsigned long now = millis();
+ // Only perform check at the specified interval
+ if (safeMillisSubtract(now, lastStallCheckTime) < stallCheckIntervalMs)
+ {
+ return;
+ }
+ lastStallCheckTime = now; // Update last check time
+
+ 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
+ }
+ }
+ else
+ { // Motor is not running (stopped normally or finished move)
+ stallStartTimeX = 0; // Reset stall timer
+ lastKnownStepperXPosForStall = stepperX.currentPosition(); // Keep known position updated
+ }
+
+ // --- 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
+ }
+ }
+ else
+ { // Motor not running
+ stallStartTimeY = 0; // Reset timer
+ lastKnownStepperYPosForStall = stepperY.currentPosition(); // Update position
+ }
+
+ // --- 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)
+{
+#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
+ 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
+ }
+
+// 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
+}
+
+/**
+ * @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)
+{
+// Print diagnostics if Serial is enabled
+#if defined(ENABLE_DEBUG_PRINTING) || defined(ENABLE_TELEMETRY)
+ Serial.println(F("\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"));
+ Serial.println(F(" >>> FATAL SYSTEM ERROR <<<"));
+ Serial.println(F("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"));
+ Serial.print(F("REASON: "));
+ Serial.println(message);
+ Serial.print(F("STATE AT ERROR: "));
+ Serial.println(getStateName(currentState));
+ Serial.print(F("TIME IN STATE (ms): "));
+ Serial.println(safeMillisSubtract(millis(), stateEntryTime));
+ Serial.print(F("Position X: "));
+ 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(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
+#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)
+ 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)
+ }
+}
+
+/**
+ * @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)
+{
+ if (current >= previous)
+ {
+ return current - previous;
}
else
{
- Serial.println("Unknown command.");
- Serial.println(
- "Available commands: 'search', 'track', 'move X Y', 'calibrate', 'stop', 'idle', 'status'");
+ // Rollover occurred
+ return (ULONG_MAX - previous) + current + 1;
}
}
-//---------------------------------------------------------
-// Setup: Initialize Serial, Pins, and Watchdog Timer
-//---------------------------------------------------------
-void setup()
+/**
+ * @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)
{
- Serial.begin(9600);
-
- // Setup the motor enable pins and disable motors initially.
- pinMode(ENABLE_X, OUTPUT);
- pinMode(ENABLE_Y, OUTPUT);
- disableMotors();
-
- // Set initial motor speeds
- stepperX.setSpeed(SEARCH_SPEED);
- stepperY.setSpeed(SEARCH_SPEED);
-
- // Initialize light sensor pins
- pinMode(LDR_TOP_LEFT, INPUT);
- pinMode(LDR_TOP_RIGHT, INPUT);
- pinMode(LDR_BOTTOM_LEFT, INPUT);
- pinMode(LDR_BOTTOM_RIGHT, INPUT);
-
- Serial.println("Light Tracking System Initialized");
- Serial.println(
- "Commands: 'search', 'track', 'move X Y', 'calibrate', 'stop', 'idle', 'status'");
-
- currentState = IDLE;
- lastActionTime = millis();
-
- // Disable watchdog during initial setup then enable it with a 2-second timeout
- wdt_disable();
- wdt_enable(WDTO_2S);
+ if (val < minVal)
+ return minVal;
+ if (val > maxVal)
+ return maxVal;
+ return val;
}
-//---------------------------------------------------------
-// Main Loop: State Machine and Watchdog Reset
-//---------------------------------------------------------
-void loop()
+/**
+ * @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)
{
- // Process any incoming serial commands
- if (Serial.available() > 0)
- {
- String command = Serial.readStringUntil('\n');
- processCommand(command);
- }
-
- // Run state machine logic
- switch (currentState)
- {
- case IDLE:
- // No movement in idle mode
- break;
-
- case SEARCHING:
- if (millis() - lastActionTime > 2000)
- {
- search();
- // After a search pattern, if enough light is detected, switch to tracking
- if (getAverageLightLevel() > MIN_LIGHT_LEVEL)
- {
- Serial.println(
- "Potential light source found! Switching to tracking mode.");
- currentState = TRACKING;
- }
- }
- break;
-
- case TRACKING:
- if (millis() - lastActionTime > 500)
- {
- track();
- }
- break;
-
- case INITIALIZING:
- currentState = IDLE;
- break;
- }
-
- // Reset the watchdog timer each loop iteration.
- wdt_reset();
+ stepperX.setMaxSpeed(speedX);
+ stepperX.setAcceleration(accelX);
+ stepperY.setMaxSpeed(speedY);
+ 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)
+{
+ // Copy the string from PROGMEM into the buffer
+ strcpy_P(stateNameBuffer, (char *)pgm_read_word(&(stateNames[static_cast<uint8_t>(state)])));
+ return stateNameBuffer;
+}
+
+/**
+ * @brief Prints periodic telemetry data to the Serial Monitor (if enabled).
+ */
+void printTelemetry()
+{
+#ifdef ENABLE_TELEMETRY
+ Serial.print(F("TLM: St:"));
+ Serial.print(getStateName(currentState));
+ Serial.print(F(" P:[X:"));
+ Serial.print(stepperX.currentPosition());
+ 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(sensorActive[SENSOR_R] ? '1' : '0');
+ Serial.print(F("U"));
+ 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.println();
+// No Serial.flush() needed here, let buffer handle periodic output.
+#endif // ENABLE_TELEMETRY
+}
\ No newline at end of file