// Pin Definitions for Motor 1 (Encoder Motor 1)
#define ENA1 9
#define IN1_1 7
#define IN1_2 8
#define ENCODER1_A 2
#define ENCODER1_B 3
// Pin Definitions for Motor 2 (Encoder Motor 2)
#define ENA2 10
#define IN2_1 4
#define IN2_2 5
#define ENCODER2_A 6
#define ENCODER2_B 11
// Pin Definitions for Ultrasonic Sensor
#define TRIG_PIN 12
#define ECHO_PIN 13
#define OBSTACLE_DISTANCE 60
#define STOP_DISTANCE 20
// Encoder tracking
volatile long encoderPosition1 = 0;
volatile long encoderPosition2 = 0;
int motorSpeed1 = 150;
int motorSpeed2 = 150;
// Counter for obstacle detections
int obstacleCount = 0;
void setup() {
// Motor and encoder pin setups
pinMode(ENA1, OUTPUT);
pinMode(IN1_1, OUTPUT);
pinMode(IN1_2, OUTPUT);
pinMode(ENCODER1_A, INPUT_PULLUP);
pinMode(ENCODER1_B, INPUT_PULLUP);
pinMode(ENA2, OUTPUT);
pinMode(IN2_1, OUTPUT);
pinMode(IN2_2, OUTPUT);
pinMode(ENCODER2_A, INPUT_PULLUP);
pinMode(ENCODER2_B, INPUT_PULLUP);
// Ultrasonic sensor setup
pinMode(TRIG_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);
// Attach encoder interrupts
attachInterrupt(digitalPinToInterrupt(ENCODER1_A), readEncoder1, CHANGE);
attachInterrupt(digitalPinToInterrupt(ENCODER2_A), readEncoder2, CHANGE);
Serial.begin(9600);
}
void loop() {
long distance = measureDistance();
if (distance > 0 && distance <= STOP_DISTANCE) {
stopMotors();
Serial.println("Obstacle detected within stop distance. Motors stopped.");
delay(500);
return;
}
if (distance > STOP_DISTANCE && distance <= OBSTACLE_DISTANCE) {
obstacleCount++;
if (obstacleCount % 2 != 0) { // Odd obstacle count: turn right
Serial.println("Obstacle detected! Turning right...");
turnRight(170); // Perform right turn
} else { // Even obstacle count: turn left
Serial.println("Obstacle detected! Turning left...");
turnLeft(170); // Perform left turn
}
delay(500);
} else {
runMotorsForward();
}
}
long measureDistance() {
digitalWrite(TRIG_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);
long duration = pulseIn(ECHO_PIN, HIGH);
return duration * 0.034 / 2; // Convert time to distance in cm
}
void turnRight(int angle) {
long targetTicks = encoderPosition1 + calculateEncoderTicks(angle);
unsigned long startTime = millis();
while (encoderPosition1 < targetTicks && millis() - startTime < 3000) { // 3s timeout
digitalWrite(IN1_1, HIGH);
digitalWrite(IN1_2, LOW);
analogWrite(ENA1, motorSpeed1);
digitalWrite(IN2_1, LOW);
digitalWrite(IN2_2, HIGH);
analogWrite(ENA2, motorSpeed2);
}
stopMotors();
}
void turnLeft(int angle) {
long targetTicks = encoderPosition2 + calculateEncoderTicks(angle);
unsigned long startTime = millis();
while (encoderPosition2 < targetTicks && millis() - startTime < 3000) { // 3s timeout
digitalWrite(IN1_1, LOW);
digitalWrite(IN1_2, HIGH);
analogWrite(ENA1, motorSpeed1);
digitalWrite(IN2_1, HIGH);
digitalWrite(IN2_2, LOW);
analogWrite(ENA2, motorSpeed2);
}
stopMotors();
}
void runMotorsForward() {
digitalWrite(IN1_1, HIGH);
digitalWrite(IN1_2, LOW);
analogWrite(ENA1, motorSpeed1);
digitalWrite(IN2_1, HIGH);
digitalWrite(IN2_2, LOW);
analogWrite(ENA2, motorSpeed2);
Serial.println("No obstacle detected. Motors running forward.");
}
void stopMotors() {
digitalWrite(IN1_1, LOW);
digitalWrite(IN1_2, LOW);
analogWrite(ENA1, 0);
digitalWrite(IN2_1, LOW);
digitalWrite(IN2_2, LOW);
analogWrite(ENA2, 0);
}
long calculateEncoderTicks(int angle) {
int ticksPerRevolution = 20; // Update based on your encoder
int wheelDegreesPerRevolution = 360;
return (ticksPerRevolution * angle) / wheelDegreesPerRevolution;
}
void readEncoder1() {
if (digitalRead(ENCODER1_A) == digitalRead(ENCODER1_B)) {
encoderPosition1++;
} else {
encoderPosition1--;
}
}
void readEncoder2() {
if (digitalRead(ENCODER2_A) == digitalRead(ENCODER2_B)) {
encoderPosition2++;
} else {
encoderPosition2--;
}
}
I ran the above code on an Arduino Mega or Mega 2560 board, but when the obstacle is detected by the ultrasonic sensor for the first time, the motors make a 170-degree left turn. However, when the obstacle is detected for the second time, the motors make a turn of more than 170 degrees.
