#define m1 6 //Right Motor MA1
#define m2 5 //Right Motor MA2
#define m3 4 //Left Motor MB1
#define m4 3 //Left Motor MB2
#define e1 7 //Right Motor Enable Pin EA
#define e2 2 //Left Motor Enable Pin EB
//*5 Channel IR Sensor Connection*//
#define ir1 A0
#define ir2 A1
#define ir3 A2
#define ir4 A3
#define ir5 A4
//*//
// PID constants
float Kp = 1.0; // Proportional
float Ki = 0.0; // Integral
float Kd = 0.0; // Derivative
// Variables for PID calculations
float previousError = 0;
float integral = 0;
float error = 0;
float derivative = 0;
float pidOutput = 0;
void setup() {
pinMode(m1, OUTPUT);
pinMode(m2, OUTPUT);
pinMode(m3, OUTPUT);
pinMode(m4, OUTPUT);
pinMode(e1, OUTPUT);
pinMode(e2, OUTPUT);
pinMode(ir1, INPUT);
pinMode(ir2, INPUT);
pinMode(ir3, INPUT);
pinMode(ir4, INPUT);
pinMode(ir5, INPUT);
}
void loop() {
// Reading sensor values
int s1 = digitalRead(ir1); // Left Most Sensor
int s2 = digitalRead(ir2); // Left Sensor
int s3 = digitalRead(ir3); // Middle Sensor
int s4 = digitalRead(ir4); // Right Sensor
int s5 = digitalRead(ir5); // Right Most Sensor
// Calculate the "error" based on sensor inputs
error = (s1 * -2) + (s2 * -1) + (s3 * 0) + (s4 * 1) + (s5 * 2);
// Calculate the integral and derivative
integral += error;
derivative = error - previousError;
// Calculate the PID output
pidOutput = Kp * error + Ki * integral + Kd * derivative;
// Adjust motor speeds based on PID output
int baseSpeed = 200; // Base speed for the motors
int leftSpeed = baseSpeed + pidOutput; // Left motor speed
int rightSpeed = baseSpeed - pidOutput; // Right motor speed
// Ensure the motor speeds stay within bounds (0-255)
leftSpeed = constrain(leftSpeed, 0, 255);
rightSpeed = constrain(rightSpeed, 0, 255);
// Set motor speeds
analogWrite(e1, rightSpeed); // Right motor speed
analogWrite(e2, leftSpeed); // Left motor speed
// Set motor direction
if (pidOutput > 0) {
// Turn right
digitalWrite(m1, HIGH);
digitalWrite(m2, LOW);
digitalWrite(m3, LOW);
digitalWrite(m4, HIGH);
} else if (pidOutput < 0) {
// Turn left
digitalWrite(m1, LOW);
digitalWrite(m2, HIGH);
digitalWrite(m3, HIGH);
digitalWrite(m4, LOW);
} else {
// Go straight
digitalWrite(m1, HIGH);
digitalWrite(m2, LOW);
digitalWrite(m3, HIGH);
digitalWrite(m4, LOW);
}
// Save the current error for the next iteration
previousError = error;
}
