Scribd
Upload
11 views2 pages

L.F.R With Pid

Uploaded by

ALI RAZZAQ
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
11 views2 pages

L.F.R With Pid

Uploaded by

ALI RAZZAQ
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 2

#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;
}

You might also like