So the team I was working with last weekend is running a standard tank drive and was looking to use two encoders and a gyro to provide more reliable autonomous movement. What we ended up something like this.
DriveTrain Subsystem Class
public class Drivetrain extends Subsystem {
private Talon frontLeft = new Talon(RobotMap.DRIVETRAIN_FRONT_LEFT);
private Talon frontRight = new Talon(RobotMap.DRIVETRAIN_FRONT_RIGHT);
private Talon rearLeft = new Talon(RobotMap.DRIVETRAIN_REAR_LEFT);
private Talon rearRight = new Talon(RobotMap.DRIVETRAIN_REAR_RIGHT);
private Encoder leftEncoder = new Encoder(RobotMap.DRIVETRAIN_ENCODER_LEFT_A, RobotMap.DRIVETRAIN_ENCODER_LEFT_B);
private Encoder rightEncoder = new Encoder(RobotMap.DRIVETRAIN_ENCODER_RIGHT_A, RobotMap.DRIVETRAIN_ENCODER_RIGHT_B);
private ADXRS450_Gyro gyro = new ADXRS450_Gyro(SPI.Port.kOnboardCS0);
private RobotDrive drive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
public void initDefaultCommand() {
final double WHEEL_CIRCUMFERENCE = 0.1524 * Math.PI; // 6 inch diameter wheel
final double GEAR_RATIO = 4.0; // Reduction from encoder shaft and output shaft
final double PULSES_PER_REVOLUTION = 1440.0; // Number of encoder counts per revolution
leftEncoder.setDistancePerPulse(WHEEL_CIRCUMFERENCE * GEAR_RATIO / PULSES_PER_REVOLUTION);
rightEncoder.setDistancePerPulse(WHEEL_CIRCUMFERENCE * GEAR_RATIO / PULSES_PER_REVOLUTION);
drive.setSafetyEnabled(true);
setDefaultCommand(new DrivetrainArcadeDrive());
}
public void stop() {
setLeftMotors(0.0);
setRightMotors(0.0);
}
public void arcadeDrive(double move, double rotate) {
drive.arcadeDrive(move, rotate);
}
public void setLeftMotors(double speed) {
frontLeft.set(speed);
rearLeft.set(speed);
}
public void setRightMotors(double speed) {
frontRight.set(speed);
rearRight.set(speed);
}
public void rotate(double speed) {
drive.arcadeDrive(0, speed);
}
// Get Sensor Values
public void resetLeftEncoder() {
leftEncoder.reset();
}
public void resetRightEncoder() {
rightEncoder.reset();
}
public void resetGyro() {
gyro.reset();
}
public double getAngle() {
return gyro.getAngle();
}
public double getLeftDistance() {
return leftEncoder.getDistance();
}
public double getRightDistance() {
return rightEncoder.getDistance();
}
}
DriveToDistanceCommand (use encoders to drive roughly straight to distance)
public class DrivetrainForwardToDistance extends Command {
private double distance;
private double tolerence;
private PIDController leftMotorController;
private PIDController rightMotorController;
// distance : the distance in meters to move
// tolerance: the distance in meters of acceptable error
public DrivetrainForwardToDistance(double distance, double tolerance) {
this.distance = distance;
this.tolerence = tolerance;
requires(Robot.drivetrain);
}
// Called just before this Command runs the first time
protected void initialize() {
Robot.drivetrain.resetLeftEncoder();
Robot.drivetrain.resetRightEncoder();
PIDSource leftMotorSource = new PIDSource() {
@Override
public void setPIDSourceType(PIDSourceType pidSource) {}
@Override
public double pidGet() {
return Robot.drivetrain.getLeftDistance();
}
@Override
public PIDSourceType getPIDSourceType() {
return PIDSourceType.kDisplacement;
}
};
PIDSource rightMotorSource = new PIDSource() {
@Override
public void setPIDSourceType(PIDSourceType pidSource) {}
@Override
public double pidGet() {
return Robot.drivetrain.getLeftDistance();
}
@Override
public PIDSourceType getPIDSourceType() {
return PIDSourceType.kDisplacement;
}
};
PIDOutput leftMotorOutput = new PIDOutput() {
@Override
public void pidWrite(double output) {
Robot.drivetrain.setLeftMotors(output);
}
};
PIDOutput rightMotorOutput = new PIDOutput() {
@Override
public void pidWrite(double output) {
Robot.drivetrain.setRightMotors(output);
}
};
final double Kp = 0.3;
final double Ki = 0.0;
final double Kd = 0.001;
leftMotorController = new PIDController(Kp, Ki, Kd, leftMotorSource, leftMotorOutput);
rightMotorController = new PIDController(Kp, Ki, Kd, rightMotorSource, rightMotorOutput);
leftMotorController.setAbsoluteTolerance(tolerence);
rightMotorController.setAbsoluteTolerance(tolerence);
final double MIN_SPEED = 0.1;
final double MAX_SPEED = 0.5;
if (distance > 0) {
leftMotorController.setOutputRange(MIN_SPEED, MAX_SPEED);
rightMotorController.setOutputRange(MIN_SPEED, MAX_SPEED);
}
else {
leftMotorController.setOutputRange(-MIN_SPEED, -MAX_SPEED);
rightMotorController.setOutputRange(-MIN_SPEED, -MAX_SPEED);
}
leftMotorController.setSetpoint(distance);
rightMotorController.setSetpoint(distance);
leftMotorController.enable();
rightMotorController.enable();
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
SmartDashboard.putNumber("Left Distance", Robot.drivetrain.getLeftDistance());
SmartDashboard.putNumber("Right Distance", Robot.drivetrain.getRightDistance());
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return leftMotorController.onTarget() && rightMotorController.onTarget();
}
// Called once after isFinished returns true
protected void end() {
leftMotorController.disable();
rightMotorController.disable();
Robot.drivetrain.stop();
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
end();
}
}
RotateToAngle (use gyro to rotate to certain angle)
public class DrivetrainRotateToAngle extends Command {
private double angle;
private double tolerance;
private PIDController angleMotorController;
public DrivetrainRotateToAngle(double angle, double tolerance) {
this.angle = angle;
this.tolerance = tolerance;
requires(Robot.drivetrain);
}
// Called just before this Command runs the first time
protected void initialize() {
Robot.drivetrain.resetGyro();
PIDSource angleSource = new PIDSource() {
@Override
public void setPIDSourceType(PIDSourceType pidSource) {}
@Override
public double pidGet() {
return Robot.drivetrain.getAngle();
}
@Override
public PIDSourceType getPIDSourceType() {
return PIDSourceType.kDisplacement;
}
};
PIDOutput angleOutput = new PIDOutput() {
@Override
public void pidWrite(double output) {
Robot.drivetrain.rotate(output);
}
};
final double Kp = 0.3;
final double Ki = 0.01;
final double Kd = 0.05;
angleMotorController = new PIDController(Kp, Ki, Kd, angleSource, angleOutput);
angleMotorController.setAbsoluteTolerance(tolerance);
final double MIN_SPEED = 0.5;
final double MAX_SPEED = 1.0;
if (angle > 0) {
angleMotorController.setOutputRange(MIN_SPEED, MAX_SPEED);
}
else {
angleMotorController.setOutputRange(-MIN_SPEED, -MAX_SPEED);
}
angleMotorController.setSetpoint(angle);
angleMotorController.enable();
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
SmartDashboard.putNumber("Angle", Robot.drivetrain.getAngle());
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return angleMotorController.onTarget();
}
// Called once after isFinished returns true
protected void end() {
angleMotorController.disable();
Robot.drivetrain.stop();
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
end();
}
}
We had some success with this code, but it is still far from perfect.
Questions:
- Is this a recommended way to have multiple PID controllers on a single subsystem?
- Do I need to have multiple PID controllers to get this functionality (a command to turn precisely and a command to drive to distance)? Is there a more simple and elegant way possibly utilizing the RobotDrive.arcadeDrive() method I am overlooking?
- How do you add the controller to the Smart Dashboard so that the gains can be tuned without the need to re-deploy code on every change?
- Can you give an example explaining what the PIDController.setContinuous() method does? I have read the Javadoc, but still don’t understand how it could be useful.