Very Simple Very Accurate Chassis Control Code Release

Yes, he’s controlling the voltage of the motors directly. Less math = more consistent results.

5 Likes

Can you explain what each variable is intended to do? You didn’t leave comments in the code for all of them and it would make it a lot more understandable.

No problem, sorry for the confusion.

  int errorLeft;
  int errorRight;
  float kp = 0.075;
  float kpTurn = 0.2;
  int acc = 5;
  int voltageLeft = 0;
  int voltageRight = 0;
  int signLeft;
  int signRight;

Here are my variables. errorLeft and errorRight are the error of each half of the base, which is calculated by subtracting the intended position from the current position every 20 milliseconds.

kp and kpTurn are the constants of proportionality, which basically means they’re a coefficient I multiply the error by to figure out the voltage the motor should go to. I’m not sure how experienced a programmer you are, but this video gives really good information about a basic P loop.

The kp values for forward / backward driving and for turning are most likely not the same value, and they need to be tuned for each robot. If the robot can’t get to the target without a high error, the values need to be increased, and if the robot stops jerkily or oscillates about the correct position, the values need to be decreased.

acc is the amount velCap is increased every time through the loop. Remember that velCap is not the voltage passed to the motors, it is the upper limit that can be passed. As the robot gathers more and more speed, the upper limit can be increased further and further without the wheels slipping. acc will also need to be tuned- too low a value will cause sluggish motions and too high a value won’t protect from slippage.

voltageLeft and voltageRight are the final voltages passed to the left and right drive motors. If voltageLeft is -68, the left base motors will be given a voltage of -68.

And signLeft and signRight are simply the signs of the left and right voltages- either 1 or -1. I check if signLeft equals signRight to determine if I’m supposed to be driving straight or turning, and I use it when capping the voltage to keep the signs right. signLeft and signRight can be calculated by dividing the error by the absolute value of the error (or the voltage by the absolute value of the voltage) at any time, but storing the value in a variable made it easier to access when I wrote the loop.

Let me know if you’re still confused- even though this loop is a lot simpler than most drive PID loops, it’s fairly hard to understand.

9 Likes

I have a PID control loop coded as well as a P-Loop coded. In fact, my PID has an integrated straight line control algorithm integrated within it. So lets say you veer off course slightly because of slippage, or a robot hits the rear of your robot or anything like that, it will automatically correct it’s position. I also have a turning PID separate from the straight line PID, its based on a gyro value. The straight line correction will only work with free spinning wheels btw.

I don’t have slew control, I didn’t even know that was a thing. So I understand your code, I just didn’t know what the variables were for.

4 Likes

Ok awesome! Actually if you’d be willing to dm me your straight line code that sounds kinda epic.

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.

17 Likes

Everyone knows real men use doubles, not floats!

2 Likes

@Anomaly, @theol0403 it is great that you have shared your code in an easy to read and copy&paste format, along with the clear to understand explanations!

This is what matters for the many beginner teams, for whom going to a github repository and extracting relevant code from a tangled codebase of another team might be too much of a challenge.

Since you asked about heading correction code, here is a relevant thread with a sample code:

@theol0403 one thing I noticed in your code is that you don’t check if the “dT” is 0 before dividing by it. You may want to do something about it, like not calculating derivative during the first step, because it could generate exception.

Another potential issue with using derivative is the measurement noise because if dT is a small number you can get roundoff errors and jumpy “D” component. A better solution may be to use V5 builtin get_actual_velocity() function. (RobotMesh reference)

8 Likes

Right, I forgot about that! Thanks for the correction. The best solution for that is to set a min refresh rate for the PID calculation, I will update my earlier post.

As for the jumpy dT, I think the best solution is to filter the derivative. I actually have it filtered by EMA filter in my code, but omitted it here for simplicity. I can’t use get-actual-velocity, because this is a generic PID controller which just does D on the Error.

Also, limiting the refresh rate for the loop, which ensures a more consistent dT, will help with this.

6 Likes

I also have straight line assist. Can confirm that it’s epic.

If you could dm that to me too, that would be great :smiley:

Straight-line assist is common in most of the chassis PIDs I’ve seen made from more experienced programmers. If you look on github (okapi, tabor’s, etc) you can find many people use it.

Its not too complicated - basically, you have one PID dedicated to the forward movement of the robot, and another one dedicated to keeping the robot straight.

With a generic PID implementation like I showed above, a simple loop might be:

PID distancePID(1, 0.1);
PID anglePID(0.01, 0.1);
double target = 100;

while(true) {
  //average of both sides
  double distance = (getLeftEnc() + getRightEnc()) / 2.0;
  //difference between left and right side
  double angle = getLeftEnc() - getRightEnc();
  //if left side is above right, angle will be positive, 
  //which we will use to turn the robot more to the left

  double distancePower = distancePid.calculate(target, distance);
  double anglePower = anglePid.calculate(0, angle);

  double leftPower = distancePower + anglePower;
  double rightPower = distancePower - anglePower;

  //now you can set motor power
  delay(10);
}

Edit: rename and fix

11 Likes

I wouldn’t mind sharing it to you, it’s on pros though so I would have to share my repository.

1 Like

Hi, I am programming for my team this year and I was going to use this and I was wondering how you can get PID to be a variable type. Do you have to be in PROS or do a #include or something? Please let me know. Thank you.

It’s not library code. It appears @theol0403 was referencing code from an earlier post.

1 Like

no I mean like a #include because when i put in "PID distancePID(1, 0.1); in PROS I get an error saying "unknown type name ‘PID’

I know what you meant. I was telling you that what you were trying to do was the wrong thing entirely. It’s not library code that you can #include. #include does nothing here. #include is barking up entirely the wrong tree. This is code that he wrote earlier in this thread. It isn’t a part of any file that you have when installing anyone’s IDE, so it can’t be #include’d as it doesn’t exist on your machine. The type “PID” won’t exist anywhere on your machine until you implement it. If you implement it in another file you could #include that file, but you yourself would still have to implement the type somewhere. Which is why I directed you to the post where he laid out the implementation for it.

5 Likes

I understand what you are saying now. Thank you for the clarification. What would be the best way to implement this. Do I make it a function or do I just copy paste the “Motor.voltage(leftPower);” ?

Copy his implementation of PID into a “PID.cpp” or similar being sure to replace any placeholders with real code, write a “PID.h” that forward declares everything in “PID.cpp”, then #include that “PID.h” from any file where you want to use the PID class. This is the usual method for implementing and integrating stuff in a multi-file C++ program.

Hmm, this makes it sound like you don’t know what it means to implement a class. The class implementation is the stuff after “class PID…” and any method declaration that starts with “PID::”. (C++ lets you put the full definition for methods outside of the class declaration, so long as you include a prototype for them in the class declaration. You then provide the full definition later on just like you would define a function, but you preface the method name with “yourclassnamehere::”.)

All that being said, unless you have a competition coming up soon, I think your time might be better used to go grab some general knowledge on C++ itself rather than these robot-specific applications. Sitting down with a beginner’s tutorial on C++ will also be faster than getting all of it drip-fed through forum responses :wink:

7 Likes

What is your take on using PID control when driving, in the driver control period, when going straight? To make driving easier when going straight?