1 package org
.usfirst
.frc
.team3501
.robot
.subsystems
;
3 import org
.usfirst
.frc
.team3501
.robot
.CheesyDriveHelper
;
4 import org
.usfirst
.frc
.team3501
.robot
.Constants
;
5 import org
.usfirst
.frc
.team3501
.robot
.commands
.driving
.JoystickDrive
;
7 import edu
.wpi
.first
.wpilibj
.CANTalon
;
8 import edu
.wpi
.first
.wpilibj
.CounterBase
.EncodingType
;
9 import edu
.wpi
.first
.wpilibj
.DoubleSolenoid
;
10 import edu
.wpi
.first
.wpilibj
.DoubleSolenoid
.Value
;
11 import edu
.wpi
.first
.wpilibj
.Encoder
;
12 import edu
.wpi
.first
.wpilibj
.RobotDrive
;
13 import edu
.wpi
.first
.wpilibj
.command
.PIDSubsystem
;
15 public class DriveTrain
extends PIDSubsystem
{
16 // Determines if the "front" of the robot has been reversed
17 private boolean outputFlipped
= false;
19 private static double pidOutput
= 0;
21 private Encoder leftEncoder
, rightEncoder
;
23 private CANTalon frontLeft
, frontRight
, rearLeft
, rearRight
;
24 private RobotDrive robotDrive
;
26 private DoubleSolenoid leftGearPiston
, rightGearPiston
;
28 private CheesyDriveHelper cheese
;
30 // Drivetrain specific constants that relate to the inches per pulse value for
34 super(Constants
.DriveTrain
.kp
, Constants
.DriveTrain
.ki
,
35 Constants
.DriveTrain
.kd
);
37 frontLeft
= new CANTalon(Constants
.DriveTrain
.DRIVE_FRONT_LEFT
);
38 frontRight
= new CANTalon(Constants
.DriveTrain
.DRIVE_FRONT_RIGHT
);
39 rearLeft
= new CANTalon(Constants
.DriveTrain
.DRIVE_REAR_LEFT
);
40 rearRight
= new CANTalon(Constants
.DriveTrain
.DRIVE_REAR_RIGHT
);
42 robotDrive
= new RobotDrive(frontLeft
, rearLeft
, frontRight
, rearRight
);
44 leftEncoder
= new Encoder(Constants
.DriveTrain
.ENCODER_LEFT_A
,
45 Constants
.DriveTrain
.ENCODER_LEFT_B
, false, EncodingType
.k4X
);
46 rightEncoder
= new Encoder(Constants
.DriveTrain
.ENCODER_RIGHT_A
,
47 Constants
.DriveTrain
.ENCODER_RIGHT_B
, false, EncodingType
.k4X
);
48 leftEncoder
.setDistancePerPulse(Constants
.DriveTrain
.INCHES_PER_PULSE
);
49 rightEncoder
.setDistancePerPulse(Constants
.DriveTrain
.INCHES_PER_PULSE
);
51 leftEncoder
.setDistancePerPulse(Constants
.DriveTrain
.INCHES_PER_PULSE
);
52 rightEncoder
.setDistancePerPulse(Constants
.DriveTrain
.INCHES_PER_PULSE
);
56 leftGearPiston
= new DoubleSolenoid(Constants
.DriveTrain
.LEFT_SHIFT_MODULE
,
57 Constants
.DriveTrain
.LEFT_SHIFT_FORWARD
,
58 Constants
.DriveTrain
.LEFT_SHIFT_REVERSE
);
59 rightGearPiston
= new DoubleSolenoid(
60 Constants
.DriveTrain
.RIGHT_SHIFT_MODULE
,
61 Constants
.DriveTrain
.RIGHT_SHIFT_FORWARD
,
62 Constants
.DriveTrain
.RIGHT_SHIFT_REVERSE
);
64 cheese
= new CheesyDriveHelper(this);
68 protected void initDefaultCommand() {
69 setDefaultCommand(new JoystickDrive());
72 // Print tne PID Output
73 public void printOutput() {
74 System
.out
.println("PIDOutput: " + pidOutput
);
77 private double getAvgEncoderDistance() {
78 return (leftEncoder
.getDistance() + rightEncoder
.getDistance()) / 2;
81 // Whether or not the PID Controller thinks we have reached the target
83 public boolean reachedTarget() {
84 if (this.onTarget()) {
96 public void resetEncoders() {
101 public double getRightSpeed() {
102 return rightEncoder
.getRate(); // in inches per second
105 public double getLeftSpeed() {
106 return leftEncoder
.getRate(); // in inches per second
109 public double getSpeed() {
110 return (getLeftSpeed() + getRightSpeed()) / 2.0; // in inches per second
113 public double getRightDistance() {
114 return rightEncoder
.getDistance(); // in inches
117 public double getLeftDistance() {
118 return leftEncoder
.getDistance(); // in inches
121 // Get error between the setpoint of PID Controller and the current state of
123 public double getError() {
124 return Math
.abs(this.getSetpoint() - getAvgEncoderDistance());
127 public void printEncoder(int i
, int n
) {
129 System
.out
.println("Left: " + this.getLeftDistance());
130 System
.out
.println("Right: " + this.getRightDistance());
136 * returns the PID output that is returned by the PID Controller
138 public double getOutput() {
142 // Updates the PID constants based on which control mode is being used
143 public void updatePID() {
144 this.getPIDController().setPID(Constants
.DriveTrain
.kp
,
145 Constants
.DriveTrain
.ki
, Constants
.DriveTrain
.kd
);
148 public CANTalon
getFrontLeft() {
152 public CANTalon
getFrontRight() {
156 public CANTalon
getRearLeft() {
160 public CANTalon
getRearRight() {
165 * Method is a required method that the PID Subsystem uses to return the
166 * calculated PID value to the driver
168 * @param Gives the user the output from the PID algorithm that is calculated
171 * Body: Uses the output, does some filtering and drives the robot
174 protected void usePIDOutput(double output
) {
177 double drift
= this.getLeftDistance() - this.getRightDistance();
178 if (Math
.abs(output
) > 0 && Math
.abs(output
) < 0.3)
179 output
= Math
.signum(output
) * 0.3;
181 right
= output
+ drift
* Constants
.DriveTrain
.kp
/ 10;
182 setMotorSpeeds(left
, right
);
187 protected double returnPIDInput() {
188 return sensorFeedback();
192 * Checks the drive mode
194 * @return the current state of the robot in each state Average distance from
195 * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE
197 private double sensorFeedback() {
198 return getAvgEncoderDistance();
201 public void joystickDrive(double left
, double right
) {
202 // Handle flipping of the "front" of the robot
203 double k
= (isFlipped() ?
-1 : 1);
204 cheese
.cheesyDrive(-left
* k
, -right
,
205 getGearPistonValue() == Constants
.DriveTrain
.HIGH_GEAR
);
208 public void setMotorSpeeds(double left
, double right
) {
209 double k
= (isFlipped() ?
-1 : 1);
211 * cheese.cheesyDrive(-left * k, -right, getGearPistonValue() ==
212 * Constants.DriveTrain.HIGH_GEAR);
217 frontRight
.set(right
);
218 rearRight
.set(right
);
222 * @return a value that is the current setpoint for the piston (kReverse or
225 public Value
getGearPistonValue() {
226 return leftGearPiston
.get(); // Pistons should always be in the same state
230 * Changes the ball shift gear assembly to high
232 public void setHighGear() {
233 changeGear(Constants
.DriveTrain
.HIGH_GEAR
);
237 * Changes the ball shift gear assembly to low
239 public void setLowGear() {
240 changeGear(Constants
.DriveTrain
.LOW_GEAR
);
244 * Changes the gear to a DoubleSolenoid.Value
246 public void changeGear(DoubleSolenoid
.Value gear
) {
247 leftGearPiston
.set(gear
);
248 rightGearPiston
.set(gear
);
252 * Switches drivetrain gears from high to low or low to high
254 public void switchGear() {
255 Value currentValue
= getGearPistonValue();
256 Value setValue
= (currentValue
== Constants
.DriveTrain
.HIGH_GEAR
)
257 ? Constants
.DriveTrain
.LOW_GEAR
: Constants
.DriveTrain
.HIGH_GEAR
;
258 changeGear(setValue
);
262 * Toggle whether the motor outputs are flipped, effectively switching which
263 * side of the robot is the front.
265 public void toggleFlipped() {
266 outputFlipped
= !outputFlipped
;
269 public boolean isFlipped() {
270 return outputFlipped
;