Code:
package edu.wpi.first.wpilibj.templates.subsystems;
import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.CommandBase;
import edu.wpi.first.wpilibj.templates.commands.DriveWithJoysticks;
/**
* <p>The drive train is PID subsystem, but unlike the {@link Wrist} and
* {@link Elevator}, it is not always running PID. Instead, it can be run in a
* manual tank drive or PID can be enabled and it will use a range finder to
* drive a fixed distance away from the target.</p>
*
* <p>Recommended next step: {@link CommandBase}</p>
*
* @author Alex Henning
*/
public class DriveTrain extends PIDSubsystem {
// The constants for the P, I and D portion of PID
private static final double Kp = 3;
private static final double Ki = .2;
private static final double Kd = 0.0;
RobotDrive drive;
AnalogChannel rangefinder;
// Initialize your subsystem here
public DriveTrain() {
super("DriveTrain", Kp, Ki, Kd);
drive = new RobotDrive(RobotMap.leftMotor, RobotMap.rightMotor);
rangefinder = new AnalogChannel(RobotMap.rangefinder);
}
/**
* Set the default command to drive with joysticks.
*/
public void initDefaultCommand() {
setDefaultCommand(new DriveWithJoysticks());
}
/**
* @return The value of the rangefinder used as the PID input device.
* These values correspond to your PID setpoint, in this case the
* value can be anywhere between 0v and 5v.
*/
protected double returnPIDInput() {
return rangefinder.getVoltage();
}
/**
* @param output The value to set the output to as determined by the PID
* algorithm. This gets called each time through the PID loop
* to update the output to the motor.
*/
protected void usePIDOutput(double output) {
tankDrive(output, output);
}
/**
* Implements the tank drive capability of the drivetrain.
*
* @param left The speed for the left side of the drivetrain.
* @param right The speed for the right side of the drivetrain.
*/
public void tankDrive(double left, double right) {
drive.tankDrive(left, right);
}
}
This is his Drive Subsystem.
Code:
package edu.wpi.first.wpilibj.templates.commands;
/**
* <p>Similar to SetWristSetpoint, but it also has to handle enabling and
* disabling the PID loop</p>
*
* <p>Recommended next step: {@link PrepareToGrab}</p>
*
* @author Alex Henning
*/
public class DriveToDistance extends CommandBase {
double setpoint;
/**
* Require the drive train and store the desired setpoint.
*
* @param setpoint The desired setpoint for the drive train.
*/
public DriveToDistance(double setpoint) {
requires(drivetrain);
this.setpoint = setpoint;
}
// Called just before this Command runs the first time
/**
* Set the setpoint to the stored value and enable PID on the drivetrain.
*/
protected void initialize() {
drivetrain.setSetpoint(setpoint);
drivetrain.enable();
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
}
// Make this return true when this Command no longer needs to run execute()
/**
* @return true when it's close enough to the setpoint
*/
protected boolean isFinished() {
return Math.abs(drivetrain.getPosition() - setpoint) < .02;
}
// Called once after isFinished returns true
/**
* When this command ends, disable the drivetrain's PID
*/
protected void end() {
drivetrain.disable();
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
/**
* When this command exits, disable the drivetrain's PID
*/
protected void interrupted() {
drivetrain.disable();
}
}
This is the only PID Drive command I see...I'm not sure where he even uses the rangefinder.
because actually using a rangefinder is exactly what I'm going to be doing in the end.