Yet more PID support

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.

// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//
// 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.
//
/////////////////////////////////////////////////////////////////////////////////////////

// .....................................................................................
// Insert user code here.
// .....................................................................................

AutonomousCodePlaceholderForTesting();  // Remove this function call once you have "real" code.
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//
// 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.
//
/////////////////////////////////////////////////////////////////////////////////////////

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

A few tips.

Watch for divide by zero, deltaTime can/will be zero when the code first operates, protect potential places like this.

``````  // Update actual velocity
if(system->deltaTime != 0 &&  system->ticksPerRev != 0)
system->currentVelocity = (1000.0 / system->deltaTime) * system->deltaCount * (60 / system->ticksPerRev);

``````

and this

``````  if(system->deltaTime != 0)
system->derivative = (system->error - system->lastError) / system->deltaTime;

``````

There is some integer rounding. This has issues.

``````float motorPowerToRPM(int motorPower) {
return (motorPower / 127) * 240;
}
``````

you send a motor power of 80 in the example code, 80 / 127 will always be 0 using integer math. change that code to this.

``````float motorPowerToRPM(int motorPower) {
return (motorPower / 127.0) * 240.0;
}

``````

In the demo the encoder is on the wrong motor.

Otherwise looks good, it obviously needs tuning but I will leave that to you.