/*
2 Stepper Motors + Ultrasonic + Line Follower (Switch)
- Stepper 1: Step 19, Dir 21 (Left Motor)
- Stepper 2: Step 22, Dir 23 (Right Motor)
- Ultrasonic: Trig 5, Echo 18
- Left Sensor (Switch): Pin 25
- Right Sensor (Switch): Pin 26
*/
// --- Pin Definitions ---
const int trigPin = 5;
const int echoPin = 18;
const int leftSensorPin = 25; // Switch 1
const int rightSensorPin = 26; // Switch 2
// Motor 1 (Left)
const int stepPin1 = 19;
const int dirPin1 = 21;
// Motor 2 (Right)
const int stepPin2 = 22;
const int dirPin2 = 23;
// --- Settings ---
const int obstacleThreshold = 20; // cm
// --- Helper Functions ---
void moveForward() {
digitalWrite(dirPin1, HIGH); // Left Forward
digitalWrite(dirPin2, HIGH); // Right Forward
digitalWrite(stepPin1, HIGH);
digitalWrite(stepPin2, HIGH);
delayMicroseconds(1000);
digitalWrite(stepPin1, LOW);
digitalWrite(stepPin2, LOW);
delayMicroseconds(1000);
}
void turnRight() {
digitalWrite(dirPin1, HIGH); // Left Forward
digitalWrite(dirPin2, LOW); // Right Backward (Spin)
digitalWrite(stepPin1, HIGH);
digitalWrite(stepPin2, HIGH);
delayMicroseconds(2000); // Slower for turning
digitalWrite(stepPin1, LOW);
digitalWrite(stepPin2, LOW);
delayMicroseconds(2000);
}
void turnLeft() {
digitalWrite(dirPin1, LOW); // Left Backward (Spin)
digitalWrite(dirPin2, HIGH); // Right Forward
digitalWrite(stepPin1, HIGH);
digitalWrite(stepPin2, HIGH);
delayMicroseconds(2000);
digitalWrite(stepPin1, LOW);
digitalWrite(stepPin2, LOW);
delayMicroseconds(2000);
}
int getDistance() {
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
long duration = pulseIn(echoPin, HIGH);
return duration * 0.034 / 2;
}
// --- Main Setup ---
void setup() {
Serial.begin(115200);
// Ultrasonic
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
// Line Sensors (Switches)
pinMode(leftSensorPin, INPUT);
pinMode(rightSensorPin, INPUT);
// Motors
pinMode(stepPin1, OUTPUT);
pinMode(dirPin1, OUTPUT);
pinMode(stepPin2, OUTPUT);
pinMode(dirPin2, OUTPUT);
Serial.println("System Started");
}
// --- Main Loop ---
void loop() {
// 1. Check Distance
int distance = getDistance();
// 2. Read Line Sensors (Switches)
// In simulation: HIGH means Switch is ON (Simulating "Line Detected")
int leftVal = digitalRead(leftSensorPin);
int rightVal = digitalRead(rightSensorPin);
// Debugging info
Serial.print("Dist: "); Serial.print(distance);
Serial.print("cm | L: "); Serial.print(leftVal);
Serial.print(" R: "); Serial.println(rightVal);
// 3. Logic Control
if (distance < obstacleThreshold) {
// PRIORITY 1: Obstacle Detected -> Turn Right to avoid
turnRight();
} else if (leftVal == HIGH && rightVal == LOW) {
// Line is on the Left -> Turn Left
turnLeft();
} else if (rightVal == HIGH && leftVal == LOW) {
// Line is on the Right -> Turn Right
turnRight();
} else {
// Path clear, no line detected (or centered) -> Go Forward
moveForward();
}
}