For some time I have been trying to develop a PID. I took a look at @Connor’s video on how to make one. I understand some parts of it, but cannot seem to get it working in autonomous. If someone could tell me what’s wrong with the code, and maybe help me a little with the changes, that would be amazing.
double kP = 1; // Error
double kI = 0.001 ; // Last, Increase Precision
double kD = 0.01; // To Fast or Too Slow
double turnkP = 1 ;
double turnkI = 0.001;
double turnkD = 0.1;
// Auton Settings
int targetValue = 200;
int targetTurnValue = 0;
double error; // SensorValue - DesireValue : Positional Value
double previouserror = 0; // Position 20 miliseconds ago
double derivative; // error - previouserror : Speed
double totalerror= 0 ; // totalerror = totalerror + error
double turnerror; // SensorValue - DesireValue : Positional Value
double turnpreviouserror = 0; // Position 20 miliseconds ago
double turnderivative; // error - previouserror : Speed
double turntotalerror= 0; // totalerror = totalerror + error
bool resetDriveSensors = true;
// Variation modified for use
bool enableDrivePID = true;
int drivePID(){
while(enableDrivePID){
if (resetDriveSensors) {
resetDriveSensors = false;
LeftMotor.setPosition(0,degrees);
RightMotor.setPosition(0,degrees);
}
// Get Position of Both Motors
int leftMotorPosition = LeftMotor.position(degrees);
int rightMotorPosition = RightMotor.position(degrees);
// Average Of the Motors
int averagePosition = (leftMotorPosition + rightMotorPosition)/2;
// Proportional
error = averagePosition - targetValue;
// Derivative
derivative = error - previouserror;
// Integral : Velocity -> Position -> Absenent ( Position x Time )
totalerror += error;
double lateralMotorPower = (error * kP + derivative * kD + totalerror * kI);
// Average Of the Motors
int turnDifference = leftMotorPosition - rightMotorPosition;
// Proportional
turnerror = turnDifference - targetTurnValue;
// Derivative
turnderivative = turnerror - turnpreviouserror;
// Integral : Velocity -> Position -> Absenent ( Position x Time )
turntotalerror += turnerror;
double turnMotorPower = (turnerror * turnkP + turnderivative * turnkD + turntotalerror * turnkI) ;
///////////////////////////////////////////////////////////////////////
LeftMotor.spin(reverse, lateralMotorPower + turnMotorPower, voltageUnits::volt);
RightMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
previouserror = error;
turnpreviouserror = turnerror;
task::sleep(20);
}
return 1;
}
/*---------------------------------------------------------------------------*/
void autonomous(void) {
vex::task start(drivePID);
targetValue = 300 ;
Hello, I have looked at that thread and I have a few questions.
What does Clamping do exactly?
I don’t really understand much about the derivative value in a PID
I tried copy/pasting some of the sample code from the other forum to see if it would work, but I keep getting errors in my code. The errors are at the int main() at the bottom of the code.
//
// P - Proportional ( Like P-Control )
//
// I - Integral ( )
//
// D - Derivative ( )
//
//
#include "vex.h"
using namespace vex;
// A global instance of competition
competition Competition;
// define your global instances of motors and other devices here
extern inertial Inertial20 ;
extern motor LeftMotor;
extern motor RightMotor;
extern motor BLM ;
extern motor BRM ;
extern drivetrain Drivetrain ;
extern controller Controller1 ;
extern inertial InertialS ;
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, ...
}
// Settings
double kP = 12; // Error
double kI = 12 ; // Last, Increase Precision
double kD = 12; // To Fast or Too Slow
double turnkP = 1 ;
double turnkI = 0.00;
double turnkD = 0.1;
// Auton Settings
int targetValue = 200;
int targetTurnValue = 0;
double error; // SensorValue - DesireValue : Positional Value
double previouserror = 0; // Position 20 miliseconds ago
double derivative; // error - previouserror : Speed
double totalerror= 0; // totalerror = totalerror + error
double turnerror; // SensorValue - DesireValue : Positional Value
double turnpreviouserror = 0; // Position 20 miliseconds ago
double turnderivative; // error - previouserror : Speed
double turntotalerror= 0; // totalerror = totalerror + error
bool resetDriveSensors = true;
// Variation modified for use
bool enableDrivePID = true;
bool pidAbort = false; // added for testing during usercontrol
double deadBand = 5.0; // this is the close enough to target value
int drivePID(){
while(1) {
if (resetDriveSensors) {
resetDriveSensors = false;
totalerror = 0.0;
previouserror = 0.0;
turntotalerror = 0.0;
turnpreviouserror = 0.0;
LeftMotor.setPosition(0,degrees);
RightMotor.setPosition(0,degrees);
}
if (enableDrivePID) {
// PID CODE GOES HERE
// stop PID from controlling motor when at taget
if((fabs(error) < deadBand) || pidAbort) {
enableDrivePID = false;
}
task::sleep(20);
}
////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////
// Get Position of Both Motors
int leftMotorPosition = LeftMotor.position(degrees);
int rightMotorPosition = RightMotor.position(degrees);
// Average Of the Motors
int averagePosition = (leftMotorPosition + rightMotorPosition)/2;
// Proportional
error = targetValue - averagePosition ;
// Derivative
derivative = error - previouserror;
// Integral : Velocity -> Position -> Absenent ( Position x Time )
totalerror += error;
double lateralMotorPower = (error * kP + derivative * kD + totalerror * kI);
// Average Of the Motors
int turnDifference = leftMotorPosition - rightMotorPosition;
// Proportional
turnerror = turnDifference - targetTurnValue;
// Derivative
turnderivative = turnerror - turnpreviouserror;
// Integral : Velocity -> Position -> Absenent ( Position x Time )
turntotalerror += turnerror;
double turnMotorPower = (turnerror * turnkP + turnderivative * turnkD + turntotalerror * turnkI) ;
///////////////////////////////////////////////////////////////////////
LeftMotor.spin(reverse, lateralMotorPower + turnMotorPower, voltageUnits::volt);
RightMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
previouserror = error;
turnpreviouserror = turnerror;
task::sleep(20);
}
return 1;
}
void pidMoveToPosition (double setPoint, double setTurn, bool wait4done=true) {
resetDriveSensors = true;
targetValue = setPoint;
targetTurnValue = setTurn;
enableDrivePID = true;
while (enableDrivePID & wait4done) {
pidAbort = Controller1.ButtonB.pressing(); // abort PID by press button B
vex::task::sleep(20);
}
pidAbort = false;
return;
}
/*---------------------------------------------------------------------------*/
void autonomous(void) {
vex::task start(drivePID);
targetValue = 300 ;
task::sleep(1000);
Inertial20.calibrate(false) ;
while(Inertial20.isCalibrating()){
wait(1,msec);
}
LeftMotor.spin(directionType::fwd);
RightMotor.spin(directionType::fwd);
BLM.spin(directionType::fwd);
BRM.spin(directionType::fwd);
waitUntil(Inertial20.rotation(degrees) >= 270 );
LeftMotor.stop() ;
RightMotor.stop() ;
BLM.stop() ;
BRM.stop() ;
}
void usercontrol(void) {
enableDrivePID = false;
while (1) {
vex::task t1(drivePID);
// ADD THIS CODE - it doesn't mater if it is before or after Driver Control code
// start PID by pressing button A
if (Controller1.ButtonA.pressing()) {
pidMoveToPosition(720,0,true);
pidMoveToPosition(-720,0,true);
wait(20, msec); // Sleep the task for a short amount of time to
// prevent wasted resources.
}
}
//
// 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);
}
}
Clamping prevents the integral term from getting much larger than the max value of the motor voltage. Also if you are going to use the turn part of the PID then you also need to clamp the output of the first drive PID so that it doesn’t get to say 40. And then you are getting a turn output of 3 so your two values are 40+3 and 40-3 both of which are greater than 12 so the motor clamps to 12 and then you don’t turn.
Derivative attempts to limit how fast the change in error affects the output. It tries to smooth large jumps between the last point measured and the current point measured. I kind of think of it as putting a damper on the enthusiasm of the P term early on and then later (if enough iterations are run) possibly the I term. It seems to be the trickiest to get tuned from what I’ve read.
The error is not in main but above it. You are missing a } somewhere. I suggest selecting Tools->Format Document from the menu to make brace alignment easy to see. Then cursor up to each brace } and see where its companion brace { is. It shouldn’t be too many above main.
double kP = 12; // Error
double kI = 12; // Last, Increase Precision
double kD = 12; // To Fast or Too Slow
these values are way to large. start small with kP first slowing increasing it until it gets to your target reasonable fast. for tuning look up Ziegler-Nichols method
double kP = 0.02; // Error
double kI = 0.0 // Last, Increase Precision
double kD = 0.0; // To Fast or Too Slow
I do see an issue with the drivePID code.
there are two task::sleep(20);
the PID code is not in the if (enableDrivePID) { code block meaning it will run all the time and not stop driving the motors. This is something you might do if you need hold. However there are better ways to implement that by adding a enableHold variable to that if-statement.
targetValue and targetTurnValue have to be the return types of what? LeftMotor.position() or RightMotor.position() and where would I put it?
Sorry Im kind of new to text code.
you have them declared as int, they need to be double.
in your posted code
// Auton Settings
int targetValue = 200;
int targetTurnValue = 0;
and
int leftMotorPosition = LeftMotor.position(degrees);
int rightMotorPosition = RightMotor.position(degrees);
from the API:
◆ position() double vex::motor::position ( rotationUnits units )
Gets the current position of the motor’s encoder.
Returns Returns a double that represents the current position of the motor in the units defined in the parameter.
Parameters
units The measurement unit for the position.
You still need to fix #1 (task sleep) and #2 put PID code inside if (enableDrivePID) { from my prior post.
I’m not sure what starting a task in a while loop does but I’m pretty sure you should not be doing that. Start the PID task in main(). This way it is available before Autonomous (for when you get it working).
Strange. I’d offer to look through it but I’m really tired and not really a PID expert. I’ll leave this to @rpm. If for some reason rpm doesn’t see it, maybe I can look tomorrow.
The PID code needs a few updates but looks like it will mostly work so you just need to get the order of operation correct. One thing is set turnKp to 0.0 so you are only debugging drviePID.
The order for the PID task is
int drivePID() { // this is the task
// put local variable definitions here. its ok for your PID vars to be global for now
while (1) { // continuously monitor resetDriveSensors and enableDrivePID
if (resetDriveSensors) { // really this is resetDrivePID not just the sensors
// reset code goes here
}
if (enableDrivePID) {
// Put PID code goes here
// stop the PID from driving motors when target is reached
// currently does not support hold
if ((fabs(error) < deadBand) || pidAbort) {
enableDrivePID = false;
}
} // if (enableDrivePID) -- i like to comment the end of long code blocks
vex::task::sleep(20);
} // while (1)
return 1;
} // drivePID