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 <<<<<<< 3921ddf563ce7727724494c62d65c2ca0e8ff8cc
41 this.gyroController
= Robot
.getDriveTrain().getGyroController();
43 this.gyroP
= driveTrain
.turnP
;
44 this.gyroI
= driveTrain
.turnI
;
45 this.gyroD
= driveTrain
.turnD
;
47 this.gyroController
= Robot
.getDriveTrain().getDriveController();
49 this.gyroController
.setDoneRange(1);
50 this.gyroController
.setMinDoneCycles(5);
54 protected void initialize() {
55 this.driveTrain
.resetEncoders();
56 this.gyroController
.setSetPoint(this.target
);
57 this.zeroAngle
= driveTrain
.getAngle();
61 protected void execute() {
64 xVal
= this.gyroController
65 .calcPID(Math
.abs(this.driveTrain
.getAngle() - this.zeroAngle
));
68 double rightDrive
= 0;
69 if (direction
== Constants
.Direction
.RIGHT
) {
72 } else if (direction
== Constants
.Direction
.LEFT
) {
77 this.driveTrain
.setMotorValues(leftDrive
, rightDrive
);
81 protected boolean isFinished() {
82 return timeSinceInitialized() >= maxTimeOut
|| gyroController
.isDone();
86 protected void end() {
90 protected void interrupted() {