1 package org
.usfirst
.frc
.team3501
.robot
.commands
.driving
;
3 import org
.usfirst
.frc
.team3501
.robot
.MathLib
;
4 import org
.usfirst
.frc
.team3501
.robot
.Robot
;
5 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.DriveTrain
;
6 import org
.usfirst
.frc
.team3501
.robot
.utils
.PIDController
;
8 import edu
.wpi
.first
.wpilibj
.Preferences
;
9 import edu
.wpi
.first
.wpilibj
.command
.Command
;
12 * This command makes the robot drive a specified distance using encoders on the
13 * robot and using a feedback loop
15 * parameters: distance the robot will move in inches motorVal: the motor input
16 * to set the motors to
18 public class DriveDistance
extends Command
{
19 private DriveTrain driveTrain
= Robot
.getDriveTrain();
20 private double maxTimeOut
;
21 private PIDController driveController
;
22 private PIDController gyroController
;
23 private Preferences prefs
;
25 private double target
;
30 private double driveP
;
31 private double driveI
;
32 private double driveD
;
34 public DriveDistance(double distance
, double maxTimeOut
) {
36 this.maxTimeOut
= maxTimeOut
;
37 this.target
= distance
;
39 private double target
;
44 private double driveP
;
45 private double driveI
;
46 private double driveD
;
48 public DriveDistance(double distance
, double motorVal
) {
50 this.driveP
= driveTrain
.driveP
;
51 this.driveI
= driveTrain
.driveI
;
52 this.driveD
= driveTrain
.driveD
;
53 this.gyroP
= driveTrain
.defaultGyroP
;
54 this.gyroI
= driveTrain
.defaultGyroI
;
55 this.gyroD
= driveTrain
.defaultGyroD
;
56 this.driveController
= new PIDController(this.driveP
, this.driveI
,
58 this.driveController
.setDoneRange(0.5);
59 this.driveController
.setMaxOutput(1.0);
60 this.driveController
.setMinDoneCycles(5);
62 this.gyroController
= new PIDController(this.gyroP
, this.gyroI
, this.gyroD
);
63 this.gyroController
.setDoneRange(1);
64 this.gyroController
.setMinDoneCycles(5);
68 protected void initialize() {
69 this.driveTrain
.resetEncoders();
70 this.driveTrain
.resetGyro();
71 this.driveController
.setSetPoint(this.target
);
72 this.gyroController
.setSetPoint(this.driveTrain
.getZeroAngle());
76 protected void execute() {
78 double yVal
= this.driveController
79 .calcPID(this.driveTrain
.getAvgEncoderDistance());
81 if (this.driveTrain
.getAngle() - this.driveTrain
.getZeroAngle() < 30) {
82 xVal
= -this.gyroController
83 .calcPID(this.driveTrain
.getAngle() - this.driveTrain
.getZeroAngle());
85 double leftDrive
= yVal
- xVal
;
86 double rightDrive
= yVal
+ xVal
;
88 this.driveTrain
.setMotorValues(leftDrive
, rightDrive
);
90 driveTrain
.printEncoderOutput();
91 // System.out.println("turn: " + xVal);
92 double leftDrive
= MathLib
.calcLeftTankDrive(-xVal
, yVal
);
93 double rightDrive
= MathLib
.calcRightTankDrive(xVal
, -yVal
);
95 this.driveTrain
.setMotorValues(leftDrive
, rightDrive
);
97 System
.out
.println(driveTrain
.getAvgEncoderDistance());
98 // System.out.println("motorval: " + yVal);
102 protected boolean isFinished() {
103 boolean isDone
= this.driveController
.isDone();
104 if (timeSinceInitialized() >= maxTimeOut
|| isDone
)
105 System
.out
.println("time: " + timeSinceInitialized());
106 return timeSinceInitialized() >= maxTimeOut
|| isDone
;
110 protected void end() {
115 protected void interrupted() {