bs
This commit is contained in:
Mohamad
parent
123ad527e1
commit
60469f1a0f
@ -1,123 +1,520 @@
|
||||
// Includes the Arduino Stepper Library
|
||||
#include <Stepper.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 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);
|
||||
}
|
||||
// Reset the watchdog timer each loop iteration.
|
||||
wdt_reset();
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user