f9e5e890be195df4a417cbb05e89580aadca3ead
1 package org
.usfirst
.frc
.team3501
.robot
.commands
.driving
;
3 import org
.usfirst
.frc
.team3501
.robot
.Constants
;
4 import org
.usfirst
.frc
.team3501
.robot
.Robot
;
6 import edu
.wpi
.first
.wpilibj
.command
.Command
;
9 * This command will move the robot at the specified speed for the specified
12 * post-condition: has driven for the distance and speed specified
14 * @author Meryem and Avi
17 public class TurnForAngle
extends Command
{
18 private double maxTimeOut
;
22 public TurnForAngle(double angle
, double maxTimeOut
) {
23 requires(Robot
.driveTrain
);
24 this.maxTimeOut
= maxTimeOut
;
29 protected void initialize() {
30 Robot
.driveTrain
.resetGyro();
31 System
.out
.println(Robot
.driveTrain
.getMode());
35 protected void execute() {
36 Robot
.driveTrain
.turnAngle(angle
);
37 Robot
.driveTrain
.printGyroOutput();
38 Robot
.driveTrain
.printOutput();
44 protected boolean isFinished() {
45 if (timeSinceInitialized() >= maxTimeOut
47 .reachedTarget() || Robot
.driveTrain
.getError() < 8) {
48 System
.out
.println("time: " + timeSinceInitialized());
49 Constants
.DriveTrain
.time
= timeSinceInitialized();
56 protected void end() {
57 Robot
.driveTrain
.disable();
61 protected void interrupted() {