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
;
28 private double zeroAngle
;
30 public TurnForAngle(double angle
, Direction direction
, double maxTimeOut
) {
31 requires(Robot
.getDriveTrain());
32 this.direction
= direction
;
33 this.maxTimeOut
= maxTimeOut
;
34 this.target
= Math
.abs(angle
);
37 this.zeroAngle
= driveTrain
.getAngle();
38 >>>>>>> fix alot of unnecessary
/not used code and init gyroController
40 this.gyroController
= Robot
.getDriveTrain().getGyroController();
41 this.gyroController
.setDoneRange(1);
42 this.gyroController
.setMinDoneCycles(5);
46 protected void initialize() {
47 this.driveTrain
.resetEncoders();
48 this.gyroController
.setSetPoint(this.target
);
49 this.zeroAngle
= driveTrain
.getAngle();
53 protected void execute() {
56 xVal
= this.gyroController
57 .calcPID(Math
.abs(this.driveTrain
.getAngle() - this.zeroAngle
));
60 double rightDrive
= 0;
61 if (direction
== Constants
.Direction
.RIGHT
) {
64 } else if (direction
== Constants
.Direction
.LEFT
) {
69 this.driveTrain
.setMotorValues(leftDrive
, rightDrive
);
73 protected boolean isFinished() {
74 return timeSinceInitialized() >= maxTimeOut
|| gyroController
.isDone();
78 protected void end() {
82 protected void interrupted() {