Modify code to allow switching of joystick drive type (tank vs. arcade)
[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / subsystems / DriveTrain.java
CommitLineData
38a404b3
KZ
1package org.usfirst.frc.team3501.robot.subsystems;
2
3import org.usfirst.frc.team3501.robot.Constants;
43338192 4import org.usfirst.frc.team3501.robot.Robot;
33141cdd 5import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
111dc444 6
38a404b3 7import edu.wpi.first.wpilibj.CANTalon;
111dc444 8import edu.wpi.first.wpilibj.CounterBase.EncodingType;
d9c04720 9import edu.wpi.first.wpilibj.DoubleSolenoid;
2aea5cc2 10import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
111dc444 11import edu.wpi.first.wpilibj.Encoder;
33141cdd
KZ
12import edu.wpi.first.wpilibj.RobotDrive;
13import edu.wpi.first.wpilibj.command.PIDSubsystem;
14
15public class DriveTrain extends PIDSubsystem {
571a0e2a 16 private boolean outputFlipped = false;
fb75626b 17 private static double pidOutput = 0;
38a404b3 18
1884c3cf 19 private Encoder leftEncoder, rightEncoder;
96215d97 20
d7bf2340 21 private CANTalon frontLeft, frontRight, rearLeft, rearRight;
33141cdd
KZ
22 private RobotDrive robotDrive;
23
d9c04720 24 private DoubleSolenoid leftGearPiston, rightGearPiston;
fb75626b 25
9e65c056
ME
26 // Drivetrain specific constants that relate to the inches per pulse value for
27 // the encoders
71d73690 28
d7bf2340 29 public DriveTrain() {
fb75626b
KZ
30 super(Constants.DriveTrain.kp, Constants.DriveTrain.ki,
31 Constants.DriveTrain.kd);
33141cdd 32
e1f7a742
HD
33 frontLeft = new CANTalon(Constants.DriveTrain.DRIVE_FRONT_LEFT);
34 frontRight = new CANTalon(Constants.DriveTrain.DRIVE_FRONT_RIGHT);
35 rearLeft = new CANTalon(Constants.DriveTrain.DRIVE_REAR_LEFT);
36 rearRight = new CANTalon(Constants.DriveTrain.DRIVE_REAR_RIGHT);
1884c3cf 37
33141cdd 38 robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
96215d97 39
d7bf2340
KZ
40 leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
41 Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
42 rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
43 Constants.DriveTrain.ENCODER_RIGHT_B, false, EncodingType.k4X);
45bdf5b9
KZ
44 leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
45 rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
33141cdd
KZ
46
47 leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
48 rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
49
33141cdd 50 this.disable();
d004deee 51
e1f7a742 52 leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_SHIFT_MODULE,
43338192
HD
53 Constants.DriveTrain.LEFT_SHIFT_FORWARD,
54 Constants.DriveTrain.LEFT_SHIFT_REVERSE);
55 rightGearPiston = new DoubleSolenoid(
56 Constants.DriveTrain.RIGHT_SHIFT_MODULE,
57 Constants.DriveTrain.RIGHT_SHIFT_FORWARD,
58 Constants.DriveTrain.RIGHT_SHIFT_REVERSE);
d7bf2340
KZ
59 }
60
61 @Override
62 protected void initDefaultCommand() {
33141cdd
KZ
63 setDefaultCommand(new JoystickDrive());
64 }
65
7a4df3c5 66 // Print tne PID Output
33141cdd
KZ
67 public void printOutput() {
68 System.out.println("PIDOutput: " + pidOutput);
69 }
70
71 private double getAvgEncoderDistance() {
72 return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
73 }
74
7a4df3c5
KZ
75 // Whether or not the PID Controller thinks we have reached the target
76 // setpoint
33141cdd
KZ
77 public boolean reachedTarget() {
78 if (this.onTarget()) {
79 this.disable();
80 return true;
81 } else {
82 return false;
83 }
84 }
85
86 public void stop() {
43338192 87 setMotorSpeeds(0, 0);
d7bf2340
KZ
88 }
89
90 public void resetEncoders() {
91 leftEncoder.reset();
92 rightEncoder.reset();
93 }
94
d7bf2340 95 public double getRightSpeed() {
6833a887 96 return rightEncoder.getRate(); // in inches per second
d7bf2340
KZ
97 }
98
99 public double getLeftSpeed() {
6833a887 100 return leftEncoder.getRate(); // in inches per second
d7bf2340
KZ
101 }
102
103 public double getSpeed() {
6833a887 104 return (getLeftSpeed() + getRightSpeed()) / 2.0; // in inches per second
d7bf2340
KZ
105 }
106
d7bf2340 107 public double getRightDistance() {
6833a887 108 return rightEncoder.getDistance(); // in inches
d7bf2340
KZ
109 }
110
d7bf2340 111 public double getLeftDistance() {
6833a887 112 return leftEncoder.getDistance(); // in inches
d7bf2340
KZ
113 }
114
7a4df3c5
KZ
115 // Get error between the setpoint of PID Controller and the current state of
116 // the robot
33141cdd 117 public double getError() {
f8bfcc62 118 return Math.abs(this.getSetpoint() - getAvgEncoderDistance());
33141cdd
KZ
119 }
120
121 public void printEncoder(int i, int n) {
122 if (i % n == 0) {
123 System.out.println("Left: " + this.getLeftDistance());
124 System.out.println("Right: " + this.getRightDistance());
125
126 }
127 }
128
7a4df3c5
KZ
129 /*
130 * returns the PID output that is returned by the PID Controller
131 */
33141cdd
KZ
132 public double getOutput() {
133 return pidOutput;
134 }
135
7a4df3c5 136 // Updates the PID constants based on which control mode is being used
33141cdd 137 public void updatePID() {
f8bfcc62
HD
138 this.getPIDController().setPID(Constants.DriveTrain.kp,
139 Constants.DriveTrain.ki, Constants.DriveTrain.kd);
33141cdd
KZ
140 }
141
142 public CANTalon getFrontLeft() {
143 return frontLeft;
144 }
145
146 public CANTalon getFrontRight() {
147 return frontRight;
148 }
149
150 public CANTalon getRearLeft() {
151 return rearLeft;
152 }
153
154 public CANTalon getRearRight() {
155 return rearRight;
156 }
157
7a4df3c5
KZ
158 /*
159 * Method is a required method that the PID Subsystem uses to return the
160 * calculated PID value to the driver
571a0e2a 161 *
7a4df3c5
KZ
162 * @param Gives the user the output from the PID algorithm that is calculated
163 * internally
571a0e2a 164 *
7a4df3c5
KZ
165 * Body: Uses the output, does some filtering and drives the robot
166 */
33141cdd
KZ
167 @Override
168 protected void usePIDOutput(double output) {
169 double left = 0;
170 double right = 0;
f8bfcc62
HD
171 double drift = this.getLeftDistance() - this.getRightDistance();
172 if (Math.abs(output) > 0 && Math.abs(output) < 0.3)
173 output = Math.signum(output) * 0.3;
174 left = output;
175 right = output + drift * Constants.DriveTrain.kp / 10;
43338192 176 setMotorSpeeds(left, right);
33141cdd 177 pidOutput = output;
d7bf2340
KZ
178 }
179
33141cdd
KZ
180 @Override
181 protected double returnPIDInput() {
182 return sensorFeedback();
d7bf2340 183 }
33141cdd 184
7a4df3c5 185 /*
c688e9da 186 * Checks the drive mode
571a0e2a 187 *
7e360ef5
LM
188 * @return the current state of the robot in each state Average distance from
189 * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE
7a4df3c5 190 */
33141cdd 191 private double sensorFeedback() {
f8bfcc62 192 return getAvgEncoderDistance();
33141cdd
KZ
193 }
194
43338192
HD
195 public void joystickDrive(double left, double right) {
196 int type = Constants.DriveTrain.DRIVE_TYPE;
197 double k = (isFlipped() ? -1 : 1);
198 if (type == Constants.DriveTrain.TANK_DRIVE) {
199 robotDrive.tankDrive(-left * k, -right * k);
200 } else if (type == Constants.DriveTrain.ARCADE_DRIVE) {
201 robotDrive.arcadeDrive(left * k, right);
202 }
33141cdd
KZ
203 }
204
205 public void setMotorSpeeds(double left, double right) {
43338192
HD
206 double k = (Robot.driveTrain.isFlipped() ? -1 : 1);
207 robotDrive.tankDrive(-left * k, -right * k);
33141cdd
KZ
208 }
209
571a0e2a
HD
210 /**
211 * @return a value that is the current setpoint for the piston (kReverse or
212 * kForward)
7a4df3c5 213 */
571a0e2a
HD
214 public Value getGearPistonValue() {
215 return leftGearPiston.get(); // Pistons should always be in the same state
2a099bc6
KZ
216 }
217
571a0e2a 218 /**
7a4df3c5
KZ
219 * Changes the ball shift gear assembly to high
220 */
2a099bc6
KZ
221 public void setHighGear() {
222 changeGear(Constants.DriveTrain.HIGH_GEAR);
223 }
224
571a0e2a 225 /**
7a4df3c5
KZ
226 * Changes the ball shift gear assembly to low
227 */
2a099bc6
KZ
228 public void setLowGear() {
229 changeGear(Constants.DriveTrain.LOW_GEAR);
230 }
231
571a0e2a
HD
232 /**
233 * Changes the gear to a DoubleSolenoid.Value
7a4df3c5 234 */
2a099bc6
KZ
235 public void changeGear(DoubleSolenoid.Value gear) {
236 leftGearPiston.set(gear);
237 rightGearPiston.set(gear);
238 }
600a1a1c 239
571a0e2a
HD
240 /**
241 * Switches drivetrain gears from high to low or low to high
242 */
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);
248 }
249
250 /**
251 * Toggle whether the motor outputs are flipped, effectively switching which
252 * side of the robot is the front.
253 */
254 public void toggleFlipped() {
255 outputFlipped = !outputFlipped;
256 }
257
258 public boolean isFlipped() {
259 return outputFlipped;
260 }
261
38a404b3 262}