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 private double zeroAngle
;
33 public TurnForAngle(double angle
, Direction direction
, double maxTimeOut
) {
34 requires(Robot
.getDriveTrain());
35 this.direction
= direction
;
36 this.maxTimeOut
= maxTimeOut
;
37 this.target
= Math
.abs(angle
);
39 this.gyroP
= driveTrain
.turnP
;
40 this.gyroI
= driveTrain
.turnI
;
41 this.gyroD
= driveTrain
.turnD
;
43 this.gyroController
= Robot
.getDriveTrain().getGyroController();
44 this.gyroController
.setDoneRange(1);
45 this.gyroController
.setMinDoneCycles(5);
49 protected void initialize() {
50 this.driveTrain
.resetEncoders();
51 this.gyroController
.setSetPoint(this.target
);
52 this.zeroAngle
= driveTrain
.getAngle();
56 protected void execute() {
59 xVal
= this.gyroController
60 .calcPID(Math
.abs(this.driveTrain
.getAngle() - this.zeroAngle
));
63 double rightDrive
= 0;
64 if (direction
== Constants
.Direction
.RIGHT
) {
67 } else if (direction
== Constants
.Direction
.LEFT
) {
72 this.driveTrain
.setMotorValues(leftDrive
, rightDrive
);
76 protected boolean isFinished() {
77 return timeSinceInitialized() >= maxTimeOut
|| gyroController
.isDone();
81 protected void end() {
85 protected void interrupted() {