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
;
9 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SmartDashboard
;
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 double target
;
22 private double zeroAngle
;
23 private Preferences prefs
;
24 private PIDController driveController
;
26 private double driveStraightGyroP
;
28 public DriveDistance(double distance
, double maxTimeOut
) {
30 this.target
= distance
;
31 this.maxTimeOut
= maxTimeOut
;
32 driveStraightGyroP
= SmartDashboard
.getNumber(driveTrain
.DRIVE_GYRO_P_Val
,
33 driveTrain
.PID_ERROR
);
35 this.driveController
= driveTrain
.getDriveController();
36 this.driveController
.setDoneRange(0.5);
37 this.driveController
.setMaxOutput(1.0);
38 this.driveController
.setMinDoneCycles(5);
42 protected void initialize() {
43 this.driveTrain
.resetEncoders();
44 this.driveController
.setSetPoint(this.target
);
45 zeroAngle
= driveTrain
.getAngle();
49 protected void execute() {
50 System
.out
.printf("angle: %.2f\t", driveTrain
.getAngle() - zeroAngle
);
51 double xVal
= driveStraightGyroP
* (driveTrain
.getAngle() - zeroAngle
);
52 double yVal
= driveController
.calcPID(driveTrain
.getAvgEncoderDistance());
54 double leftDrive
= yVal
- xVal
;
55 double rightDrive
= yVal
+ xVal
;
56 this.driveTrain
.setMotorValues(leftDrive
, rightDrive
);
58 // SmartDashboard.putNumber("x", xVal);
59 // SmartDashboard.putNumber("y", yVal);
61 // SmartDashboard.putNumber("left", driveTrain.getLeftEncoderDistance());
62 // SmartDashboard.putNumber("right", driveTrain.getRightEncoderDistance());
64 // System.out.printf("x: %.2f\t", xVal);
65 // System.out.printf("y: %.2f\t", yVal);
67 // System.out.printf("dist: %.2f\n", driveTrain.getAvgEncoderDistance());
72 protected boolean isFinished() {
73 return timeSinceInitialized() >= maxTimeOut
74 || this.driveController
.isDone();
78 protected void end() {
82 protected void interrupted() {