I have recently created this PID code and the robot does accelerate and decelerate to achieve the desired position, however, I am not sure what units it is driving in nor why it continues driving until it runs into the wall. I could not find a place to specify the units I wanted the PID to be in. I am not sure if maybe I need to use the internal motor encoders instead of the inertial sensor heading. Any help would be greatly appreciated.
//settings
double kP = 0.2;
double kI = 0.01;
double kD = 0.0;
double turnkP = 0.0;
double turnkI = 0.0;
double turnkD = 0.0;
//Autonomous Settings
int error; //Sensor value-Desired value=Position
int prevError; //Position 20 milliseconds ago
int derivative; //error-prevError : Speed
int totalError; //totalError = totalError + error
int turnError; //Sensorvalue - DesiredValue : Position
int turnPrevError; //Position 20 milliseconds ago
int turnDerivative; // error - prevError : Speed
int turntotalError; //totalError = totalError + error
bool resetinertial = false;
//variables modified for use
bool enableDrivePID = true;
void drivePID(int driveDistance, bool frontback) {
//Brain.Screen.print("drivePID Activated");
error = driveDistance;
while(abs(error) > .1){
Brain.Screen.clearScreen();
Brain.Screen.print(Inertial9.rotation());
Brain.Screen.newLine();
if (resetinertial) {
resetinertial = true;
Inertial9.resetHeading();
}
//////////////////////////////////
//Lateral movement PID
///////////////////////////
//Get the current position
int currentPosition = Inertial9.rotation(degrees);
//Potential
error = driveDistance - currentPosition;
//Derivative
derivative = error - prevError;
//Integral
totalError += error;
double lateralMotorPower = error * kP + derivative * kD + totalError * kI;
//Brain.Screen.print("Motor Power Calculate");
//////////////////////////////////////////////
if (bool frontback = true){//drive forward
RightDriveSmart.spin(forward, lateralMotorPower, volt);
LeftDriveSmart.spin(forward, lateralMotorPower, volt);
}
else{//drive backwards
RightDriveSmart.spin(reverse, lateralMotorPower, volt);
LeftDriveSmart.spin(reverse, lateralMotorPower, volt);
}
}
}
bool enableturnPID = true;
void turnPID(int turnAngle, bool turnLR) {
while(abs(turnError) > 5){
if (resetinertial) {
resetinertial = true;
Inertial9.resetHeading();
}
///////////////////////////
//Turning Movement PID
////////////////////////////////////////////
//Get the current position
int turncurrentposition = Inertial9.rotation(rotationUnits::deg);
//Potential
turnError = turnAngle - turncurrentposition;
//Derivative
turnDerivative = turnError - turnPrevError;
//Integral
turntotalError += turnError;
double turnMotorPower = turnError * turnkP + turnDerivative * turnkD + turntotalError * turnkI;
///////////////////////////////////////////////////////////
if (bool turnLR = true){//turn right
RightDriveSmart.spin(forward, turnMotorPower, volt);
LeftDriveSmart.spin(reverse, turnMotorPower, volt);
}
else{//turn left
RightDriveSmart.spin(reverse, turnMotorPower, volt);
LeftDriveSmart.spin(forward, turnMotorPower, volt);
}
//prevError-error;
//turnPrevError - error;
vex::task::sleep(20);
}
}
void calibration(){
Inertial9.calibrate();
while(Inertial9.isCalibrating()){
Controller1.Screen.clearLine(3);
Controller1.Screen.print("calibrating");
}
Controller1.Screen.clearLine(3);
Controller1.Screen.print("done");
wait(10, msec);
}
void pre_auton(void){
// Initializing Robot Configuration. DO NOT REMOVE!
void vexcodeInit();
calibration();
wait(100, msec);
}
For turning, you specified the units as degrees here. This gets your heading error though, so the inertial should be used in turning PID. For lateral PID, if you do not have odometry then you should use the motor encoders, and the units will be whatever units you convert the rotations of the wheels into. The reason that your robot never stopped is because your lateral PID was using turn error, and since it didn’t turn, the error never reached 0.
There isn’t any stops (so it will just drive straight forever)
Integral, using integral isn’t bad, its just its really hard to tune correctly and can lead to inconsistancy. I recommend not using it for a drive pid.
The current position var is a int and not a double or float. This can cause accuracy issues with your pid
Your error calculation. Right now you calculate error by subtracting the current position from the prevError. Which I belive will cause your pid not to move after a certain number of itterations. Instead you should calculate it like this
error = driveDistance-currentPosition;
Your prevError calculation is right before your derivative, this will cause calculation issues as the prevError will always be equal to the current error, causing the derivative to always be 0 . So instead you should calculate the prevError after you calculate derivative like this :
derivative = error- prevError;
prevError = error;
You dont have a wait, all while loops should have a wait of some kind (to prevent resources from being wasted). So I recommend right under the drive backwards you put a wait of 10-20 msecs.
In addition to what @4233XBen mentioned, you are using degrees as your unit (
), which means you will have to input all of your distances in degrees of your drivebase wheels, which generally is not a good idea. If you want to be able to input your distances in a distance unit like inches to get a physical sense for the distance that your robot will go first, you can calculate that with [motor position in degrees] / 360 * [gear ratio to convert to wheel speed] * pi * [wheel diameter].
You don’t necessarily need integral, and it can be hard to tune, but, if tuned well (read this), it can be nice to have. I have never really experienced any inconsistencies with integral, but it does take a while to tune. If this is your first time doing PID and you are inexperienced with it, you don’t need to use integral, but if you have a while to spend tuning your PID you could probably try using it.
Thank you, for all the help you have given me! One last question, can I put the calculation for the distance units in for degrees, or do I have to make it a separate thing.
I am not sure what you mean by “put the calculation for the distance units in for degrees,” but if you mean use it when assigning your position variable instead of as a separate variable, then yes you can do that; generally the only reason for having additional variables that manipulate existing variables that are never used is for readability, but in that case it wouldn’t be that much harder to read.
If it’s what you’ve determined through tuning, than that’s what it is for your robot. The kP value is very affected by the way you measure error and motor power. If error is motor degrees vs motor rotations, you will get kP ~360x lower using degrees vs rotations. How short are you talking? 12 inches on 2.75s is about 250 degrees of wheel rotation, at 1.3 gear ratio is 327 degrees of motor error. seems you’re using motor percent power, not volts, so that’s a higher kP for that decision.
0.005 does seem low, that’s an output of 327*.005 = 1.6, which is a pretty low drive power percentage. Can you put some print funcitons in your code to see what your variable values are?