@@ -49,11 +49,13 @@ public class Drivetrain extends Subsystem implements PIDOutput, PIDSource {
4949 private final AnalogGyro dtGyro = RobotMap .dtGyro ;
5050 private final DoubleSolenoid dtGear = RobotMap .dtGear ;
5151
52+ private double pidOut = 0 ;
53+
5254 /**
5355 * Activates the solenoid to push the drivetrain into low or high gear
5456 *
5557 * @param forw
56- * if the solenoid is to be pushed forward or not
58+ * If the solenoid is to be pushed forward or not (backwards)
5759 */
5860 public void changeShiftGear (boolean forw ) {
5961 if (forw ^ Robot .getBool ("Drivetrain Gear Shift Backwards" , false )) {
@@ -63,42 +65,73 @@ public void changeShiftGear(boolean forw) {
6365 }
6466 }
6567
68+ /**
69+ * Stops the solenoid that pushes the drivetrain into low or high gear
70+ */
6671 public void stopGear () {
6772 dtGear .set (DoubleSolenoid .Value .kOff );
6873 }
6974
70- private double pidOut = 0 ;
71-
75+ /**
76+ * Resets the AHRS value
77+ */
7278 public void resetAHRS () {
7379 ahrs .reset ();
7480 }
7581
82+ /**
83+ * Runs the left side of the drivetrain at the specified speed
84+ *
85+ * @param value
86+ * Value for the motor(s) to run at
87+ */
7688 public void setLeftMotor (double value ) {
7789 dtLeft .set (value );
7890 }
7991
92+ /**
93+ * Tells the left side of the drivetrain to stop running
94+ */
8095 private void stopLeftMotor () {
8196 dtLeft .stopMotor ();
8297 }
8398
99+ /**
100+ * Runs the right side of the drivetrain at the specified speed
101+ *
102+ * @param value
103+ * Value for the motor(s) to run at
104+ */
84105 public void setRightMotor (double value ) {
85106 dtRight .set (value );
86107 }
87108
109+ /**
110+ * Tells the right side of the drivetrain to stop running
111+ */
88112 private void stopRightMotor () {
89113 dtRight .stopMotor ();
90114 }
91115
116+ /**
117+ * Tells the drivetrain to stop running
118+ */
92119 public void stopDrive () {
93120 stopRightMotor ();
94121 stopLeftMotor ();
95122 }
96123
124+ /**
125+ * Resets the encoders' distances to zero
126+ */
97127 public void resetEnc () {
98128 leftEnc .reset ();
99129 rightEnc .reset ();
100130 }
101131
132+ /**
133+ * Drives based on joystick input and SmartDashboard values
134+ */
102135 public void teleopDrive () {
103136 if (Robot .getBool ("Arcade Drive" , true )) {
104137 if (Robot .getBool ("Arcade Drive Default Setup" , true )) {
@@ -111,26 +144,62 @@ public void teleopDrive() {
111144 }
112145 }
113146
147+ /**
148+ * Drives the robot based on parameters and SmartDashboard values
149+ *
150+ * @param speed
151+ * The amount to move forward
152+ * @param turn
153+ * The amount to turn
154+ */
114155 public void arcadeDrive (double speed , double turn ) {
115156 robotDrive .arcadeDrive (speed , turn , Robot .getBool ("Square Drive Values" , false ));
116157 }
117158
159+ /**
160+ * Drive the robot based on parameters and SmartDashboard values
161+ *
162+ * @param leftSpeed
163+ * The value to run the left of the drivetrain at
164+ * @param rightSpeed
165+ * The value to run the right of the drivetrain at
166+ */
118167 public void tankDrive (double leftSpeed , double rightSpeed ) {
119168 robotDrive .tankDrive (leftSpeed , rightSpeed , Robot .getBool ("Square Drive Values" , false ));
120169 }
121170
171+ /**
172+ * Used for getting the speed at which the left side of the drivetrain is
173+ * currently running
174+ *
175+ * @return The speed that the left side of the drivetrain is set to
176+ */
122177 public double getDtLeft () {
123178 return dtLeft .get ();
124179 }
125180
181+ /**
182+ * Used for getting the speed at which the right side of the drivetrain is
183+ * currently running
184+ *
185+ * @return The speed that the right side of the drivetrain is set to
186+ */
126187 public double getDtRight () {
127188 return dtRight .get ();
128189 }
129190
191+ /**
192+ * Used to get the angle that the gyro currently reads
193+ *
194+ * @return The angle that the gyro reads
195+ */
130196 public double getGyro () {
131197 return dtGyro .getAngle ();
132198 }
133199
200+ /**
201+ * Resets the gyro to 0
202+ */
134203 public void resetGyro () {
135204 dtGyro .reset ();
136205 }
@@ -139,46 +208,79 @@ public void initDefaultCommand() {
139208 setDefaultCommand (new TeleopDrive ());
140209 }
141210
211+ /**
212+ * Disables the turnPID PIDController used for turning
213+ */
142214 public void disableTurnPid () {
143215 turnController .disable ();
144216 }
145217
218+ /**
219+ * Enables the turnPID PIDController used for turning
220+ */
146221 public void enableTurnPid () {
147222 turnController .enable ();
148223 }
149224
225+ /**
226+ * Sets the setPoint of the turnPID PIDController
227+ *
228+ * @param set
229+ * The value to set the setPoint at
230+ */
150231 public void setSetTurn (double set ) {
151232 turnController .setSetpoint (set );
152233 }
153234
154- // public void disableMoveLeftPid() {
155- // moveLeftController.disable();
156- // }
157- // public void enableMoveLeftPid() {
158- // moveLeftController.enable();
159- // }
160- // public void setSetMoveLeft(double set) {
161- // moveLeftController.setSetpoint(set);
162- // }
163- //
164- // public void disableMoveRightPid() {
165- // moveRightController.disable();
166- // }
167- // public void enableMoveRightPid() {
168- // moveRightController.enable();
169- // }
170- // public void setSetMoveRight(double set) {
171- // moveRightController.setSetpoint(set);
172- // }
235+ /**
236+ * Enable the movePID PIDController used for moving
237+ */
238+ public void enableMovePid () {
239+ moveController .enable ();
240+ }
241+
242+ /**
243+ * Disables the movePID PIDController used for moving
244+ */
245+ public void disableMovePid () {
246+ moveController .disable ();
247+ }
248+
249+ /**
250+ * Sets the setPoint of the movePID PIDController
251+ *
252+ * @param set
253+ * The value to set the setPoint at
254+ */
255+ public void setSetMove (double set ) {
256+ moveController .setSetpoint (set );
257+ }
173258
259+ /**
260+ * Sets the distancePerPulse property on the left encoder
261+ *
262+ * @param dist
263+ * The distance to set the distancePerPulse at
264+ */
174265 public void setDistancePerPulseLeft (double dist ) {
175266 leftEnc .setDistancePerPulse (dist );
176267 }
177268
269+ /**
270+ * Sets the distancePerPulse property on the right encoder
271+ *
272+ * @param dist
273+ * The distance to set the distancePerPulse at
274+ */
178275 public void setDistancePerPulseRight (double dist ) {
179276 rightEnc .setDistancePerPulse (dist );
180277 }
181278
279+ /**
280+ * Returns the value that Drivetrain receives due to implementing PIDOutput
281+ *
282+ * @return The value that is written by PIDControllers
283+ */
182284 public double getPidOut () {
183285 return pidOut ;
184286 }
@@ -188,45 +290,45 @@ public double pidGet() {
188290 return average (leftEnc .getDistance (), rightEnc .getDistance ());
189291 }
190292
191- public PIDSource getLeftDrive () {
192- return (PIDSource ) dtLeftDrive ;
193- }
194-
195- public PIDSource getRightDrive () {
196- return (PIDSource ) dtRightDrive ;
197- }
198-
199293 // public boolean onTargetLeft() {
200294 // return moveLeftController.onTarget();
201295 // }
202296 // public boolean onTargetRight() {
203297 // return moveRightController.onTarget();
204298 // }
205- public void enableMovePid () {
206- moveController .enable ();
207- }
208-
209- public void disableMovePid () {
210- moveController .disable ();
211- }
212-
213- public void setSetMove (double set ) {
214- moveController .setSetpoint (set );
215- }
216299
217300 @ Override
218301 public void pidWrite (double output ) {
219302 pidOut = output ;
220303 }
221304
305+ /**
306+ * Returns the average of two numbers
307+ *
308+ * @param a
309+ * First number to average
310+ * @param b
311+ * Second number to average
312+ * @return The arithmetic mean of a and b
313+ */
222314 public static double average (double a , double b ) {
223315 return (a + b ) / 2 ;
224316 }
225317
318+ /**
319+ * Returns whether the turnController PIDController senses that it's on target
320+ *
321+ * @return Whether the turnController PIDController is on target
322+ */
226323 public boolean onTurnTarg () {
227324 return turnController .onTarget ();
228325 }
229326
327+ /**
328+ * Returns whether the moveController PIDController senses that it's on target
329+ *
330+ * @return Whether the moveController PIDController is on target
331+ */
230332 public boolean onDriveTarg () {
231333 return moveController .onTarget ();
232334 }
0 commit comments