Commit | Line | Data |
---|---|---|
ba9f0b12 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.Climber; | |
5 | ||
6 | import edu.wpi.first.wpilibj.command.Command; | |
7 | ||
8 | public class ToggleWinch extends Command { | |
9 | Climber climber = Robot.getClimber(); | |
10 | private double climbingSpeed; | |
11 | ||
12 | public ToggleWinch() { | |
13 | requires(climber); | |
14 | climbingSpeed = climber.CLIMBER_SPEED; | |
15 | } | |
16 | ||
17 | @Override | |
18 | protected void initialize() { | |
19 | } | |
20 | ||
21 | @Override | |
22 | protected void execute() { | |
23 | if (climber.shouldBeClimbing) { | |
24 | climber.setCANTalonsBrakeMode(climber.COAST_MODE); | |
25 | climber.setMotorValues(climbingSpeed); | |
26 | } else { | |
27 | climber.setCANTalonsBrakeMode(climber.BRAKE_MODE); | |
8275a069 RR |
28 | |
29 | /* Not sure if should have */ | |
30 | climber.stop(); | |
ba9f0b12 CZ |
31 | end(); |
32 | } | |
33 | } | |
34 | ||
35 | @Override | |
36 | protected boolean isFinished() { | |
37 | return false; | |
38 | } | |
39 | ||
40 | @Override | |
41 | protected void end() { | |
42 | climber.shouldBeClimbing = !climber.shouldBeClimbing; | |
43 | } | |
44 | ||
45 | @Override | |
46 | protected void interrupted() { | |
47 | end(); | |
48 | } | |
49 | } |