PID for Autonomous in Vex Coding Studio

So, recently I have found out what PIDs are. I do understand how it works logically, but I cannot seem to understand how to write it in code with Vex Coding Studio. Also, if I were to do PIDs, would I control the motor errors with voltage, rotations, etc. ? I really need help with PIDs in autonomous but I would appreciate it if anyone could help with PIDs for DR4B.

1 Like

First please use VEXcode, Vex Coding Studio was replaced be VEXcode and the coding is virtually the same.

Before doing custom PID know that v5 has built-in PID. What is there by default is by no means the best but can save time and effort. You can use custom PID on some system and built-in PID on the rest. Although it is best to use custom PID everywhere, because it gives you more fine control over how the PID functions.

For your first question, you should apply the error as voltage.

How program PID

Remember it doesnâ€™t always have to be PID. Just using the P part is sometimes all you need.
Anyways, here is an example I wroteâ€¦

Declarationsâ€¦

``````double kP = 1.0, kI = 0.01, kD = 0.1;
long preUpdate = 0, timeout = 50;
double prePosition = actualPosition;
double error, speed, integral = 0;
``````

In the loopâ€¦

``````error = target - actualPosition;

long elapsedTime = vexSystemTimeGet() - preUpdate;
if(elapsedTime >= timeout)
{
speed = (actualPosition - prePosition) * (1000.0 / elapsedTime);
integral += error * (1000.0 / elapsedTime);
prePosition = actualPosition;
preUpdate = vexSystemTimeGet();
}

motorVoltage = error * kP - speed * kD + integral * kI;
``````

Here it calculates error and speed. Then it update the integral, the `I` component. For speed ( `D` ) and integral ( `I` ) they are time based to they are in an if statement that run at set a interval. Then all the values get add up, which is what you want the voltage to be set to. The `kP, kI, kD` variable are what you want to change to tune your PID.

Also, since you would probably need to use this code numerous time, it would make sense to make this a class. I encourage you to do that part yourself to get better at coding (I donâ€™t know you skill level though, you might be a pro). If you need any help though feel free to ask.

5 Likes

Because we want to be delicate with our tray when scoring a stack, we coded a tray PID on our robot. We coded PID to give us the options of how we can control the tray, but we ended up using only KP and KI of the control loop. The code was rushed because we were cutting it close on time for our competition at Space City. Whenever running your own PID in vexcode or VCS, use voltage as anything other than that will cause the motor to run two PIDâ€™s at the same time (Your PID and the motorâ€™s internal PID) which is no bueno and can cause issues with the motor. I have condensed the code to only run the tray PID for this post:

``````double trayDesired;
double trayKP = 0.0;
double trayPrevError = 0.0;
double trayDerivative = 0.0;
double trayKD = 0.0;
double totalError = 0.0;
trayKI = 0.0;
int trayError;
while(1==1){

//Controller Input
if(Controller1.ButtonL1.pressing()){
intaking = false;
if(outedCube){
trayDesired = 166;
trayKP = 0.00002;
trayKD = 0;
trayKI = 0.00000005;
}
}
else{
trayDesired = 45;
trayKP = 0.02;
trayKD = 0;
trayKI = 0;
intaking = true;
}

//Tray PID
trayError = pow((TrayPot.angle(degrees) - trayDesired),3);
trayDerivative = trayError - trayPrevError;

if( ((TrayPot.angle(degrees) - trayDesired) < 5.0 && (TrayPot.angle(degrees) - trayDesired) > -5.0) )
totalError = 0;
else totalError += trayError;

TrayMotor.spin(fwd, (trayError * trayKP + trayDerivative * trayKD + totalError * trayKI), voltageUnits::volt);
}

trayPrevError = trayError;
}
``````

EDIT//: I know that much of my code has bad habits, but it was rushed and I did not care how bad it looked. I cared most about functionality.

5 Likes

Ok thank you. I didnâ€™t hear about VEXcode so Iâ€™ll definitely check out. Do you know any good forums or websites that teach classes for VEXcode?

Thank you so much for the example, I will look over the code to see how it works. Additionally, did you use the encoders built into the motor or a 3-wired shaft encoder? And which one would be more accurate for autonomous?

The problem with you code is the timing. You have a `vex::task::sleep(20)`, but, especially with other task, it isnâ€™t always 20 milliseconds (additionally the code itself also takes time to run). If you implemented something like my code example the timing would be a lot more precise. You donâ€™t need the if statement, but divide by elapsed time. That way your PID units would be based in an actual time interval and not imprecise loop cycles.

There are a lot of tutorials for vexcode blocks, but to be honest I like the text version better. There are no tutorials for the text version, so over december break I will work on having tutorials. If you are doing an autonomous I would recommend using shaft encoders on a dummy, non-powered wheel. It would be more accurate at tracking distance, but I would suggest having the encoder be inside of the robot and not outside of the robot or else you will encounter the same mistakes I made at competition (Defense broke our encoders at the event, destroying our autonomous). In addition, you can see

``trayError = pow((TrayPot.angle(degrees) - trayDesired),3);``

This was for my tray bot so I wanted the tray error very small so the integral will take care of the rest, so I did an exponential curve for the error. I would suggest replacing that to a mere

``trayError = TrayPot.angle(degrees) - trayDesired;``

Thats very smart and intuitive! I was a bit confused until you told me that it was actually using the internal clock. Genius!

1 Like

Ok thank you. Also, what is the purpose of the tray PID? Would this kind of PID also be useful for a DR4B?

I donâ€™t know if you understood it correctly or not.

The v5 internal clock is quite accurate, which is what cpu uses. (Might be a bit more complicated but that doesnâ€™t matter.) The thing is that the cpu only really makes sure the delay is at least 20 milliseconds.

1 Like

I looked at the example code, and I was wondering if you could explain to me why timeout variable is 50 and how I would use this voltage change on a motor. I am a beginner in Vex Coding Studio and VEXcode so I donâ€™t know how to make it into a class.

The timeout variable is delay between each update, in this case 50 milliseconds or 0.05 seconds.

If you do not know what classes are at all then you will need to learn about them. Here is a link to a tutorial or just google cpp class tutorial and you will get lots of results. At that you can ask question if you have any. If you already know about classes can you be more specific.

1 Like

I donâ€™t know anything about classes, so I canâ€™t really be any more specific. Thanks for the tutorials, Iâ€™ll check them out!

I know it has been a while but I completed my tray PID but for some reason, it only works when going to the target. When I try to go to the initial position, the motor does not move. After looking back to your code, I realized that you have a kP, kI, and kD value for tilting the column forward and backwards. I was wondering what outedCube is and how it knows the tray is tilting forward. Pardon if I am saying anything wrong but I just assumed some of this.

If you are having your tray be at its initial position which is to intake cubes, I would suggest kP having a value and kI and kD having a value of 0 just to prevent motor burnout over time. outedCube was another tray angle that we found that would split our intake at its widest point, making the intake have completely no contact with the cubes allowing us to back away freely without any problems. Hopefully this makes sense. Also,

Bear in mind that

``trayError = pow((TrayPot.angle(degrees) - trayDesired),3);``

(Basically error^3) is existent for being a bit more of an exponential velocity. This allows us to have acceleration that is a slope instead of a parallel line, upon being graphed, which would make the cubes sway almost none when the tray is moving to be completely vertical.

2 Likes