Hi, recently I’ve been digging into PID, and while I do understand the basic moving forward/backward, I don’t understand how I would implement and develop a turning PID. Can anyone show some of their sample code or offer some explanations? Thank you!
2 Likes
This is what I have and it is almost a copy of Connors,
/*----------------------------------------------------------------------------*/
/* */
/* Module: main.cpp */
/* Author: VEX */
/* Created: Thu Sep 26 2019 */
/* Description: Competition Template */
/* */
/*----------------------------------------------------------------------------*/
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// Controller1 controller
// LeftMotor motor 1
// RightMotor motor 2
// Claw motor 3
// Arm motor 4
// ---- END VEXCODE CONFIGURED DEVICES ----
#include "vex.h"
using namespace vex;
// A global instance of competition
competition Competition;
// define your global instances of motors and other devices here
/*---------------------------------------------------------------------------*/
/* Pre-Autonomous Functions */
/* */
/* You may want to perform some actions before the competition starts. */
/* Do them in the following function. You must return from this function */
/* or the autonomous and user control tasks will not be started. This */
/* function is only called once after the V5 has been powered on and */
/* not every time that the robot is disabled. */
/*---------------------------------------------------------------------------*/
void pre_auton(void) {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
}
bool enableDrivePID = true;
bool enableArmPID = true;
// Settings
const double kP = 1.0;
const double kI = 0.0;
const double kD = 0.0;
const double armkP = 0.0;
const double armkI = 0.0;
const double armkD = 0.0;
const double turnkP = 1.0;
const double turnkI = 0.0;
const double turnkD = 0.0;
int error; // sensor - desired
int prevError = 0; // pos from last loop
int derivative; // diference bettween error and prev error : Speed
int totalError = 0; // Integral Total error = totalError + error, every loop
int turnError; // sensor - desired
int turnPrevError = 0; // pos from last loop
int turnDerivative; // diference bettween error and prev error : Speed
int turnTotalError = 0; // Integral Total error = totalError + error, every loop
int armError; // sensor - desired
int armPrevError = 0; // pos from last loop
int armDerivative; // diference bettween error and prev error : Speed
int armTotalError = 0; // Integral Total error = totalError + error, every loop
// Settings
bool resetDrive = false;
bool resetArm = false;
int desiredValue;
int turnDesiredValue;
int armDesiredValue;
int maxTurnIntegral = 300; // These cap the integrals
int maxIntegral = 300;
int integralBound = 3;
double signnum_c(double x) {
if (x > 0.0) return 1.0;
if (x < 0.0) return -1.0;
return x;
}
int drivePID(){
while(enableDrivePID){
if(resetDrive){
resetDrive = false;
LeftMotor.setPosition(0, degrees);
RightMotor.setPosition(0, degrees);
}
int leftMotorPosition = LeftMotor.position(degrees);
int rightMotorPosition = RightMotor.position(degrees);
/////////////////////////////////////////////////////////////
// Lateral movement
/////////////////////////////////////////////////////////////
//Mean of values
int averagePosition = (rightMotorPosition + leftMotorPosition);
error = desiredValue - averagePosition;
derivative = error - prevError;
totalError += error;
if(abs(error) < integralBound){
totalError+=error;
} else {
totalError = 0;
}
double lateralMotorPower = (error * kP + derivative * kD + totalError *kI);
//////////////////////////////////////////////////////////////////////////////
//turning
////////////////////////////////////////////////////////////////////////////
int turnDifference = leftMotorPosition - rightMotorPosition;
turnError = turnDesiredValue - turnDifference;
turnDerivative = turnError - turnPrevError;
turnTotalError += turnError;
if(abs(error) < integralBound){
turnTotalError+=turnError;
} else {
turnTotalError = 0;
}
double turnMotorPower = (error * turnkP + derivative * turnkD + totalError * turnkI);
LeftMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
RightMotor.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
prevError = error;
turnPrevError = turnError;
vex::task::sleep(50);
}
return 1;
}
int armPID() {
while(enableArmPID){
if(resetArm){
resetArm = false;
LeftMotor.setPosition(0, degrees);
RightMotor.setPosition(0, degrees);
}
int armMotorPosition = Arm.position(degrees);
armError = armDesiredValue - armMotorPosition;
armDerivative = armError - armPrevError;
armTotalError += armError;
double armMotorPower = (armError * armkP + armDerivative * armkD + armTotalError * armkI);
Arm.spin(forward, armMotorPower, voltageUnits::volt);
armPrevError = armError;
task::sleep(50);
}
return 1;
}
void autonomous(void) {
vex::task drivingPID(drivePID);
vex::task armTaskPID(armPID);
desiredValue = 360;
}
/*---------------------------------------------------------------------------*/
/* */
/* User Control Task */
/* */
/* This task is used to control your robot during the user control phase of */
/* a VEX Competition. */
/* */
/* You must modify the code to add your own robot specific commands here. */
/*---------------------------------------------------------------------------*/
void usercontrol(void) {
// User control code here, inside the loop
while (1) {
wait(20, msec);
}
}
//
// Main will set up the competition functions and callbacks.
//
int main() {
// Set up callbacks for autonomous and driver control periods.
Competition.autonomous(autonomous);
Competition.drivercontrol(usercontrol);
// Run the pre-autonomous function.
pre_auton();
// Prevent main from exiting with an infinite loop.
while (true) {
wait(100, msec);
}
}I really hope this can help you.
1 Like
Why is the turnDifference = leftMotorPosition - rightMotorPosition and what exactly does it mean? If I wanted the robot to go (ex. 90 deg turn), how would I input the target?
1 Like
If you were using a gyro that value would just be the degrees of the gyro heading or something. I hope this helps you.
just giving the code does not help at all.
Do you mean in terms of the drivetrain, or just an arm?
What you could do is have the setpoint be based off degrees (or radians I guess), and have your drivetrain or your arm detect how far it is in(your current), then get error based off of that (setpoint - current), then there.
For a drivetrain, you would probably want to find out how far you’ve gone with a gyro, with an arm, you can use an encoder.
1 Like
@PenguinHasAGun and @CR7,
I will use gyroDeg to signify the gyro degrees
you have too add all of the different variables for turn like error and kP.
turnError = turnDesiredValue - gyroDeg;
This is the error of how much left you have to turn.
turnDerivative = turnError - turnPrevError;
This is the derivative to make sure it overshoots
turnTotalError += turnError;
This is the integral value to make a turn it can be tuned
double turnMotorPower = (turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI);
This is how fast you would like to turn. with about the same calculations as moving forward and backward
LeftMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
RightMotor.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
If Lateral Motor Power is the speed forward power you can change the turning direction by changing the signs before turnMotorPower.
then the last thing you have to add it at the end right before your waityou have to add
turnPrevError = turnError;
1 Like
This PID is extremely redundant and inefficient, and there is no need for all those variable declarations (and the data types should be of type float or double for most of these calculations and declarations). What you should do is make a PID class and create instances of that class for the calculation.
The instance data would specify the tuning constants and integral bounds for each instance of the class. An instance in this case would be a linear PID for straight-line movements of the chassis and turn PID for turns (or straight-line correction) and arm PID. This would eliminate so much clutter currently in the code you provided.
Alternatively, you could make a PID function and input the tuning parameters that way. This way you could have a general function for all PID cases (this is inherently part of the PID class declaration, but it isn’t exclusive to it).
The reason behind this is that PID calculation doesn’t change for any case. It is always a general calculation based on a particular sensor feedback value. Long story short, I think the PID that @Connor provided in his tutorial is nice for beginners, but it is overly complex and inefficient. There are much faster solutions for the same problem, and this approach would only really work well when you’re using one instance of a PID calculation.
3 Likes
Building on @mvas’s comment, one rule of thumb good programmers adhere to is “DRY” - Don’t Repeat Yourself. In this case, @Skynet’s code:
int turnDifference = leftMotorPosition - rightMotorPosition;
turnError = turnDesiredValue - turnDifference;
turnDerivative = turnError - turnPrevError;
turnTotalError += turnError;
if(abs(error) < integralBound){
turnTotalError+=turnError;
} else {
turnTotalError = 0;
}
double turnMotorPower = (error * turnkP + derivative * turnkD + totalError * turnkI);
For drivePID is virtually the same as for armPID
In this case, the DRY principal would consolidate this into:
double evaluatePID(double targetReading, double currentReading, double previousError, double totalError, double kP, double kI, double kP, double integralBound) {
double curError = targetReading - currentReading;
double curDerivative = curError - previousError;
totalError+= curError;
if(abs(totalError) > integralBound){
totalError = integralBound;
}
return (curError* kP + curDerivative * kD + totalError * kI);
However, this implementation is incorrect because the calculation is stateful, in the sense that totalError and previousError depend on prior evaluations.
To address this, we can leverage C++'s notion of being an object-oriented programming language and “Encapsulate” this functionality.
Here’s an example of a PID class the Intertubes offered up, which may be a bit overly complicated for these purposes. We can see how the author creates a class to hold the state variables as well as the values for kP, kI, and kD:
class PIDImpl
{
public:
PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki );
~PIDImpl();
double calculate( double setpoint, double pv );
private:
double _dt;
double _max;
double _min;
double _Kp;
double _Kd;
double _Ki;
double _pre_error;
double _integral;
};
PID::PID( double dt, double max, double min, double Kp, double Kd, double Ki )
{
pimpl = new PIDImpl(dt,max,min,Kp,Kd,Ki);
}
double PID::calculate( double setpoint, double pv )
{
return pimpl->calculate(setpoint,pv);
}
PID::~PID()
{
delete pimpl;
}
/**
* Implementation
*/
PIDImpl::PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki ) :
_dt(dt),
_max(max),
_min(min),
_Kp(Kp),
_Kd(Kd),
_Ki(Ki),
_pre_error(0),
_integral(0)
{
}
double PIDImpl::calculate( double setpoint, double pv )
{
// Calculate error
double error = setpoint - pv;
// Proportional term
double Pout = _Kp * error;
// Integral term
_integral += error * _dt;
double Iout = _Ki * _integral;
// Derivative term
double derivative = (error - _pre_error) / _dt;
double Dout = _Kd * derivative;
// Calculate total output
double output = Pout + Iout + Dout;
// Restrict to max/min
if( output > _max )
output = _max;
else if( output < _min )
output = _min;
// Save error to previous error
_pre_error = error;
return output;
}
Now, one can declare many different instances of a PID for each different need. For example in psuedocode:
PID drivePID(1.0, 2.0, 3.0, ...);
PID armPID(1.1, 2.2, 3.3, ...);
PID liftPID(4.3,3.2,2.1,....);
double driveSpeed = drivePID.calculate(desiredWheelState,....);
double armSpeed = armPID.calculate(desiredArmState,...);
7 Likes
This was really nicely put @Mentor_355v , and I have a very similar approach to this with the use of a data structure that doesn’t require all of the declarations of a class but acts very similar to it. One thing I’d like to point out is this approach to PID opens up the program to be much more versatile and dare I say modular. Modularity in code is something all teams should work towards.
1 Like
I have also heard of people using multitasking to run a turn pid and a drive pid at the same time. How would this be beneficial and how would I go about doing so?
Here’s how I did it:
Calculate independent drive and turning motor values. The drive values will be based on the error (distance to desired point), kP, kI, and kD, and the orientation of the robot (just using trigonometry to calculate independent wheel speeds based on the robot’s orientation). The turning values are based only on the turn error (smallest error between robot’s orientation and desired orientation), kP, kI, and kD.
Then, simply add the turning values and the drive values for all wheels repeatedly, and, with proper tuning, the robot can turn and slide concurrently to a desired position and orientation. Let me know if I need to explain it better 
It is certainly possible use multitasking to run multiple PID tasks at the same time. One thing to be careful of with multitasking is making sure that the two “concurrent” tasks do not interfere with each other.
To use an analogy, suppose that you have 2 joysticks and you and your friend each have one and you want to drive the robot. If you want the robot to turn right and your friend wants the robot to turn left, what is the robot actually going to do?
So the first step to running a turn PID and a drive PID is to learn how multitasking works.
2 Likes
I did this without multitasking; instead I essentially added the two vectors for driving and turning. I don’t think it’s all that difficult, and the only task I used was to do position tracking in the background.
2 Likes
Sorry - i was replying to CR7 not you. There are lots of ways to accomplish this. He asked specifically about multitasking.
I’m a fan of keeping things simple. Unless you already understand multitasking, having your first use-case be drive-train PID controls is probably overly ambitious. I definitely recommend learning PIDs and multitasking separately, so that when you bring the concepts together you already understand how each works on its own.
5 Likes
Sorry, but I still don’t think I understand how I were to implement a turning PID. How would I do so only with encoders? Also, would the PID constants of the turn and drive PID be the same or would I have to tune each of them accordingly?
1 Like
A general formula to find the relative angle of the robot would be:
(lEncoder - rEncoder)/(lDistance+rDistance)
Where the first term is the encoder values from the wheels you are using to track the robot’s position and the second term is the sum of the distances of those wheels to the tracking center of the robot of the two encoders. This can all be found in Pilons document which can be found here. Alternatively, you can just use a gyro as the sensor input to make this process much simpler. The PID process does not change, only the sensor being used.
This might sound like a foreign language to you but it is very simple. It seems that you’re jumping several steps in the coding process, First learn what PID is and how it works before you even begin to try to implement any code for it. This is the best resource I’ve found back when I was learning PID. There is also a bunch of sample code and simulations online and youtube.
Also, I know it is very hard to learn how to code without access to a physical robot so be very patient. That is the best advice I could give you coming from a veteran vex programmer.
Edit: As for the formula, (from a logical standpoint) think of what happens to the robot when it turns, the difference between each side of the chassis increases (one side becomes more positive and the other side becomes more negative). The larger the angle, the more each side of the chassis will turn in opposite directions. The difference between these two values can provide an angle of the robot relatively speaking. The actual mathematical proof is derived from the arc length formula which I personally learned when I took pre-calc but it is basic circle geometry.
2 Likes
I looked over the position tracking document, but I think what I am trying to ask is how to calculate how much the robot rotated in place. In the doc, the angle was obtained by using the values of the arcs created by the 2 tracking wheels, and the angle represents the angle relative to the center of rotation, not the center of the robot itself. Is there something missing or an alternative method that I don’t know of? Or does the math still apply to turning in place?
1 Like
i believe it still applies for turning in place because almost all robots don’t turn exactly about the center of the robot. You’d need basically a perfectly built chassis for that (which is virtually impossible given the discrepancies from part to part). The radius of a point turn would be 0, but technically there is no radius in the angle calculation. So yes, the odometry math works for point turns.
Though, I’d imagine the approximation would get less and less accurate, so for this case I’d say just the IMU. It’s a worthy investment. I personally haven’t used odometry for point turns so I’m not exactly sure of the answer to this question.
2 Likes
If I wanted to create classes for my variables and PID loops, would I need to create a separate class for turning and another one for driving straight?
1 Like
No, you create an instance of the class, aka an “object”
2 Likes