From 59434137316e362befbd8b83eb33c825992fac1f Mon Sep 17 00:00:00 2001 From: Mohamad Date: Fri, 2 May 2025 08:34:06 +0200 Subject: [PATCH] working demo (hopefully) + readme --- Arduino-Satellite-Dish/src/README | 433 ++++++ Arduino-Satellite-Dish/src/main.cpp | 2047 +++++++++++++++++++++------ 2 files changed, 2052 insertions(+), 428 deletions(-) create mode 100644 Arduino-Satellite-Dish/src/README 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) + +[![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](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 +/** + * @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 -#include +#include +#include +#include -//--------------------------------------------------------- -// 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(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