[Jared Russell]

Your understanding of PIDController seems to be correct - it takes PIDSources, crunches the numbers in a separate thread, and asynchronously writes to PIDOutputs.

What makes this tricky is that the WPILib PIDController writes its output to a PIDOutput, but RobotDrive does not implement PIDOutput. Likewise, Encoder doesn't implement PIDSource (because it would be ambiguous to do so - do you want to measure speed or distance?).

There are several possible solutions to this problem.

First, we need to turn the encoder's distance measurement into a PIDSource. You could re-write the WPILib Encoder class, but in general I try not to mess with a library unless I absolutely need to. Instead, how about a wrapper class?

class DistanceEncoder extends PIDSource
{
public:
  DistanceEncoder(Encoder *baseEncoder)
  {
    m_baseEncoder = baseEncoder;
  {

  double pidGet()
  {
    return m_baseEncoder->GetDistance();
  }

private:
  Encoder* m_baseEncoder;
};

Similarly, you can wrap RobotDrive in another PIDOutput class:

class RobotDriveOutput extends PIDOutput
{
public:
  RobotDriveOutput(RobotDrive* baseDrive)
  {
    m_baseDrive = baseDrive;
  }

  void pidWrite(double output)
  {
    m_baseDrive->Drive(output, 0.0);
  }

private:
  RobotDrive* m_baseDrive;
};

Then you can put it all together like you said.

*Note: there are many ways to do this; this is only one. Also, I banged out the above code from memory, so there might be syntax and/or API errors, but you should get the point.