I’m the programmer on a first year team who just got our invitation to states. Until now, we’ve been using what we call a “manual PID” where I (the driver) would turn up or down the power based on our shots. This was, needless to say, very inefficient. After we got the invite, I searched the VEX forums, looked at the algorithm and wrote up this:
pid.c - A include file that houses all PID code
/** Define Motor Gearings **/
#define VEX_393_Standard 627.2
#define VEX_393_HighSpeed 392.0
#define VEX393Turbo 261.333
typedef struct {
/** Constants **/
float Kp;
float Ki;
float Kd;
// number of ticks in a revolution (for Vex 393 Turbo Motors)
float ticksPerRev;
// current values (raw: ticks, current: RPM)
int currentCountRaw;
float currentVelocity;
// Target, the target RPM
int target;
// Error, the difference between our current value and out target value
int error;
// Integral, the accumulate of all error
int integral;
// Derivative, the current error minus the last error
int derivative;
// Last error, used for calculating derivative
int lastError;
// The last time we did a motor check (should always be 20, since we don't have super heavy calculations)
long lastTime;
// The last IME count
int lastCount;
// Total time (in ms) since last recalculating the velocity
int deltaTime;
// Total change in ticks since last reclaculation
int deltaCount;
// The motor that the IME is installed on
tMotor IMEMotor;
// Actual output to set your motors to
int drive;
} PIDControlled;
void PIDUpdateVelocity(PIDControlled *system) {
// Get the current tick count from the motor encoder
system->currentCountRaw = nMotorEncoder[system->IMEMotor];
// Recalculate the deltaTime
system->deltaTime = nSysTime - system->lastTime;
system->lastTime = nSysTime;
// Calculate change in ticks
system->deltaCount = system->currentCountRaw - system->lastCount;
system->lastCount = system->currentCountRaw;
// Update actual velocity
system->currentVelocity = (1000.0 / system->deltaTime) * system->deltaCount * (60 / system->ticksPerRev);
}
float motorPowerToRPM(int motorPower) {
return (motorPower / 127) * 240;
}
void PIDInit(PIDControlled *system, float kp, float ki, float kd, tMotor IMEMotor, float ticksPerRev) {
system->Kp = kp;
system->Ki = ki;
system->Kd = kd;
system->IMEMotor = IMEMotor;
system->lastError = 0;
system->lastTime = nSysTime;
system->lastCount = 0;
system->ticksPerRev = ticksPerRev;
}
void PIDUpdate(PIDControlled *system) {
// Update velocity
//PIDUpdateVelocity(system);
// Calculate error, for Proportional
system->error = system->target - system->currentVelocity;
// Calculate error accumulate, for Integral (and account for deltaTime)
system->integral += system->error * system->deltaTime;
// Calculate derivative, the previous error minus the current error (divided by deltaTime)
system->derivative = (system->error - system->lastError) / system->deltaTime;
// Calculate final drive, the value to be sent to the motors
system->drive =
/** Proportional **/
(system->Kp * system->error) +
(system->Ki * system->integral) +
(system->Kd * system->derivative);
// Clamp drive to motor values
if (system->drive > 127) {
system->drive = 127;
} else if (system->drive < -127) {
system->drive = -127;
}
// Update last error
system->lastError = system->error;
}
main.c - An example file showing usage of pid.c
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1, FlywheelSensor, sensorQuadEncoderOnI2CPort, , AutoAssign)
#pragma config(Motor, port2, FlywheelLeft, tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor, port3, FlywheelRight, tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor, port4, DriveFrontRight, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port5, DriveFrontLeft, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port6, DriveBackLeft, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port7, DriveBackRight, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port8, ElevatorRight, tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor, port9, ElevatorLeft, tmotorVex393TurboSpeed_MC29, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#pragma platform(VEX)
//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)
#include "Vex_Competition_Includes.c" //Main competition background code...do not modify!
#include "pid.c"
/////////////////////////////////////////////////////////////////////////////////////////
//
// Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////
static PIDControlled flywheel;
void pre_auton() {
// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
bStopTasksBetweenModes = true;
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
}
/////////////////////////////////////////////////////////////////////////////////////////
//
// Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////
task autonomous() {
// .....................................................................................
// Insert user code here.
// .....................................................................................
AutonomousCodePlaceholderForTesting(); // Remove this function call once you have "real" code.
}
/////////////////////////////////////////////////////////////////////////////////////////
//
// 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.
//
/////////////////////////////////////////////////////////////////////////////////////////
task usercontrol() {
PIDInit(flywheel,
// PID variables (Kp, Ki, Kd)
0, 0, 0,
// IMEMotor
FlywheelLeft,
// Motor Gearings
VEX393Turbo
);
while (true) {
PIDUpdate(&flywheel);
flywheel.target = motorPowerToRPM(80);
writeDebugStreamLine("flywheel.drive: %d", flywheel.drive);
// We use deltaTime so this doesn't matter, but it will ease calculations
wait1Msec(20);
}
}
All of this code compiles cleanly. I just wanted to get an outside opinion to see if they could spot any errors before we go into the nitty-gritty of tuning it. (@jpearman maybe?)
Any help would be excellent.
Thanks,
Brendan