This is a general PID controller, any tips for improvement?
This may be a long read… (I can’t figure out how to import a v5cpp file)
#pragma region VEXcode Generated Robot Configuration
// Make sure all required headers are included.
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>
#include "vex.h"
using namespace vex;
// Brain should be defined by default
brain Brain;
// START V5 MACROS
#define waitUntil(condition) \
do { \
wait(5, msec); \
} while (!(condition))
#define repeat(iterations) \
for (int iterator = 0; iterator < iterations; iterator++)
// END V5 MACROS
// Robot configuration code.
controller Controller1 = controller(primary);
motor L1 = motor(PORT1, ratio6_1, true);
motor L2 = motor(PORT2, ratio6_1, true);
motor L3 = motor(PORT6, ratio6_1, true);
motor R1 = motor(PORT19, ratio6_1, false);
motor R2 = motor(PORT3, ratio6_1, false);
motor R3 = motor(PORT10, ratio6_1, false);
gps GPS15 = gps(PORT15, 114.30, -114.30, mm, 180);
inertial Inertial21 = inertial(PORT21);
// generating and setting random seed
void initializeRandomSeed(){
int systemTime = Brain.Timer.systemHighResolution();
double batteryCurrent = Brain.Battery.current();
double batteryVoltage = Brain.Battery.voltage(voltageUnits::mV);
// Combine these values into a single integer
int seed = int(batteryVoltage + batteryCurrent * 100) + systemTime;
// Set the seed
srand(seed);
}
void vexcodeInit() {
//Initializing random seed.
initializeRandomSeed();
}
// Helper to make playing sounds from the V5 in VEXcode easier and
// keeps the code cleaner by making it clear what is happening.
void playVexcodeSound(const char *soundName) {
printf("VEXPlaySound:%s\n", soundName);
wait(5, msec);
}
// define variable for remote controller enable/disable
bool RemoteControlCodeEnabled = true;
#pragma endregion VEXcode Generated Robot Configuration
// -------------------- Device Setup --------------------
// A global instance of competition
competition Competition;
// Custom motor groups
motor_group leftDrive(L1, L2, L3);
motor_group rightDrive(R1, R2, R3);
// Shortern sensors (for quicker typing)
inertial imu(PORT21);
gps gps(PORT15);
// -------------------- Constants -----------------------
// Unchanging variables used for calculations (constexpr as these values will not change throughout the program)
constexpr double WHEEL_DIAMETER_MM = 101.6; // 4.0 inches = 101.6 mm
constexpr double PI = 3.141592653589;
constexpr double TICKS_PER_REV = 360.0; // In our context, one degree is one tick
// Distance PID
constexpr double dist_kP = 0.5;
constexpr double dist_kI = 0.002;
constexpr double dist_kD = 0.1;
constexpr double dist_iMin = -50;
constexpr double dist_iMax = 50;
constexpr double dist_dA = 0.2;
// Heading PID
constexpr double head_kP = 1.0;
constexpr double head_kI = 0.0;
constexpr double head_kD = 0.2;
constexpr double head_iMin = -10;
constexpr double head_iMax = 10;
constexpr double head_dA = 0.1;
// Turning PID
constexpr double turn_kP = 0.9;
constexpr double turn_kI = 0.001;
constexpr double turn_kD = 0.3;
constexpr double turn_iMin = -30;
constexpr double turn_iMax = 30;
constexpr double turn_dA = 0.2;
// -------------------- PID Controller ------------------
class PIDController {
public:
// Variables
double kP, kI, kD; // Gains
double integralMin, integralMax; // Integral clamping
double derivativeConstant; // The lower, the smoother the derivative; the higher, the faster response time of the derivative
// Constructor, uses reset(); on declaration for extra saftey
PIDController(double p, double i, double d, double iMin, double iMax, double dAlpha)
: kP(p), kI(i), kD(d), integralMin(iMin), integralMax(iMax), derivativeConstant(dAlpha) {
reset();
}
// Resets crucial values
void reset() {
lastError = 0;
integral = 0;
lastDerivative = 0;
}
// Find the correction needed, given the target and actual positions
double compute(double target, double current) {
double error = target - current; // Proportional value
integral += error; // Integral value
// Integral clamp for anti-windup
if (integral > integralMax) integral = integralMax;
if (integral < integralMin) integral = integralMin;
// Derivative value
double rawDerivative = error - lastError;
double derivative = (derivativeConstant * rawDerivative) + ((1 - derivativeConstant) * lastDerivative); // Filitered derivative
// Update variables
lastError = error;
lastDerivative = derivative;
// Return values * their respective gains
return (kP * error) + (kI * integral) + (kD * derivative);
}
private:
// Non-global variables
double lastError;
double integral;
double lastDerivative;
};
// ---------------- PID Instances ---------------------
// FYI: PIDController type(kP gain, kI gain, kD gain, integral minimum, integral maximum, derivative filter constant);
PIDController distancePID(dist_kP, dist_kI, dist_kD, dist_iMin, dist_iMax, dist_dA);
PIDController headingPID(head_kP, head_kI, head_kD, head_iMin, head_iMax, head_dA);
PIDController turnPID(turn_kP, turn_kI, turn_kD, turn_iMin, turn_iMax, turn_dA);
// ---------------- Utility Functions ------------------
// Convert millimeters to ticks (degrees)
double mmToTicks(double mm) {
return (mm / (PI * WHEEL_DIAMETER_MM)) * TICKS_PER_REV;
}
// ---------------- Drive Forward ----------------------
// When calling this function, the drive amount is required but the maximum time and voltage already have defaults, thus they are optional
void driveDistance(double targetMM, int timeMax = 3000, double maxVoltage = 8.0) {
// Reset crucial values
distancePID.reset();
headingPID.reset();
leftDrive.setPosition(0, degrees);
rightDrive.setPosition(0, degrees);
double originalHeading = imu.heading(degrees);
int timeSpent = 0;
// Store target
double targetTicks = mmToTicks(targetMM);
// Feedback loop
while (true) {
// Calculate corrections/errors
double leftPos = leftDrive.position(degrees);
double rightPos = rightDrive.position(degrees);
double avgPos = (leftPos + rightPos) / 2;
double distanceOutput = distancePID.compute(targetTicks, avgPos);
double headingError = originalHeading - imu.heading(degrees);
double headingOutput = headingPID.compute(0, headingError);
double leftPower = distanceOutput - headingOutput;
double rightPower = distanceOutput + headingOutput;
// Limit max speed (voltage)
if (leftPower > maxVoltage) leftPower = maxVoltage;
if (leftPower < -maxVoltage) leftPower = -maxVoltage;
if (rightPower > maxVoltage) rightPower = maxVoltage;
if (rightPower < -maxVoltage) rightPower = -maxVoltage;
// Use correction
leftDrive.spin(fwd, leftPower, volt);
rightDrive.spin(fwd, rightPower, volt);
// Exit when close to target
if ((fabs(targetTicks - avgPos) < 10 && fabs(distanceOutput) < 1.5) || timeSpent >= timeMax) break;
// Rest
wait(20, msec);
// Add time spent
timeSpent += 20;
}
// Stop when loop exited
leftDrive.stop();
rightDrive.stop();
}
// ---------------- Turn For ---------------------
// When calling this function, the turn amount is required while the maximum time, voltage, and turnTo boolean is optional
void turnFor(double turnAmount, int timeMax = 3000, double maxVoltage = 8.0, bool turnTo = false) {
// Reset crucial values
turnPID.reset();
double targetHeading;
// If we want to turn to the specified heading, then the target heading is equal to the turn amount
// If we want to turn for the turn amount, then the target heading is equal to the current heading + the turn amount
turnTo ? targetHeading = turnAmount : targetHeading = imu.heading(degrees) + turnAmount;
int timeSpent = 0;
// Feedback loop
while (true) {
// Calculate errors and corrections
double currentHeading = imu.heading(degrees);
double error = targetHeading - currentHeading;
// Prevent unnecessarily large turns by turning the other way
if (error > 180) error -= 360;
if (error < -180) error += 360;
// Correction
double output = turnPID.compute(0, -error);
// Limit max speed (voltage)
if (output > maxVoltage) output = maxVoltage;
if (output < -maxVoltage) output = -maxVoltage;
// Use correctoin
leftDrive.spin(fwd, -output, volt);
rightDrive.spin(fwd, output, volt);
// Exit when close to target
if ((fabs(error) < 1.0 && fabs(output) < 1.0) || timeSpent >= timeMax) break;
// Rest
wait(20, msec);
// Add time spent
timeSpent += 20;
}
// Stop when loop exited
leftDrive.stop();
rightDrive.stop();
}
// -------------------- Competition Control ----------------------------
// "when driver control" function
int whenDriver() {
// Arcade drive setup: left joystick for forward/backward, right joystick for turning
while (true) {
leftDrive.setVelocity(Controller1.Axis3.position() + Controller1.Axis4.position(), percent);
rightDrive.setVelocity(Controller1.Axis3.position() - Controller1.Axis4.position(), percent);
wait(20, msec); // A short delay to prevent excessive CPU usage
}
return 0;
}
// "when autonomous control" function
int whenAuton() {
// Test
driveDistance(600.0); // Drive 600mm
turnFor(90); // Turn 90 degrees
driveDistance(-300.0); // Drive -300mm (reverse)
turnFor(-90); // Turn 90 degrees counterclockwise
return 0;
}
// "when started" function (for additional setup if needed)
int whenStarted1() {
return 0;
}
// -------------------- Unchanging ----------------------------
// The driver control task
void Driver() {
// Start the driver control task
task driver(whenDriver);
// Wait until not driver controler
while (Competition.isDriverControl() && Competition.isEnabled()) this_thread::sleep_for(10);
// Stop task
driver.stop();
}
// The autonomous control task
void Auton() {
// Start the autonomous control task
task auton(whenAuton);
// Wait until not autonomous control
while (Competition.isAutonomous() && Competition.isEnabled()) this_thread::sleep_for(10);
// Stop task
auton.stop();
}
// -------------------- Controller Button Event Handlers --------------------
/*
// L1 Button Action
void ButtonL1() {
//placeholder
}
// L2 Button Action
void ButtonL2() {
//placeholder
}
// R1 Button Action
void ButtonR1() {
//placeholder
}
// R2 Button Action
void ButtonR2() {
//placeholder
}
// Up Button Action
void ButtonUp() {
//placeholder
}
// Down Button Action
void ButtonDown() {
//placeholder
}
// Left Button Action
void ButtonLeft() {
//placeholder
}
// Right Button Action
void ButtonRight() {
//placeholder
}
// X Button Action
void ButtonX() {
//placeholder
}
// B Button Action
void ButtonB() {
//placeholder
}
// A Button Action
void ButtonA() {
//placeholder
}
// Y Button Action
void ButtonY() {
//placeholder
}
*/
// -------------------- Main Function --------------------
int main() {
// Initialize the competition
competition::bStopTasksBetweenModes = false;
Competition.drivercontrol(Driver);
Competition.autonomous(Auton);
// Initialize Robot Configuration
vexcodeInit();
// -------------------- Controller Button Event Handlers --------------------
// Checking if any controller button is pressed and calling the respective function
/*
Controller1.ButtonL1.pressed(ButtonL1);
Controller1.ButtonL2.pressed(ButtonL2);
Controller1.ButtonR1.pressed(ButtonR1);
Controller1.ButtonR2.pressed(ButtonR2);
Controller1.ButtonUp.pressed(ButtonUp);
Controller1.ButtonDown.pressed(ButtonDown);
Controller1.ButtonLeft.pressed(ButtonLeft);
Controller1.ButtonRight.pressed(ButtonRight);
Controller1.ButtonX.pressed(ButtonX);
Controller1.ButtonB.pressed(ButtonB);
Controller1.ButtonA.pressed(ButtonA);
Controller1.ButtonY.pressed(ButtonY);
*/
// Call the "when started" function for additional setup if necessary
whenStarted1();
}
Thanks!
edit: code tags added by mods, please remember to use them.