Quote:
Originally Posted by Brian Selle
I would recommend making a single PID loop with multiple modes. Use commands to switch the mode and set distance, angle, etc.
|
So you would do something like this with more sensors?
Code:
public class DriveTrain extends PIDSubsystem {
private final AnalogGyro analogGyro1 = RobotMap.driveTrainAnalogGyro1;
private final SpeedController speedController1 = RobotMap.driveTrainSpeedController1;
private final SpeedController speedController2 = RobotMap.driveTrainSpeedController2;
private final SpeedController speedController3 = RobotMap.driveTrainSpeedController3;
private final SpeedController speedController4 = RobotMap.driveTrainSpeedController4;
private final RobotDrive robotDrive41 = RobotMap.driveTrainRobotDrive41;
public DriveTrain() {
super("DriveTrain", 1.0, 0.0, 0.0);
setAbsoluteTolerance(0.2);
getPIDController().setContinuous(false);
LiveWindow.addActuator("Drive Train", "PIDSubsystem Controller", getPIDController());
}
public void initDefaultCommand() {
}
protected double returnPIDInput(double sensor) {
return sensor;
}
protected void usePIDOutput(double output) {
speedController1.pidWrite(output);
}
public double returnGyro(){
return analogGyro1.pidGet();
}
}