Commit | Line | Data |
---|---|---|
9dc69158 CZ |
1 | package org.usfirst.frc.team3501.robot.commands.climber; |
2 | ||
3 | import org.usfirst.frc.team3501.robot.Robot; | |
4 | import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; | |
5 | ||
6 | import edu.wpi.first.wpilibj.command.Command; | |
7 | ||
8 | /** | |
9 | * | |
10 | */ | |
11 | public class ToggleWinch extends Command { | |
12 | DriveTrain driveTrain = Robot.getDriveTrain(); | |
13 | private double climbingSpeed; | |
14 | private double maintainPositionSpeed; | |
15 | ||
16 | public ToggleWinch() { | |
17 | requires(driveTrain); | |
18 | climbingSpeed = driveTrain.CLIMBER_SPEED; | |
19 | maintainPositionSpeed = driveTrain.MAINTAIN_CLIMBED_POSITION; | |
20 | } | |
21 | ||
22 | @Override | |
23 | protected void initialize() { | |
24 | } | |
25 | ||
26 | @Override | |
27 | protected void execute() { | |
28 | if (driveTrain.shouldBeClimbing) { | |
29 | driveTrain.setMotorValues(climbingSpeed, climbingSpeed); | |
30 | } else { | |
31 | driveTrain.setMotorValues(maintainPositionSpeed, maintainPositionSpeed); | |
32 | } | |
33 | } | |
34 | ||
35 | @Override | |
36 | protected boolean isFinished() { | |
47140f21 | 37 | return Robot.getOI().toggleWinch.get(); |
9dc69158 CZ |
38 | } |
39 | ||
40 | @Override | |
41 | protected void end() { | |
42 | driveTrain.shouldBeClimbing = !driveTrain.shouldBeClimbing; | |
43 | } | |
44 | ||
45 | @Override | |
46 | protected void interrupted() { | |
47 | end(); | |
48 | } | |
49 | } |