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 // Determines if the "front" of the robot has been reversed
16 private boolean outputFlipped
= false;
18 private static double pidOutput
= 0;
20 private Encoder leftEncoder
, rightEncoder
;
22 private CANTalon frontLeft
, frontRight
, rearLeft
, rearRight
;
23 private RobotDrive robotDrive
;
25 private DoubleSolenoid leftGearPiston
, rightGearPiston
;
27 // Drivetrain specific constants that relate to the inches per pulse value for
31 super(Constants
.DriveTrain
.kp
, Constants
.DriveTrain
.ki
,
32 Constants
.DriveTrain
.kd
);
34 frontLeft
= new CANTalon(Constants
.DriveTrain
.DRIVE_FRONT_LEFT
);
35 frontRight
= new CANTalon(Constants
.DriveTrain
.DRIVE_FRONT_RIGHT
);
36 rearLeft
= new CANTalon(Constants
.DriveTrain
.DRIVE_REAR_LEFT
);
37 rearRight
= new CANTalon(Constants
.DriveTrain
.DRIVE_REAR_RIGHT
);
39 robotDrive
= new RobotDrive(frontLeft
, rearLeft
, frontRight
, rearRight
);
41 leftEncoder
= new Encoder(Constants
.DriveTrain
.ENCODER_LEFT_A
,
42 Constants
.DriveTrain
.ENCODER_LEFT_B
, false, EncodingType
.k4X
);
43 rightEncoder
= new Encoder(Constants
.DriveTrain
.ENCODER_RIGHT_A
,
44 Constants
.DriveTrain
.ENCODER_RIGHT_B
, false, EncodingType
.k4X
);
45 leftEncoder
.setDistancePerPulse(Constants
.DriveTrain
.INCHES_PER_PULSE
);
46 rightEncoder
.setDistancePerPulse(Constants
.DriveTrain
.INCHES_PER_PULSE
);
48 leftEncoder
.setDistancePerPulse(Constants
.DriveTrain
.INCHES_PER_PULSE
);
49 rightEncoder
.setDistancePerPulse(Constants
.DriveTrain
.INCHES_PER_PULSE
);
53 leftGearPiston
= new DoubleSolenoid(Constants
.DriveTrain
.LEFT_SHIFT_MODULE
,
54 Constants
.DriveTrain
.LEFT_SHIFT_FORWARD
,
55 Constants
.DriveTrain
.LEFT_SHIFT_REVERSE
);
56 rightGearPiston
= new DoubleSolenoid(
57 Constants
.DriveTrain
.RIGHT_SHIFT_MODULE
,
58 Constants
.DriveTrain
.RIGHT_SHIFT_FORWARD
,
59 Constants
.DriveTrain
.RIGHT_SHIFT_REVERSE
);
63 protected void initDefaultCommand() {
64 setDefaultCommand(new JoystickDrive());
67 // Print tne PID Output
68 public void printOutput() {
69 System
.out
.println("PIDOutput: " + pidOutput
);
72 private double getAvgEncoderDistance() {
73 return (leftEncoder
.getDistance() + rightEncoder
.getDistance()) / 2;
76 // Whether or not the PID Controller thinks we have reached the target
78 public boolean reachedTarget() {
79 if (this.onTarget()) {
91 public void resetEncoders() {
96 public double getRightSpeed() {
97 return rightEncoder
.getRate(); // in inches per second
100 public double getLeftSpeed() {
101 return leftEncoder
.getRate(); // in inches per second
104 public double getSpeed() {
105 return (getLeftSpeed() + getRightSpeed()) / 2.0; // in inches per second
108 public double getRightDistance() {
109 return rightEncoder
.getDistance(); // in inches
112 public double getLeftDistance() {
113 return leftEncoder
.getDistance(); // in inches
116 // Get error between the setpoint of PID Controller and the current state of
118 public double getError() {
119 return Math
.abs(this.getSetpoint() - getAvgEncoderDistance());
122 public void printEncoder(int i
, int n
) {
124 System
.out
.println("Left: " + this.getLeftDistance());
125 System
.out
.println("Right: " + this.getRightDistance());
131 * returns the PID output that is returned by the PID Controller
133 public double getOutput() {
137 // Updates the PID constants based on which control mode is being used
138 public void updatePID() {
139 this.getPIDController().setPID(Constants
.DriveTrain
.kp
,
140 Constants
.DriveTrain
.ki
, Constants
.DriveTrain
.kd
);
143 public CANTalon
getFrontLeft() {
147 public CANTalon
getFrontRight() {
151 public CANTalon
getRearLeft() {
155 public CANTalon
getRearRight() {
160 * Method is a required method that the PID Subsystem uses to return the
161 * calculated PID value to the driver
163 * @param Gives the user the output from the PID algorithm that is calculated
166 * Body: Uses the output, does some filtering and drives the robot
169 protected void usePIDOutput(double output
) {
172 double drift
= this.getLeftDistance() - this.getRightDistance();
173 if (Math
.abs(output
) > 0 && Math
.abs(output
) < 0.3)
174 output
= Math
.signum(output
) * 0.3;
176 right
= output
+ drift
* Constants
.DriveTrain
.kp
/ 10;
177 setMotorSpeeds(left
, right
);
182 protected double returnPIDInput() {
183 return sensorFeedback();
187 * Checks the drive mode
189 * @return the current state of the robot in each state Average distance from
190 * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE
192 private double sensorFeedback() {
193 return getAvgEncoderDistance();
196 public void joystickDrive(double left
, double right
) {
197 int type
= Constants
.DriveTrain
.DRIVE_TYPE
;
199 // Handle flipping of the "front" of the robot
200 double k
= (isFlipped() ?
-1 : 1);
202 robotDrive
.tankDrive(-left
, -right
);
204 // if (type == Constants.DriveTrain.TANK) {
205 // // TODO Test this for negation and for voltage vs. [-1,1] outputs
206 // double leftSpeed = (-frontLeft.get() + -rearLeft.get()) / 2;
207 // double rightSpeed = (-frontRight.get() + -rearRight.get()) / 2;
208 // left = ((Constants.DriveTrain.kADJUST - 1) * leftSpeed + left)
209 // / Constants.DriveTrain.kADJUST;
210 // right = ((Constants.DriveTrain.kADJUST - 1) * rightSpeed + right)
211 // / Constants.DriveTrain.kADJUST;
212 // robotDrive.tankDrive(-left * k, -right * k);
213 // } else if (type == Constants.DriveTrain.ARCADE) {
214 // double speed = (-frontLeft.get() + -rearLeft.get() + -frontRight.get() +
217 // left = ((Constants.DriveTrain.kADJUST - 1) * speed + left)
218 // / Constants.DriveTrain.kADJUST;
219 // robotDrive.arcadeDrive(left * k, right);
223 public void setMotorSpeeds(double left
, double right
) {
224 double k
= (isFlipped() ?
-1 : 1);
225 robotDrive
.tankDrive(-left
* k
, -right
* k
);
229 * @return a value that is the current setpoint for the piston (kReverse or
232 public Value
getGearPistonValue() {
233 return leftGearPiston
.get(); // Pistons should always be in the same state
237 * Changes the ball shift gear assembly to high
239 public void setHighGear() {
240 changeGear(Constants
.DriveTrain
.HIGH_GEAR
);
244 * Changes the ball shift gear assembly to low
246 public void setLowGear() {
247 changeGear(Constants
.DriveTrain
.LOW_GEAR
);
251 * Changes the gear to a DoubleSolenoid.Value
253 public void changeGear(DoubleSolenoid
.Value gear
) {
254 leftGearPiston
.set(gear
);
255 rightGearPiston
.set(gear
);
259 * Switches drivetrain gears from high to low or low to high
261 public void switchGear() {
262 Value currentValue
= getGearPistonValue();
263 Value setValue
= (currentValue
== Constants
.DriveTrain
.HIGH_GEAR
) ? Constants
.DriveTrain
.LOW_GEAR
264 : Constants
.DriveTrain
.HIGH_GEAR
;
265 changeGear(setValue
);
269 * Toggle whether the motor outputs are flipped, effectively switching which
270 * side of the robot is the front.
272 public void toggleFlipped() {
273 outputFlipped
= !outputFlipped
;
276 public boolean isFlipped() {
277 return outputFlipped
;