PID is a great way to learn OOP. Since it has large volume of code, but is very similar across multiple implementations, it is easy to see how creating a generic PID controller that you can implement throughout your whole code is a better solution than copying the same lines over and over.
Using a generic PID controller for your above code would clean it up nicely, and make it easy to add a second angle correction layer. I will explain that after this post if mvas has not yet.
At a high level, OOP is not specific to a language, syntax, or implementation. It is simply the concept of creating a unit of code with settings, inputs, and outputs, that can be used in different locations as self-contained objects.
I will show two ways to do this with PID. One is pure C, which I made early season based off QCC2’s code, and the other is a full c++ class. I have removed I for simplicity, though it can be added easily.
For both these methods, the intention is to create a single PID utility that can be used independently in various locations.
C Method:
First of all, we need a way to store the PID variables in a container. In C, the best way to do this is with struct. (btw this is the closest you can get to OOP with C and is common practice). Let’s create a container that stores all the info PID needs
typedef struct {
double kP = 0;
double kD = 0;
int minDt = 10;
double error = 0;
double derivative = 0;
double lastError = 0;
double lastTime = 0;
double output = 0;
} pidStruct_t;
Now we have a way to create separate PID instances.
Now, we just need some functions to do the work for us. First, we can make a function to initialize (in OOP terms, construct) the structure using custom values.
void pidInit (pidStruct_t* pid, double kP, double kD, int minDt = 10) {
pid->kP = kP;
pid->kD = kD;
pid->minDt = minDt;
pid->lastTime = pros::c::millis();
}
This gives us a way to initialize our structure. Now we can do
pidStruct_t myPid;
pidInit(&myPid, 1, 0.1);
Now myPid contains all the information it needs.
Finally, we can do the calculations for PID in one function.
double pidCalculate(pidStruct_t* pid, double target, double current) {
pid->error = target - current; //calculate error
//calculate delta time
double dT = pros::c::millis() - pid->lastTime;
//abort if dt is too small
if(dT < pid->minDt) return pid->output;
//calculate derivative
pid->derivative = (pid->error - pid->lasterror) / dT;
//calculate pid output
pid->output = (pid->error * pid->kP) + (pid->derivative * pid->kD);
//limit output
if(abs(pid->output) > 127) output = sgn(pid->output) * 127;
//save values
pid->lastError = pid->error;
pid->lastTime = pros::c::millis();
return pid->output;
}
Now we have a generic PID utility we can use anywhere we want. Here is an example:
pidStruct_t myPid;
pidInit(&myPid, 1, 0.1);
while(true) {
double motorPower = pidCalculate(&myPid, target, current);
...
}
I hope this makes sense, and you can see how you could use this to simplify and clean PID implementations.
C++ Method:
With C++, you can go even cleaner. If you want to learn more about classes, go to learncpp.com.
Here is the header for the PID class I made:
class PID {
private:
double m_kP = 0;
double m_kD = 0;
int m_minDt = 10;
okapi::Timer m_timer;
double m_error = 0;
double m_lastError = 0;
double m_lastTime = 0;
double m_derivative = 0;
double m_output = 0;
public:
PID(double kP, double kD, int minDt = 10);
double calculateErr(double);
double calculate(double, double);
double getError();
void reset();
};
C++ makes it easier to make more functions, and it is very easy to expand the functionality of this PID utility. Here is the implementation of the functions. Notice how the initialization of the pid values are handled by the constructor.
PID::PID(double kP, double kD, int minDt) :
m_kP(kP), m_kD(kD), m_minDt(minDt) {
m_lastTime = m_timer.millis().convert(millisecond);
}
double PID::calculateErr(double ierror) {
m_error = ierror;
//calculate delta time
double dT = m_timer.millis().convert(millisecond) - m_lastTime;
//abort if dt is too small
if(dT < m_minDt) return m+output;
//calculate derivative
m_derivative = (m_error - m_lastError) / dT;
//calculate output
m_output = (m_error * m_kP) + (m_derivative * m_kD);
//limit output
if(std::abs(m_output) > 127) output = sgn(m_output) * 127;
//save values
m_lastTime = m_timer.millis().convert(millisecond);
m_lastError = m_error;
return m_output;
}
double PID::calculate(double target, double current) {
return calculateErr(target - current);
}
double PID::getError() {
return m_error;
}
void PID::reset() {
m_error = 0;
m_lastError = 0;
m_lastTime = m_timer.millis().convert(millisecond);
m_derivative = 0;
m_output = 0;
}
Now, you can type
PID myPid(1, 0.1);
while(true) {
double motorPower = myPid.calculate(target, current);
...
}
That could be implemented into your chassis control code quite easily, or it could be used for any other subsystem that needs PID control. Since PID is often always the same, it is a good solution to create a generic PID utility that you can use everywhere.
Let me know if you have any questions, I got a little carried away with this =)
Edit:
Implemented minDt and abort if dT is too small.