Im building a line follower robot and i add pid controller to the programme ..
this is the code i wrote for it ..can any one tell me the code is correct or incorrect...
If it is incorrect ,what are the corrections i should make ??
#include <PID_v1.h>
/********************************************************
*PID controlled Line Follower -By Fuzzy Mechs_2015 ©©©
********************************************************/
//Variables
boolean s_array[4]={false,false,false,false};
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
int waight=0;
//Define the aggressive and conservative Tuning Parameters
double aggKp=0.85, aggKi=0, aggKd=0.06;
double consKp=0.5, consKi=0, consKd=0.11;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);
//Mdriver Control Pins
int m1pwm=13;
int m2pwm=8;
int m1x=12;
int m1y=11;
int m2x=10;
int m2y=9;
int M1speed=0;
int M2speed=0;
void setup()
{
//initialize the variables we're linked to
Setpoint = 0;
//turn the PID on
myPID.SetMode(AUTOMATIC);
//Starting Serial commiunication
//Serial.begin(9600);
//Setting the pin modes
//TCRT5000 Sensor Input
pinMode(A4,INPUT);
pinMode(A5,INPUT);
pinMode(A6,INPUT);
pinMode(A7,INPUT);
// Mdriver OUTPUT pins
pinMode(13,OUTPUT);
pinMode(12,OUTPUT);
pinMode(11,OUTPUT);
pinMode(10,OUTPUT);
pinMode(9,OUTPUT);
pinMode(8,OUTPUT);
}
void read_sensor_values(){
Input=0;
M1speed=0;
M2speed=0;
//Reading Sensor Values
s_array[0]=digitalRead(A4);
s_array[1]=digitalRead(A6);
s_array[2]=digitalRead(A7);
s_array[3]=digitalRead(A5);
Input=Get_waight(s_array);
}
int Get_waight(boolean Sensor[4]){
int Error=0;
int waight;
if(Sensor[0]==true && Sensor[1]==false && Sensor[2]==false && Sensor[3]==false){
waight=3;
}
//setting waight value to 1 1 0 0
else if(Sensor[0]==true && Sensor[1]==true && Sensor[2]==false && Sensor[3]==false){
waight=2;
}
//setting waight value to 0 1 0 0
else if(Sensor[0]==false && Sensor[1]==true && Sensor[2]==false && Sensor[3]==false){
waight=1;
}
//setting waight value to 0 1 1 0
else if(Sensor[0]==false && Sensor[1]==true && Sensor[2]==true && Sensor[3]==false){
waight=0;
}
//setting waight value to 0 0 1 0
else if(Sensor[0]==false && Sensor[1]==false && Sensor[2]==true && Sensor[3]==false){
waight=-1;
}
//setting waight value to 0 0 1 1
else if(Sensor[0]==false && Sensor[1]==false && Sensor[2]==true && Sensor[3]==true){
waight=-2;
}
//setting waight value to 0 0 0 1
else if(Sensor[0]==false && Sensor[1]==false && Sensor[2]==false && Sensor[3]==true){
waight=-3;
}
Error=Setpoint-waight;
return Error;
}
void PID_Compute(){
myPID.SetOutputLimits(-255,+255);
myPID.SetSampleTime(1);
myPID.Compute();
}
void Motor_control(){
//Calculating the effective motor speed:
if(Output<0){
M1speed=abs(Output);
}
else if(Output>0){
M2speed=abs(Output);
}
else if(Output==0){
M1speed=85;
M2speed=85;
}
digitalWrite(12,HIGH);
digitalWrite(11,LOW);
digitalWrite(10,LOW);
digitalWrite(9,HIGH);
analogWrite(13,M1speed);
analogWrite(8,M2speed);
}
void loop(){
read_sensor_values();
PID_Compute();
Motor_control();
}