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() { | |
f74d236d | 19 | System.out.println("toggled"); |
ba9f0b12 CZ |
20 | } |
21 | ||
22 | @Override | |
23 | protected void execute() { | |
24 | if (climber.shouldBeClimbing) { | |
25 | climber.setCANTalonsBrakeMode(climber.COAST_MODE); | |
26 | climber.setMotorValues(climbingSpeed); | |
f74d236d | 27 | System.out.println("climbing"); |
ba9f0b12 CZ |
28 | } else { |
29 | climber.setCANTalonsBrakeMode(climber.BRAKE_MODE); | |
8275a069 | 30 | |
f74d236d | 31 | System.out.println("braked"); |
8275a069 RR |
32 | /* Not sure if should have */ |
33 | climber.stop(); | |
ba9f0b12 CZ |
34 | end(); |
35 | } | |
36 | } | |
37 | ||
38 | @Override | |
39 | protected boolean isFinished() { | |
40 | return false; | |
41 | } | |
42 | ||
43 | @Override | |
44 | protected void end() { | |
45 | climber.shouldBeClimbing = !climber.shouldBeClimbing; | |
f74d236d | 46 | System.out.println("ended"); |
ba9f0b12 CZ |
47 | } |
48 | ||
49 | @Override | |
50 | protected void interrupted() { | |
51 | end(); | |
52 | } | |
53 | } |