PID Control - An Example
Below is an example of a PID control function and its implementation. We highly recommend that you write your own PID control code to reinforce your learning and gain some confidence. However, we understand that being able to see an example is crucial for learning so we will do our best to break down the concepts used in this code.
Please note that this is definitely not the best PID code, we tried to keep it simple and understandable. Furthermore, all the constant values (kp, ki, kd, etc.) are random guessed values, you will have to find the right constant values for your robot through trial and error.
This PID code shows an example of a function that could be used in autonomous mode to perform PID forwards/backwards movement and turns.
The PID Calculation Function:
float t_kp;
float t_ki;
float t_kd;
int error = 0;
int prev_error = 0;
int integral = 0;
int derivative = 0;
float power = 0;
void setConstants(float kp, float ki, float kd){ //function to set constants
t_kp = kp;
t_ki = ki;
t_kd = kd;
}
//asks for the movement target, the input sensor, the integral activation point, and the integral max
float calc (int target, float input, int integralKI, int maxI){
prev_error = error; //stores previous error for calculating D
error = target - input; //calculates error for calculating P and D
if(std::abs(error) < integralKI){ //only lets I increment within a certain range of the target
integral += error; // a certain range of the target
}
else{
integral = 0;
}
if(integral >= 0){
integral = std::min(integral, maxI) //keeps integral within a limt
}
else{
integral = std::max(integral, -maxI);
}
derivative = error - prev_error; //calculates D
power = t_kp*error + t_ki*integral + t_kd*derivative; //scales with constants to find power
return power; //returns the calculated power
}
void forwardMove(int target){
setConstants(0.39, 0.15, 0.01);
float voltage;
float encoder_average;
int count = 0;
reset_encoders(); //resets the encoders for each movement
while(true){
encoder_average = (leftDrive.get_position() + rightDrive.get_position()) / 2;
voltage = calc(target, encoder_average, 200, 20);
chas_move(voltage, voltage); //(sets power to the chassis)
if (abs(target - encoder_average) <= 3) count++; //increases the counter when within 3 ticks
if (count >= 28) break; //breaks out of while loop if approx 300 milliseconds within the 3 tick range
delay(10);
}
chas_move(0,0); //stops chassis after loop
}
void turn(int target){
setConstants(1.8, 0.38, 0.01);
float voltage;
float position;
int count = 0;
while(true){
position = imu.get_rotation();
voltage = calc(target, position, 10, 20)
chas_move(voltage, -voltage);
if (abs(target - position) <= 1.5) count++;
if (count >= 28) break;
delay(10);
}
chas_move(0,0);
}
|