1 package org
.usfirst
.frc
.team3501
.robot
.commands
.driving
;
3 import org
.usfirst
.frc
.team3501
.robot
.Robot
;
4 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.DriveTrain
;
5 import org
.usfirst
.frc
.team3501
.robot
.utils
.PIDController
;
7 import edu
.wpi
.first
.wpilibj
.Preferences
;
8 import edu
.wpi
.first
.wpilibj
.command
.Command
;
11 * This command makes the robot drive a specified distance using encoders on the
12 * robot and using a feedback loop
14 * parameters: distance the robot will move in inches motorVal: the motor input
15 * to set the motors to
17 public class DriveDistance
extends Command
{
18 private DriveTrain driveTrain
= Robot
.getDriveTrain();
19 private double maxTimeOut
;
20 private double target
;
21 private double zeroAngle
;
22 private Preferences prefs
;
23 private PIDController driveController
;
25 private double driveP
;
26 private double driveI
;
27 private double driveD
;
30 public DriveDistance(double distance
, double maxTimeOut
) {
32 this.maxTimeOut
= maxTimeOut
;
33 this.target
= distance
;
35 this.driveP
= driveTrain
.driveP
;
36 this.driveI
= driveTrain
.driveI
;
37 this.driveD
= driveTrain
.driveD
;
38 this.gyroP
= driveTrain
.driveStraightGyroP
;
39 this.driveController
= new PIDController(driveP
, driveI
, driveD
);
40 this.driveController
.setDoneRange(1.0);
41 this.driveController
.setMaxOutput(1.0);
42 this.driveController
.setMinDoneCycles(5);
46 protected void initialize() {
47 this.driveTrain
.resetEncoders();
48 this.driveController
.setSetPoint(this.target
);
49 this.zeroAngle
= driveTrain
.getAngle();
53 protected void execute() {
54 double xVal
= gyroP
* (driveTrain
.getAngle() - zeroAngle
);
55 double yVal
= driveController
.calcPID(driveTrain
.getAvgEncoderDistance());
57 double leftDrive
= yVal
- xVal
;
58 double rightDrive
= yVal
+ xVal
;
59 this.driveTrain
.setMotorValues(leftDrive
, rightDrive
);
63 protected boolean isFinished() {
64 return timeSinceInitialized() >= maxTimeOut
65 || this.driveController
.isDone();
69 protected void end() {
73 protected void interrupted() {