#include <NewPing.h> // Ultrasonic sensor library
// Motor Pins for L298N
#define IN1 4 // Motor 1 Direction Pin 1
#define IN2 5 // Motor 1 Direction Pin 2
#define ENA 3 // Motor 1 Speed PWM Pin
#define IN3 6 // Motor 2 Direction Pin 1
#define IN4 7 // Motor 2 Direction Pin 2
#define ENB 8 // Motor 2 Speed PWM Pin
// Ultrasonic sensor setup
#define TRIG_PIN_FRONT 10
#define ECHO_PIN_FRONT 10
#define TRIG_PIN_LEFT 11
#define ECHO_PIN_LEFT 11
#define TRIG_PIN_RIGHT 12
#define ECHO_PIN_RIGHT 12
#define MAX_DISTANCE 200
NewPing USFront(TRIG_PIN_FRONT, ECHO_PIN_FRONT, MAX_DISTANCE);
NewPing USLeft(TRIG_PIN_LEFT, ECHO_PIN_LEFT, MAX_DISTANCE);
NewPing USRight(TRIG_PIN_RIGHT, ECHO_PIN_RIGHT, MAX_DISTANCE);
// PID control variables
int MaxSpeed = 255;
int BaseSpeed = 200;
float kp = 0.4; // Proportional gain - to be tuned
float ki = 0.02; // Integral gain - to be tuned
float kd = 0.5; // Derivative gain - to be tuned
int targetDistance = 30; // Target safe distance from obstacles in cm
int lastError = 0;
float integral = 0;
int distance1;
int distance2;
int distance3;
void setup() {
Serial.begin(115200);
// Motor control pins as outputs
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(ENA, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(ENB, OUTPUT);
}
void loop() {
distance1 = USFront.ping_cm();
distance2 = USLeft.ping_cm();
distance3 = USRight.ping_cm();
if (distance1==0)
{
distance1=MAX_DISTANCE;
}
if (distance2==0)
{
distance2=MAX_DISTANCE;
}
if (distance3==0)
{
distance3=MAX_DISTANCE;
}
Serial.println("Front :");
Serial.println(distance1);
Serial.print(" cm, Left: ");
Serial.println(distance2);
Serial.print(" cm, Right: ");
Serial.println(distance3);
int distanceFront = USFront.ping_cm();
int distanceLeft = USLeft.ping_cm();
int distanceRight = USRight.ping_cm();
// Calculate error based on distance from the target
int error = targetDistance - distanceFront;
integral += error; // Accumulate the integral term
int derivative = error - lastError;
lastError = error;
int pidAdjustment = kp * error + ki * integral + kd * derivative;
int leftMotorSpeed = BaseSpeed + pidAdjustment;
int rightMotorSpeed = BaseSpeed - pidAdjustment;
// Apply speed limits
leftMotorSpeed = constrain(leftMotorSpeed, 0, MaxSpeed);
rightMotorSpeed = constrain(rightMotorSpeed, 0, MaxSpeed);
if (distanceFront > 0 && distanceFront < targetDistance) {
// Obstacle ahead, adjust using PID
setMotorSpeeds(leftMotorSpeed, rightMotorSpeed);
} else if (distanceLeft > 0 && distanceLeft < 20) {
// Obstacle on the left side, turn right
turnRight();
} else if (distanceRight > 0 && distanceRight < 20) {
// Obstacle on the right side, turn left
turnLeft();
} else {
// Clear path, move forward
moveForward();
}
delay(100); // Small delay for sensor stability
}
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
// Set motor directions for forward movement
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
analogWrite(ENA, leftSpeed);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
analogWrite(ENB, rightSpeed);
}
void moveForward() {
setMotorSpeeds(BaseSpeed, BaseSpeed);
}
void turnRight() {
// Left motor stops, right motor moves forward
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
analogWrite(ENA, 0);
digitalWrite(IN3, HIGH);
// digitalWrite(IN4, LOW);
analogWrite(ENB, BaseSpeed);
}
void turnLeft() {
// Right motor stops, left motor moves forward
digitalWrite(IN1, HIGH);
// digitalWrite(IN2, LOW);
analogWrite(ENA, BaseSpeed);
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
analogWrite(ENB, 0);
}
void stopMotors() {
// Stop both motors
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
analogWrite(ENA, 0);
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
analogWrite(ENB, 0);
}