1 package org
.usfirst
.frc
.team3501
.robot
.commands
.driving
;
3 import org
.usfirst
.frc
.team3501
.robot
.Constants
;
4 import org
.usfirst
.frc
.team3501
.robot
.Constants
.Direction
;
5 import org
.usfirst
.frc
.team3501
.robot
.Robot
;
6 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.DriveTrain
;
7 import org
.usfirst
.frc
.team3501
.robot
.utils
.PIDController
;
9 import edu
.wpi
.first
.wpilibj
.command
.Command
;
12 * This command turns the robot for a specified angle in specified direction -
13 * either left or right
15 * parameters: angle: the robot will turn - in degrees direction: the robot will
16 * turn - either right or left maxTimeOut: the max time this command will be
17 * allowed to run on for
19 public class TurnForAngle
extends Command
{
20 private DriveTrain driveTrain
= Robot
.getDriveTrain();
23 private double maxTimeOut
;
24 private PIDController gyroController
;
26 private double target
;
31 public TurnForAngle(double angle
, Direction direction
, double maxTimeOut
) {
32 requires(Robot
.getDriveTrain());
33 this.direction
= direction
;
34 this.maxTimeOut
= maxTimeOut
;
37 this.gyroP
= driveTrain
.defaultGyroP
;
38 this.gyroI
= driveTrain
.defaultGyroI
;
39 this.gyroD
= driveTrain
.defaultGyroD
;
41 this.gyroController
= new PIDController(this.gyroP
, this.gyroI
, this.gyroD
);
42 this.gyroController
.setDoneRange(1);
43 this.gyroController
.setMinDoneCycles(5);
47 protected void initialize() {
48 this.driveTrain
.resetEncoders();
49 this.driveTrain
.resetGyro();
50 this.gyroController
.setSetPoint(this.target
);
54 protected void execute() {
57 xVal
= this.gyroController
58 .calcPID(this.driveTrain
.getAngle() - this.driveTrain
.getZeroAngle());
62 if (direction
== Constants
.Direction
.RIGHT
) {
70 this.driveTrain
.setMotorValues(leftDrive
, rightDrive
);
74 protected boolean isFinished() {
75 return timeSinceInitialized() >= maxTimeOut
|| gyroController
.isDone();
79 protected void end() {
83 protected void interrupted() {