1 package org
.usfirst
.frc
.team3501
.robot
.commands
.climber
;
3 import org
.usfirst
.frc
.team3501
.robot
.Robot
;
4 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.DriveTrain
;
6 import edu
.wpi
.first
.wpilibj
.command
.Command
;
11 public class ToggleWinch
extends Command
{
12 DriveTrain driveTrain
= Robot
.getDriveTrain();
13 private double climbingSpeed
;
14 private double maintainPositionSpeed
;
16 public ToggleWinch() {
18 climbingSpeed
= driveTrain
.CLIMBER_SPEED
;
19 maintainPositionSpeed
= driveTrain
.MAINTAIN_CLIMBED_POSITION
;
23 protected void initialize() {
27 protected void execute() {
28 if (driveTrain
.shouldBeClimbing
) {
29 driveTrain
.setMotorValues(climbingSpeed
, climbingSpeed
);
31 driveTrain
.setMotorValues(maintainPositionSpeed
, maintainPositionSpeed
);
36 protected boolean isFinished() {
41 protected void end() {
42 driveTrain
.shouldBeClimbing
= !driveTrain
.shouldBeClimbing
;
46 protected void interrupted() {