[cstelter]

Originally Posted by Ozuru

Would it make sense to setup your Drive Train subsystem itself as a PIDSubsystem using encoders?

We set up a PIDSubsystem that had a constructor to accept a SpeedController and an encoder. We also then gave this PIDSubsystem the SpeedController interface via the implements keyword.

Something like

public class NordicSpeedController extends PIDSubsystem implements SpeedController {
   SpeedController m_motor;
   Encoder m_encoder;

   public NordicSpeedController(SpeedController motor, Encoder encoder) {
       super(1.0,0,0);  // pass PID to PIDSubsystem construtor
   }

   public void usePIDOutput(double output)  {
         motor.set(output)
   }
   
   public double returnPIDInput() {
       //Be sure encoder setDistancePerPulse() is set such that this returns
       //values between -1.0 and +1.0 over the full range of speed you can go
       return encoder.getRate();
   }

   //Next functions implment SpeedController Interface
   public void set(double speed) {
       enable();
       setSetpoint(speed)
   }

   public void set(double speed, int syncGroup) {
       //We don't need syncGroup so simplify implmenetation
       set(speed)
   }

   //Not entirely sure this shouldn't return getPIDController.getSetpoint()
   public double get() {
       return encoder.getRate();
   }
}

With the above class definition, we could create 4 NordicSpeedController objects each with a different Talon and encoder. We can then pass those 4 NordicSpeedControllers to RobotDrive() constructor which requires 4 SpeedControllers and since we implement the SpeedController Interface it is happy to accept our PIDSubsystems as SpeedControllers.

This allows us to call driveTrain.mecanumDrive which in turn calls m_robotDrive.mecanumDrive_Cartesian() and we can request that we drive 0.1 (10 % of full speed). To move the robot, the PID can apply 1.0 power for a few ms and as the wheels start turning our rate increases until the robot can move, then the PID keeps increasing/decreasing motor power to maintain 10% speed. Without PID, 0.1 power would likely not even overcome friction.

You can then create PIDCommands to act on the drive train as a whole for DriveToDistance or RotateToAngle or DriveAlongAngle and it will call tankDrive or mecanumDrive in the usePIDOutput() function and read the distance off an appropriate encoder or the angle off the gyro to make adjustments to the rotation or forward speed.