Dear All I used some one else program to run my PID line follower so it does not travel on the line please advice any issue with my modified codes
#define Left_Dir1 5
#define Left_Dir2 7
#define Left_Speed 6
#define Right_Dir1 2
#define Right_Dir2 4
#define Right_Speed 3
#define buzzer A4
#define button A5
int sensor[5] = { 8, 9, 10, 11, 12 }; // Pin assignment
int sensorReading[5] = { 0 }; // Sensor reading array
float activeSensor = 0; // Count active sensors
float totalSensor = 0; // Total sensor readings
float avgSensor = 3.0; // 4.5 Average sensor reading
//72 //8 2.5 255/2.5 72
float Kp = 102; // Max deviation = 6-3.5 = 2.5 || 255/3.5 = 102
float Ki = 0.00015;//0.00015
float Kd = 5;//5
float error = 0;
float previousError = 0;
float totalError = 0;
float power = 0;
int PWM_Right, PWM_Left;
void setup(){
Serial.begin(9600);
Serial.println( "test");
pinMode(Left_Dir1, OUTPUT);
pinMode(Left_Dir2, OUTPUT);
pinMode(Left_Speed, OUTPUT);
pinMode(Right_Dir1, OUTPUT);
pinMode(Right_Dir2, OUTPUT);
pinMode(Right_Speed, OUTPUT);
for (int i = 0; i < 5; i++) {
pinMode(sensor[i], INPUT);
}
Stop();
Forward();
}
void loop(){
lineFollow();
}
void Forward(){
digitalWrite(Left_Dir1, LOW);
digitalWrite(Left_Dir2, HIGH);
digitalWrite(Right_Dir1, LOW);
digitalWrite(Right_Dir2, HIGH);
}
void Stop(){
analogWrite(Left_Speed, 0);
analogWrite(Right_Speed, 0);
}
void PID_program(){
readSensor();
previousError = error; // save previous error for differential
//Serial.println( previousError);
//4.5
error = avgSensor - 3.0; // Count how much robot deviate from center
totalError += error; // Accumulate error for integral
//Serial.println( totalError);
power = (Kp * error) + (Kd * (error - previousError)) + (Ki * totalError);
//Serial.println( power);
if ( power > 255 ) {
power = 255.0; //255.0
}
if ( power < -255.0 ) {
power = -255.0; //-255.0
}
if (power < 0) // Turn left
{
PWM_Right = 255;//255
PWM_Left = 255 - abs(int(power));//255
}
else // Turn right
{
PWM_Right = 255 - int(power);//255
PWM_Left = 255;//255
}
}
void lineFollow(void) {
PID_program();
analogWrite(Left_Speed, PWM_Left);
analogWrite(Right_Speed, PWM_Right);
}
void readSensor(void) {
for (int i = 0; i < 5; i++)
{
sensorReading[i] = !digitalRead(sensor[i]);
Serial.println( sensorReading[i]);
if (sensorReading[i] == 1) {
activeSensor += 1;
}
totalSensor += sensorReading[i] * (i + 1);
}
avgSensor = totalSensor / activeSensor;
activeSensor = 0; totalSensor = 0;
}
void testSensor(void) {
for (int i = 0; i < 8; i++) {
Serial.print(!digitalRead(sensor[i]));
}
Serial.println("");
delay(20);
}
boolean checkSensor() {
boolean state = 0;
for (int i = 0; i < 8; i++) {
if (digitalRead(sensor[1]) == 0) {
state = 1;
}
}
return state;
}