diff --git a/Arduino-Satellite-Dish/src/main.cpp b/Arduino-Satellite-Dish/src/main.cpp index a3f746f..109fb86 100644 --- a/Arduino-Satellite-Dish/src/main.cpp +++ b/Arduino-Satellite-Dish/src/main.cpp @@ -1,123 +1,520 @@ -// Includes the Arduino Stepper Library #include #include +#include -// Defines the number of steps per rotation +//--------------------------------------------------------- +// Stepper Motor Configuration and Global Constants +//--------------------------------------------------------- const int stepsPerRevolution = 4096; +const float stepsPerDegree = 4096.0 / 360.0; // More precise calculation -// TODO: fix rounding errors -// int stepsPerDegree = stepsPerRevolution / 360; -// const float stepsPerDegree = stepsPerRevolution / 360.0; -const float stepsPerDegree = 11; +// Motor Enable Pins (adjust these if needed) +const int ENABLE_X = 2; // Active LOW enable +const int ENABLE_Y = 3; -// Creates an instance of stepper class -// Pins entered in sequence IN1-IN3-IN2-IN4 for proper step sequence -Stepper stepperX = Stepper(stepsPerRevolution, 8, 10, 9, 11); -Stepper stepperY = Stepper(stepsPerRevolution, 4, 5, 6, 7); +// 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 -void setup() +// 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 + +bool targetLocked = false; + +// 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 + +//--------------------------------------------------------- +// System state and global variables +//--------------------------------------------------------- +enum SystemState { - // Nothing to do (Stepper Library sets pins as outputs) - Serial.begin(9600); // Open serial port at 9600 baud rate - stepperX.setSpeed(10); - stepperY.setSpeed(10); + INITIALIZING, + SEARCHING, + TRACKING, + IDLE +}; + +SystemState currentState = INITIALIZING; +unsigned long lastActionTime = 0; +int searchIteration = 0; + +// Calibration offsets for each light sensor +int offsetTL = 0, offsetTR = 0, offsetBL = 0, offsetBR = 0; + +//--------------------------------------------------------- +// Create stepper objects (wiring order may need to be verified) +//--------------------------------------------------------- +Stepper stepperX(stepsPerRevolution, 8, 10, 9, 11); +Stepper stepperY(stepsPerRevolution, 4, 5, 6, 7); + +//--------------------------------------------------------- +// 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 + +//--------------------------------------------------------- +// Motor Power Management Functions +//--------------------------------------------------------- +void enableMotors() +{ + // For many motor drivers, setting the enable pin LOW activates the driver. + digitalWrite(ENABLE_X, LOW); + digitalWrite(ENABLE_Y, LOW); } -void turnDegrees(Stepper stepper, int deg) +void disableMotors() { - stepperX.setSpeed(10); - stepperY.setSpeed(10); - stepper.step(deg * stepsPerDegree); + // Setting enable to HIGH disables the motor drivers. + digitalWrite(ENABLE_X, HIGH); + digitalWrite(ENABLE_Y, HIGH); } -void processCommand(String command) +//--------------------------------------------------------- +// Movement Function with Motor Enable/Disable and Speed Management +//--------------------------------------------------------- +void turnDegrees(Stepper &stepper, float deg) { - stepperX.setSpeed(10); - stepperX.step(command.toInt() * stepsPerDegree); - Serial.println(command); + // Skip very small movements + if (fabs(deg) < 0.1) + { + return; + } + + enableMotors(); + + // Choose speed based on system state + int targetSpeed = + (currentState == SEARCHING) ? SEARCH_SPEED : TRACK_SPEED; + stepper.setSpeed(targetSpeed); + + // Convert degrees to steps + int steps = round(deg * stepsPerDegree); + stepper.step(steps); + + lastActionTime = millis(); + + // Optionally disable motors when idle to save power + if (currentState == IDLE) + { + disableMotors(); + } } +//--------------------------------------------------------- +// 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; + + for (int i = 0; i < samples; i++) + { + topLeft += analogRead(LDR_TOP_LEFT); + topRight += analogRead(LDR_TOP_RIGHT); + bottomLeft += analogRead(LDR_BOTTOM_LEFT); + bottomRight += analogRead(LDR_BOTTOM_RIGHT); + delay(2); + } + + topLeft /= samples; + topRight /= samples; + bottomLeft /= samples; + bottomRight /= samples; + + // 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++) + { + 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() { - turnDegrees(stepperX, 180); - delay(10); - turnDegrees(stepperY, 45); - delay(10); - turnDegrees(stepperX, -180); - delay(10); - turnDegrees(stepperY, 45); - delay(10); + currentState = SEARCHING; + targetLocked = false; - // back to zero: - turnDegrees(stepperY, -90); -} - -void search2() -{ - turnDegrees(stepperX, 30); - delay(100); - turnDegrees(stepperY, 15); - delay(100); - turnDegrees(stepperX, 30); - delay(100); - turnDegrees(stepperY, 15); - delay(100); - turnDegrees(stepperX, 30); - delay(100); - turnDegrees(stepperY, 15); - delay(100); - turnDegrees(stepperX, 30); - delay(100); - turnDegrees(stepperY, 15); - delay(100); - turnDegrees(stepperX, 30); - delay(100); - turnDegrees(stepperY, 15); - delay(100); - turnDegrees(stepperX, 30); - delay(100); - turnDegrees(stepperY, 15); - delay(100); - - turnDegrees(stepperX, -360); - delay(100); - - // back to zero: - turnDegrees(stepperY, -90); + // 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); + break; + case 1: // Grid search + Serial.println("Executing grid search pattern"); + for (int y = -45; y <= 45; y += 30) + { + turnDegrees(stepperY, 30); + delay(50); + // Alternate X direction each row + if (((y / 30) & 1) == 0) + { + turnDegrees(stepperX, 180); + } + else + { + turnDegrees(stepperX, -180); + } + 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; + } } +//--------------------------------------------------------- +// PID-Based Tracking Routine +//--------------------------------------------------------- void track() { - // compare sensors - // move towards strongest light signal + currentState = TRACKING; + + // Use faster speeds when tracking + stepperX.setSpeed(TRACK_SPEED); + stepperY.setSpeed(TRACK_SPEED); + + // Read sensor values with filtering + int topLeft, topRight, bottomLeft, bottomRight; + readLightSensors(topLeft, topRight, bottomLeft, bottomRight); + + // 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) + { + Serial.println("Light level too low for tracking. Reverting to search mode."); + currentState = SEARCHING; + targetLocked = false; + return; + } + + // 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) + { + if (!targetLocked) + { + Serial.println("Target locked!"); + targetLocked = true; + } + return; + } + + // 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; + + // 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; + + // 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); + + 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); + } } +//--------------------------------------------------------- +// Process Serial Commands for Manual Control and Status +//--------------------------------------------------------- +void processCommand(String command) +{ + command.trim(); + + if (command.startsWith("move")) + { + int spaceIndex1 = command.indexOf(' '); + int spaceIndex2 = command.indexOf(' ', spaceIndex1 + 1); + if (spaceIndex1 != -1 && spaceIndex2 != -1) + { + 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); + } + } + else if (command.equals("search")) + { + Serial.println("Starting search mode"); + currentState = SEARCHING; + searchIteration = 0; + } + else if (command.equals("track")) + { + Serial.println("Starting tracking mode"); + currentState = TRACKING; + } + else if (command.equals("calibrate")) + { + 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) + { + case INITIALIZING: + Serial.println("Initializing"); + break; + case SEARCHING: + Serial.println("Searching"); + break; + case TRACKING: + Serial.println("Tracking"); + break; + case IDLE: + Serial.println("Idle"); + 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("===================="); + } + else + { + Serial.println("Unknown command."); + Serial.println( + "Available commands: 'search', 'track', 'move X Y', 'calibrate', 'stop', 'idle', 'status'"); + } +} + +//--------------------------------------------------------- +// Setup: Initialize Serial, Pins, and Watchdog Timer +//--------------------------------------------------------- +void setup() +{ + 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); +} + +//--------------------------------------------------------- +// Main Loop: State Machine and Watchdog Reset +//--------------------------------------------------------- void loop() { + // Process any incoming serial commands + if (Serial.available() > 0) + { + String command = Serial.readStringUntil('\n'); + processCommand(command); + } - // if (Serial.available() > 0) { // Check if data is available - // String command = Serial.readStringUntil('\n'); // Read input until newline - // command.trim(); // Remove any extra whitespace or newline characters - // processCommand(command); // Process the received command + // Run state machine logic + switch (currentState) + { + case IDLE: + // No movement in idle mode + break; - // // Serial.println(command); - // } + 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; - turnDegrees(stepperY, 180); + case TRACKING: + if (millis() - lastActionTime > 500) + { + track(); + } + break; + case INITIALIZING: + currentState = IDLE; + break; + } - // search(); - // turnDegrees(stepperX, 180); - - delay(100); - - // // Rotate CCW quickly at 10 RPM - // stepperX.setSpeed(10); - // stepperX.step(-stepsPerRevolution); - // delay(1000); - - // // Rotate CCW quickly at 10 RPM - // stepperY.setSpeed(10); - // stepperY.step(stepsPerRevolution); - // delay(1000); -} \ No newline at end of file + // Reset the watchdog timer each loop iteration. + wdt_reset(); +}