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
);
40 this.gyroP
= driveTrain
.largeTurnP
;
41 this.gyroI
= driveTrain
.largeTurnI
;
42 this.gyroD
= driveTrain
.largeTurnD
;
44 this.gyroP
= driveTrain
.smallTurnP
;
45 this.gyroI
= driveTrain
.smallTurnI
;
46 this.gyroD
= driveTrain
.smallTurnD
;
49 this.gyroController
= new PIDController(this.gyroP
, this.gyroI
, this.gyroD
);
50 this.gyroController
.setDoneRange(1);
51 this.gyroController
.setMinDoneCycles(5);
55 protected void initialize() {
56 this.driveTrain
.resetEncoders();
57 this.gyroController
.setSetPoint(this.target
);
58 this.zeroAngle
= driveTrain
.getAngle();
62 protected void execute() {
65 xVal
= this.gyroController
66 .calcPID(Math
.abs(this.driveTrain
.getAngle() - this.zeroAngle
));
69 double rightDrive
= 0;
70 if (direction
== Constants
.Direction
.RIGHT
) {
73 } else if (direction
== Constants
.Direction
.LEFT
) {
78 this.driveTrain
.setMotorValues(leftDrive
, rightDrive
);
82 protected boolean isFinished() {
83 return timeSinceInitialized() >= maxTimeOut
|| gyroController
.isDone();
87 protected void end() {
91 protected void interrupted() {