bs
This commit is contained in:
Mohamad
parent
123ad527e1
commit
60469f1a0f
@ -1,123 +1,520 @@
|
|||||||
// Includes the Arduino Stepper Library
|
|
||||||
#include <Stepper.h>
|
#include <Stepper.h>
|
||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
|
#include <avr/wdt.h>
|
||||||
|
|
||||||
// Defines the number of steps per rotation
|
//---------------------------------------------------------
|
||||||
|
// Stepper Motor Configuration and Global Constants
|
||||||
|
//---------------------------------------------------------
|
||||||
const int stepsPerRevolution = 4096;
|
const int stepsPerRevolution = 4096;
|
||||||
|
const float stepsPerDegree = 4096.0 / 360.0; // More precise calculation
|
||||||
|
|
||||||
// TODO: fix rounding errors
|
// Motor Enable Pins (adjust these if needed)
|
||||||
// int stepsPerDegree = stepsPerRevolution / 360;
|
const int ENABLE_X = 2; // Active LOW enable
|
||||||
// const float stepsPerDegree = stepsPerRevolution / 360.0;
|
const int ENABLE_Y = 3;
|
||||||
const float stepsPerDegree = 11;
|
|
||||||
|
|
||||||
// Creates an instance of stepper class
|
// Light sensor pins
|
||||||
// Pins entered in sequence IN1-IN3-IN2-IN4 for proper step sequence
|
const int LDR_TOP_LEFT = A0; // Top-left sensor
|
||||||
Stepper stepperX = Stepper(stepsPerRevolution, 8, 10, 9, 11);
|
const int LDR_TOP_RIGHT = A1; // Top-right sensor
|
||||||
Stepper stepperY = Stepper(stepsPerRevolution, 4, 5, 6, 7);
|
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)
|
INITIALIZING,
|
||||||
Serial.begin(9600); // Open serial port at 9600 baud rate
|
SEARCHING,
|
||||||
stepperX.setSpeed(10);
|
TRACKING,
|
||||||
stepperY.setSpeed(10);
|
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);
|
// Setting enable to HIGH disables the motor drivers.
|
||||||
stepperY.setSpeed(10);
|
digitalWrite(ENABLE_X, HIGH);
|
||||||
stepper.step(deg * stepsPerDegree);
|
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);
|
// Skip very small movements
|
||||||
stepperX.step(command.toInt() * stepsPerDegree);
|
if (fabs(deg) < 0.1)
|
||||||
Serial.println(command);
|
{
|
||||||
|
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()
|
void search()
|
||||||
{
|
{
|
||||||
turnDegrees(stepperX, 180);
|
currentState = SEARCHING;
|
||||||
delay(10);
|
targetLocked = false;
|
||||||
turnDegrees(stepperY, 45);
|
|
||||||
delay(10);
|
|
||||||
turnDegrees(stepperX, -180);
|
|
||||||
delay(10);
|
|
||||||
turnDegrees(stepperY, 45);
|
|
||||||
delay(10);
|
|
||||||
|
|
||||||
// back to zero:
|
// Set stepper speeds for search
|
||||||
turnDegrees(stepperY, -90);
|
stepperX.setSpeed(SEARCH_SPEED);
|
||||||
}
|
stepperY.setSpeed(SEARCH_SPEED);
|
||||||
|
|
||||||
void search2()
|
switch (searchIteration % 3)
|
||||||
{
|
{
|
||||||
turnDegrees(stepperX, 30);
|
case 0: // Horizontal sweep
|
||||||
delay(100);
|
Serial.println("Executing horizontal sweep search pattern");
|
||||||
turnDegrees(stepperY, 15);
|
turnDegrees(stepperX, 180);
|
||||||
delay(100);
|
delay(50);
|
||||||
turnDegrees(stepperX, 30);
|
turnDegrees(stepperX, -180);
|
||||||
delay(100);
|
break;
|
||||||
turnDegrees(stepperY, 15);
|
case 1: // Grid search
|
||||||
delay(100);
|
Serial.println("Executing grid search pattern");
|
||||||
turnDegrees(stepperX, 30);
|
for (int y = -45; y <= 45; y += 30)
|
||||||
delay(100);
|
{
|
||||||
turnDegrees(stepperY, 15);
|
turnDegrees(stepperY, 30);
|
||||||
delay(100);
|
delay(50);
|
||||||
turnDegrees(stepperX, 30);
|
// Alternate X direction each row
|
||||||
delay(100);
|
if (((y / 30) & 1) == 0)
|
||||||
turnDegrees(stepperY, 15);
|
{
|
||||||
delay(100);
|
turnDegrees(stepperX, 180);
|
||||||
turnDegrees(stepperX, 30);
|
}
|
||||||
delay(100);
|
else
|
||||||
turnDegrees(stepperY, 15);
|
{
|
||||||
delay(100);
|
turnDegrees(stepperX, -180);
|
||||||
turnDegrees(stepperX, 30);
|
}
|
||||||
delay(100);
|
delay(50);
|
||||||
turnDegrees(stepperY, 15);
|
}
|
||||||
delay(100);
|
// Reset Y position
|
||||||
|
turnDegrees(stepperY, -45);
|
||||||
turnDegrees(stepperX, -360);
|
break;
|
||||||
delay(100);
|
case 2: // Spiral search
|
||||||
|
Serial.println("Executing spiral search pattern");
|
||||||
// back to zero:
|
for (int i = 0; i < 4; i++)
|
||||||
turnDegrees(stepperY, -90);
|
{
|
||||||
|
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()
|
void track()
|
||||||
{
|
{
|
||||||
// compare sensors
|
currentState = TRACKING;
|
||||||
// move towards strongest light signal
|
|
||||||
|
// 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()
|
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
|
// Run state machine logic
|
||||||
// String command = Serial.readStringUntil('\n'); // Read input until newline
|
switch (currentState)
|
||||||
// command.trim(); // Remove any extra whitespace or newline characters
|
{
|
||||||
// processCommand(command); // Process the received command
|
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();
|
// Reset the watchdog timer each loop iteration.
|
||||||
// turnDegrees(stepperX, 180);
|
wdt_reset();
|
||||||
|
|
||||||
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);
|
|
||||||
}
|
}
|
||||||
Loading…
Reference in New Issue
Block a user