1 package org
.usfirst
.frc
.team3501
.robot
.subsystems
;
3 import org
.usfirst
.frc
.team3501
.robot
.Constants
;
4 import org
.usfirst
.frc
.team3501
.robot
.commands
.driving
.JoystickDrive
;
6 import edu
.wpi
.first
.wpilibj
.CANTalon
;
7 import edu
.wpi
.first
.wpilibj
.CounterBase
.EncodingType
;
8 import edu
.wpi
.first
.wpilibj
.DoubleSolenoid
;
9 import edu
.wpi
.first
.wpilibj
.DoubleSolenoid
.Value
;
10 import edu
.wpi
.first
.wpilibj
.Encoder
;
11 import edu
.wpi
.first
.wpilibj
.RobotDrive
;
12 import edu
.wpi
.first
.wpilibj
.command
.PIDSubsystem
;
14 public class DriveTrain
extends PIDSubsystem
{
15 private boolean outputFlipped
= false;
16 private static double pidOutput
= 0;
18 private Encoder leftEncoder
, rightEncoder
;
20 private CANTalon frontLeft
, frontRight
, rearLeft
, rearRight
;
21 private RobotDrive robotDrive
;
23 private DoubleSolenoid leftGearPiston
, rightGearPiston
;
25 // Drivetrain specific constants that relate to the inches per pulse value for
29 super(Constants
.DriveTrain
.kp
, Constants
.DriveTrain
.ki
,
30 Constants
.DriveTrain
.kd
);
32 frontLeft
= new CANTalon(Constants
.DriveTrain
.FRONT_LEFT
);
33 frontRight
= new CANTalon(Constants
.DriveTrain
.FRONT_RIGHT
);
34 rearLeft
= new CANTalon(Constants
.DriveTrain
.REAR_LEFT
);
35 rearRight
= new CANTalon(Constants
.DriveTrain
.REAR_RIGHT
);
37 robotDrive
= new RobotDrive(frontLeft
, rearLeft
, frontRight
, rearRight
);
39 leftEncoder
= new Encoder(Constants
.DriveTrain
.ENCODER_LEFT_A
,
40 Constants
.DriveTrain
.ENCODER_LEFT_B
, false, EncodingType
.k4X
);
41 rightEncoder
= new Encoder(Constants
.DriveTrain
.ENCODER_RIGHT_A
,
42 Constants
.DriveTrain
.ENCODER_RIGHT_B
, false, EncodingType
.k4X
);
43 leftEncoder
.setDistancePerPulse(Constants
.DriveTrain
.INCHES_PER_PULSE
);
44 rightEncoder
.setDistancePerPulse(Constants
.DriveTrain
.INCHES_PER_PULSE
);
46 leftEncoder
.setDistancePerPulse(Constants
.DriveTrain
.INCHES_PER_PULSE
);
47 rightEncoder
.setDistancePerPulse(Constants
.DriveTrain
.INCHES_PER_PULSE
);
51 leftGearPiston
= new DoubleSolenoid(Constants
.DriveTrain
.LEFT_MODULE
,
52 Constants
.DriveTrain
.LEFT_FORWARD
, Constants
.DriveTrain
.LEFT_REVERSE
);
53 rightGearPiston
= new DoubleSolenoid(Constants
.DriveTrain
.RIGHT_MODULE
,
54 Constants
.DriveTrain
.RIGHT_FORWARD
, Constants
.DriveTrain
.RIGHT_REVERSE
);
58 protected void initDefaultCommand() {
59 setDefaultCommand(new JoystickDrive());
62 // Print tne PID Output
63 public void printOutput() {
64 System
.out
.println("PIDOutput: " + pidOutput
);
67 private double getAvgEncoderDistance() {
68 return (leftEncoder
.getDistance() + rightEncoder
.getDistance()) / 2;
71 // Whether or not the PID Controller thinks we have reached the target
73 public boolean reachedTarget() {
74 if (this.onTarget()) {
86 public void resetEncoders() {
91 public double getRightSpeed() {
92 return rightEncoder
.getRate(); // in inches per second
95 public double getLeftSpeed() {
96 return leftEncoder
.getRate(); // in inches per second
99 public double getSpeed() {
100 return (getLeftSpeed() + getRightSpeed()) / 2.0; // in inches per second
103 public double getRightDistance() {
104 return rightEncoder
.getDistance(); // in inches
107 public double getLeftDistance() {
108 return leftEncoder
.getDistance(); // in inches
111 // Get error between the setpoint of PID Controller and the current state of
113 public double getError() {
114 return Math
.abs(this.getSetpoint() - getAvgEncoderDistance());
117 public void printEncoder(int i
, int n
) {
119 System
.out
.println("Left: " + this.getLeftDistance());
120 System
.out
.println("Right: " + this.getRightDistance());
126 * returns the PID output that is returned by the PID Controller
128 public double getOutput() {
132 // Updates the PID constants based on which control mode is being used
133 public void updatePID() {
134 this.getPIDController().setPID(Constants
.DriveTrain
.kp
,
135 Constants
.DriveTrain
.ki
, Constants
.DriveTrain
.kd
);
138 public CANTalon
getFrontLeft() {
142 public CANTalon
getFrontRight() {
146 public CANTalon
getRearLeft() {
150 public CANTalon
getRearRight() {
155 * Method is a required method that the PID Subsystem uses to return the
156 * calculated PID value to the driver
158 * @param Gives the user the output from the PID algorithm that is calculated
161 * Body: Uses the output, does some filtering and drives the robot
164 protected void usePIDOutput(double output
) {
167 double drift
= this.getLeftDistance() - this.getRightDistance();
168 if (Math
.abs(output
) > 0 && Math
.abs(output
) < 0.3)
169 output
= Math
.signum(output
) * 0.3;
171 right
= output
+ drift
* Constants
.DriveTrain
.kp
/ 10;
177 protected double returnPIDInput() {
178 return sensorFeedback();
182 * Checks the drive mode
184 * @return the current state of the robot in each state Average distance from
185 * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE
187 private double sensorFeedback() {
188 return getAvgEncoderDistance();
192 * @param left and right setpoints to set to the left and right side of tank
193 * inverted is for Logan, wants the robot to invert all controls left = right
194 * and right = left negative input is required for the regular rotation
195 * because RobotDrive tankdrive method drives inverted
197 public void drive(double left
, double right
) {
198 robotDrive
.tankDrive(-left
, -right
);
201 public void setMotorSpeeds(double left
, double right
) {
202 // positive setpoint to left side makes it go backwards
203 // positive setpoint to right side makes it go forwards.
204 frontLeft
.set(-left
);
206 frontRight
.set(right
);
207 rearRight
.set(right
);
211 * @return a value that is the current setpoint for the piston (kReverse or
214 public Value
getGearPistonValue() {
215 return leftGearPiston
.get(); // Pistons should always be in the same state
219 * Changes the ball shift gear assembly to high
221 public void setHighGear() {
222 changeGear(Constants
.DriveTrain
.HIGH_GEAR
);
226 * Changes the ball shift gear assembly to low
228 public void setLowGear() {
229 changeGear(Constants
.DriveTrain
.LOW_GEAR
);
233 * Changes the gear to a DoubleSolenoid.Value
235 public void changeGear(DoubleSolenoid
.Value gear
) {
236 leftGearPiston
.set(gear
);
237 rightGearPiston
.set(gear
);
241 * Switches drivetrain gears from high to low or low to high
243 public void switchGear() {
244 Value currentValue
= getGearPistonValue();
245 Value setValue
= (currentValue
== Constants
.DriveTrain
.HIGH_GEAR
) ? Constants
.DriveTrain
.LOW_GEAR
246 : Constants
.DriveTrain
.HIGH_GEAR
;
247 changeGear(setValue
);
251 * Toggle whether the motor outputs are flipped, effectively switching which
252 * side of the robot is the front.
254 public void toggleFlipped() {
255 outputFlipped
= !outputFlipped
;
258 public boolean isFlipped() {
259 return outputFlipped
;