[Jay Lundy]

We use 2 PI loops to maintain target velocities on our 2 arm joints. The integral is essential for our loop because it is what keeps the arms from falling due to gravity. For the proportional to work, it needs to have a speed error. So in order for the proportional to prevent the arm from dropping, the arm needs to drop a little first. The integral is non-zero when the arm is stationary, which is what keeps the motor at stall torque.

Here's the code for the PI loop for our elbow.

 //Joystick dead zone
 ELBOW_CONTROL = (ELBOW_CONTROL >= 127 - ELBOW_CONTROL_DEAD_ZONE && ELBOW_CONTROL <= 127 + ELBOW_CONTROL_DEAD_ZONE) ? 127 : ELBOW_CONTROL;	
  
  //Calculate velocities
  elbowVelocity = (long)(elbowCurrPosition - elbowPrevPosition) * 1000 / (long)(elbowCurrTime - elbowPrevTime);
  targetVelocity = ((int)ELBOW_CONTROL - 127) * ELBOW_MAX_POT_SPEED / 127;
  
  //Check for target position mode
  if (elbowPositionMode) {
  	targetVelocity = RepositionElbow();
  }
  
  //Calculate speed error
  speedError = targetVelocity - elbowVelocity;
  
  //Calculate proportional and integral
  proportional = speedError * ELBOW_PROPORTIONAL; 
  elbowIntegral += speedError * ELBOW_INTEGRAL;
  
  //Limit integral
  elbowIntegral = MIN_MAX(elbowIntegral, -127, 127);
  
  //Output to motors
  ELBOW_MOTORS = 254 - MIN_MAX(127 + (proportional + elbowIntegral), 0, 254);	
  
  //Set prev values to current values
  elbowPrevPosition = elbowCurrPosition;
  elbowPrevTime = elbowCurrTime;

The thing is, none of our programmers had any prior experience with PID loops. We just had an engineer come in for 2 hours one day and explain the theory to us, but he didn't provide any code. I'm curious if our implementation is correct, or if there are any improvements we should make.

By the way, our P and I constants are both 1 / 8 on our competition robot, and our elbow loop runs at 10 Hz.

Oh we also have a target position mode which feeds a target velocity into our PI loop. That's the RepositionElbow() function that you see in there.