Very Simple Very Accurate Chassis Control Code Release

VEX V5 Robotics Competition VRC > Tower Takeover (19/20)
1

Most everyone struggles with accurate base control in auton. At the highest level, 5225 developed a 3 tracking wheel solution that allowed them to set the world record in programming skills last year, and a handful of teams including 8059 and 139 have replicated their “odometry” based on a document they released last year. Team 5225 Introduction to Position Tracking Document

But at a less extreme level, for people just looking to take a step up from move.relative you may like the base control code I developed this year. Before I go any farther, I’d like to openly admit that I’m not a good coder in any capacity and my conventions and whatnot may be sloppy, but I nevertheless won 8 of my 11 autonomous periods at worlds and was overall very happy with my chassis control in auton. And when my robot did miss, it was entirely because I didn’t spend enough time tuning my constants. The results I got on the competition field were perfectly replicable on all practice fields, regardless of antistatic.

The basic philosophy of my code is that the only possible reason an encoder value would not be representative of the true position of the robot is if the wheels slip or lift off the ground. As such, code that prevents the robot from accelerating or decelerating too fast and thus has no wheel slippage results in encoder values that are perfectly representative of the true position of the robot. To prevent too high an acceleration value, I used slew rate control when speeding up and a P loop when slowing down. 9605 and 169 use a similar concept (though I haven’t seen their code) and PID with slew rate has been a concept forever, so I’m not trying to take credit for anyone else’s work. But here is mine.

int velCap; //velCap limits the change in velocity and must be global
int targetLeft;
int targetRight;

void drivePIDFn(void*){
  leftFrontDrive.tare_position(); //reset base encoders
  rightFrontDrive.tare_position();
  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;
  delay(20);
  while(isAuton){
    errorLeft = targetLeft - leftFrontDrive.get_position(); //error is target minus actual value
    errorRight = targetRight - rightFrontDrive.get_position();

    signLeft = errorLeft / abs(errorLeft); // + or - 1
    signRight = errorRight / abs(errorRight);

    if(signLeft == signRight){
      voltageLeft = errorLeft * kp; //intended voltage is error times constant
      voltageRight = errorRight * kp;
    }
    else{
      voltageLeft = errorLeft * kpTurn; //same logic with different turn value
      voltageRight = errorRight * kpTurn;
    }

    velCap = velCap + acc;  //slew rate
    if(velCap > 115){
      velCap = 115; //velCap cannot exceed 115
    }

    if(abs(voltageLeft) > velCap){ //limit the voltage
      voltageLeft = velCap * signLeft;
    }

    if(abs(voltageRight) > velCap){ //ditto
      voltageRight = velCap * signRight;
    }

    leftBackDrive.move(voltageLeft); //set the motors to the intended speed
    leftFrontDrive.move(voltageLeft);
    rightBackDrive.move(voltageRight);
    rightFrontDrive.move(voltageRight);

    delay(20);
  }
}

void drive(int left, int right){
  targetLeft = targetLeft + left;
  targetRight = targetRight + right;
  velCap = 0; //reset velocity cap for slew rate
}

In auton, to drive 100 ticks forward, I would write drive(100, 100); and then put in an appropriate delay. To turn 100 ticks, I would write drive(100, -100);.

How does this code work? Let me break it down.

If one of my auton commands is drive(100, 100); the drive function will set the targetLeft and targetRight globals to their current values plus 100 and set the velCap global to 0. Then, in the PID loop (really just a P loop in this case), the error will be the target value minus the real value (100 if the robot has not moved) and will try set the voltage on either side of the base to 100 * kp. Meanwhile, the velCap increments by a little bit each time through the loop and limits the maximum voltage the motor can receive. This means the robot will accelerate smoothly with a slowly increasing voltage because of the gradual increase in the value of velCap and will decelerate smoothly as the robot approaches the target and the error decreases.

This is a lot of information to take in at once, so be sure to read through the above paragraph and the code a few times and ask any questions below if you’re still confused. :slight_smile:

To tune the loop for your, you will need to sit down with your robot on a practice field and adjust the kp, kpTurn, and acc values. The acc value needs to be tuned for smooth acceleration, and the kp and kpTurn values need to be tuned so that the error value is very close to 0 after the motion is complete. In my worlds code, I also used a constant I value that I had to tune, and that I omitted for simplicity. If you want to use an I or D value as well, obviously that will need to be tuned too.

And that’s it! I won’t have much need for drive PID since I’m graduating this year, but hopefully some of you can benefit from my trials and failures. And if you have any questions, be sure to ask!

Good luck!

2

Thanks Anomaly. I will have my non-coders take a look at this.

3

I did something like this in Starstruck. It was a lot more complicated though, having functions for getting up to speed and slowing down. Thank you for making it a lot easier.

4

Are you directly controlling the voltage of the V5 motors and not using the functions that control the RPM of the motors?

5

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

6

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.

7

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.

8

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.

9

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

10

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.

11

Everyone knows real men use doubles, not floats!

12

@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)

13

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.

14

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

15

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

16

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

17

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

18

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.

19

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

20

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’