b7264f1c8bba799dfe8aa9f717e28564e28175ca
[3501/2017steamworks] / src / org / usfirst / frc / team3501 / robot / subsystems / DriveTrain.java
1 package org.usfirst.frc.team3501.robot.subsystems;
2
3 import org.usfirst.frc.team3501.robot.Constants;
4 import org.usfirst.frc.team3501.robot.MathLib;
5 import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
6
7 import com.ctre.CANTalon;
8
9 import edu.wpi.first.wpilibj.ADXRS450_Gyro;
10 import edu.wpi.first.wpilibj.DoubleSolenoid;
11 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
12 import edu.wpi.first.wpilibj.Encoder;
13 import edu.wpi.first.wpilibj.RobotDrive;
14 import edu.wpi.first.wpilibj.command.Subsystem;
15
16 public class DriveTrain extends Subsystem {
17 public static double driveP = 0.01, driveI = 0.00115, driveD = -0.002;
18 public static double smallTurnP = 0.004, smallTurnI = 0.0013,
19 smallTurnD = 0.005;
20 public static double largeTurnP = .003, largeTurnI = .0012, largeTurnD = .006;
21 public static double driveStraightGyroP = 0.01;
22
23 public static final double WHEEL_DIAMETER = 4; // inches
24 public static final double ENCODER_PULSES_PER_REVOLUTION = 256;
25 public static final double INCHES_PER_PULSE = WHEEL_DIAMETER * Math.PI
26 / ENCODER_PULSES_PER_REVOLUTION;
27
28 public static final double MAINTAIN_CLIMBED_POSITION = 0;
29 public static final double TIME_TO_CLIMB_FOR = 0;
30 public static final double CLIMBER_SPEED = 0;
31
32 public static final boolean DRIVE_BRAKE_MODE = true;
33 public static final boolean DRIVE_COAST_MODE = false;
34
35 private static DriveTrain driveTrain;
36
37 private final CANTalon frontLeft, frontRight, rearLeft, rearRight;
38 private final RobotDrive robotDrive;
39 private final Encoder leftEncoder, rightEncoder;
40 private final DoubleSolenoid leftDriveTrainPiston, rightDriveTrainPiston;
41 private final DoubleSolenoid gearManipulatorPiston;
42
43 private ADXRS450_Gyro imu;
44
45 public boolean shouldBeClimbing = false;
46
47 private DriveTrain() {
48 // MOTOR CONTROLLERS
49 frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
50 frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
51 rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
52 rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
53
54 // ENCODERS
55 leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
56 Constants.DriveTrain.ENCODER_LEFT_B, false, Encoder.EncodingType.k4X);
57 rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
58 Constants.DriveTrain.ENCODER_RIGHT_B, false, Encoder.EncodingType.k4X);
59
60 leftEncoder.setDistancePerPulse(INCHES_PER_PULSE);
61 rightEncoder.setDistancePerPulse(INCHES_PER_PULSE);
62
63 // ROBOT DRIVE
64 robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
65
66 this.imu = new ADXRS450_Gyro(Constants.DriveTrain.GYRO_PORT);
67
68 // TODO: Not sure if MODULE_NUMBER should be the same for both
69 leftDriveTrainPiston = new DoubleSolenoid(
70 Constants.DriveTrain.PISTON_MODULE,
71 Constants.DriveTrain.LEFT_GEAR_PISTON_FORWARD,
72 Constants.DriveTrain.LEFT_GEAR_PISTON_REVERSE);
73 rightDriveTrainPiston = new DoubleSolenoid(
74 Constants.DriveTrain.PISTON_MODULE,
75 Constants.DriveTrain.RIGHT_GEAR_PISTON_FORWARD,
76 Constants.DriveTrain.RIGHT_GEAR_PISTON_REVERSE);
77
78 gearManipulatorPiston = new DoubleSolenoid(
79 Constants.DriveTrain.PISTON_MODULE,
80 Constants.DriveTrain.GEAR_MANIPULATOR_PISTON_FORWARD,
81 Constants.DriveTrain.GEAR_MANIPULATOR_PISTON_REVERSE);
82 }
83
84 public static DriveTrain getDriveTrain() {
85 if (driveTrain == null) {
86 driveTrain = new DriveTrain();
87 }
88 return driveTrain;
89 }
90
91 // DRIVE METHODS
92 public void setMotorValues(double left, double right) {
93 left = MathLib.restrictToRange(left, -1.0, 1.0);
94 right = MathLib.restrictToRange(right, -1.0, 1.0);
95
96 frontLeft.set(left);
97 rearLeft.set(left);
98
99 frontRight.set(-right);
100 rearRight.set(-right);
101 }
102
103 public void joystickDrive(final double thrust, final double twist) {
104 if ((thrust < 0.1 && thrust > -0.1) && (twist < 0.1 && twist > -0.1))
105 robotDrive.arcadeDrive(0, 0, true);
106 else
107 robotDrive.arcadeDrive(thrust, twist, true);
108 }
109
110 public void stop() {
111 setMotorValues(0, 0);
112 }
113
114 public double getLeftMotorVal() {
115 return (frontLeft.get() + rearLeft.get()) / 2;
116 }
117
118 public double getRightMotorVal() {
119 return (frontRight.get() + rearRight.get()) / 2;
120 }
121
122 // ENCODER METHODS
123
124 public double getLeftEncoderDistance() {
125 return leftEncoder.getDistance();
126 }
127
128 public double getRightEncoderDistance() {
129 return rightEncoder.getDistance();
130 }
131
132 public void printEncoderOutput() {
133 System.out.println("left: " + getLeftEncoderDistance());
134 System.out.println("right: " + getRightEncoderDistance());
135 // System.out.println(getAvgEncoderDistance());
136 }
137
138 public double getAvgEncoderDistance() {
139 return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
140 }
141
142 public void resetEncoders() {
143 leftEncoder.reset();
144 rightEncoder.reset();
145 }
146
147 public double getLeftSpeed() {
148 return leftEncoder.getRate();
149 }
150
151 public double getRightSpeed() {
152 return rightEncoder.getRate();
153 }
154
155 public double getSpeed() {
156 return (getLeftSpeed() + getRightSpeed()) / 2.0;
157 }
158
159 // ------Gyro------//
160 public double getAngle() {
161 return this.imu.getAngle();
162 }
163
164 public void resetGyro() {
165 this.imu.reset();
166 }
167
168 /*
169 * @return a value that is the current setpoint for the piston kReverse or
170 * KForward
171 */
172 public Value getLeftDriveTrainPiston() {
173 return leftDriveTrainPiston.get();
174 }
175
176 /*
177 * @return a value that is the current setpoint for the piston kReverse or
178 * KForward
179 */
180 public Value getRightDriveTrainPiston() {
181 return rightDriveTrainPiston.get();
182 }
183
184 /*
185 * Changes the ball shift gear assembly to high
186 */
187 public void setHighGear() {
188 changeGear(Constants.DriveTrain.FORWARD_PISTON_VALUE);
189 }
190
191 /*
192 * Changes the ball shift gear assembly to low
193 */
194 public void setLowGear() {
195 changeGear(Constants.DriveTrain.REVERSE_PISTON_VALUE);
196 }
197
198 /*
199 * Changes the gear to a DoubleSolenoid.Value
200 */
201 private void changeGear(DoubleSolenoid.Value gear) {
202 leftDriveTrainPiston.set(gear);
203 rightDriveTrainPiston.set(gear);
204 }
205
206 public Value getGearManipulatorPistonValue() {
207 return gearManipulatorPiston.get();
208 }
209
210 public void extendGearManipulatorPiston() {
211 gearManipulatorPiston.set(Constants.DriveTrain.FORWARD_PISTON_VALUE);
212 }
213
214 public void retractGearManipulatorPiston() {
215 gearManipulatorPiston.set(Constants.DriveTrain.REVERSE_PISTON_VALUE);
216 }
217
218 @Override
219 protected void initDefaultCommand() {
220 setDefaultCommand(new JoystickDrive());
221 }
222
223 public void setCANTalonsBrakeMode(boolean mode) {
224 frontLeft.enableBrakeMode(mode);
225 rearLeft.enableBrakeMode(mode);
226
227 frontRight.enableBrakeMode(mode);
228 rearRight.enableBrakeMode(mode);
229 }
230 }