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
  // 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 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() {
		// PID variables (Kp, Ki, Kd)
		0, 0, 0,
		// IMEMotor
		// Motor Gearings

	while (true) {
		PIDUpdate(&flywheel); = motorPowerToRPM(80);
	  	writeDebugStreamLine(" %d",;

	  	// We use deltaTime so this doesn't matter, but it will ease calculations

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.


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.