package org.usfirst.frc.team3501.robot.commands.driving;
+import org.usfirst.frc.team3501.robot.Constants;
+import org.usfirst.frc.team3501.robot.Robot;
+
import edu.wpi.first.wpilibj.command.Command;
+/***
+ * This command will move the robot at the specified speed for the specified
+ * distance.
+ *
+ * post-condition: has driven for the distance and speed specified
+ *
+ * @author Meryem and Avi
+ *
+ */
public class TurnForAngle extends Command {
+ private double maxTimeOut;
+ double angle;
+ int count = 0;
+
+ public TurnForAngle(double angle, double maxTimeOut) {
+ requires(Robot.driveTrain);
+ this.maxTimeOut = maxTimeOut;
+ this.angle = angle;
+ }
- public TurnForAngle(double degrees) {
- }
+ @Override
+ protected void initialize() {
+ Robot.driveTrain.resetGyro();
+ System.out.println(Robot.driveTrain.getMode());
+ }
- @Override
- protected void initialize() {
- }
+ @Override
+ protected void execute() {
+ Robot.driveTrain.turnAngle(angle);
+ Robot.driveTrain.printGyroOutput();
+ Robot.driveTrain.printOutput();
+ count++;
- @Override
- protected void execute() {
- }
+ }
- @Override
- protected boolean isFinished() {
- return false;
- }
+ @Override
+ protected boolean isFinished() {
+ if (timeSinceInitialized() >= maxTimeOut
+ || Robot.driveTrain
+ .reachedTarget() || Robot.driveTrain.getError() < 8) {
+ System.out.println("time: " + timeSinceInitialized());
+ Constants.DriveTrain.time = timeSinceInitialized();
+ return true;
+ }
+ return false;
+ }
- @Override
- protected void end() {
- }
+ @Override
+ protected void end() {
+ Robot.driveTrain.disable();
+ }
- @Override
- protected void interrupted() {
- }
+ @Override
+ protected void interrupted() {
+ end();
+ }
}