Add command group for ToggleWinch
[3501/2017steamworks] / src / org / usfirst / frc / team3501 / robot / commandgroups / ToggleWinch.java
1 package org.usfirst.frc.team3501.robot.commandgroups;
2
3 import org.usfirst.frc.team3501.robot.Robot;
4 import org.usfirst.frc.team3501.robot.commands.climber.MaintainClimbedPosition;
5 import org.usfirst.frc.team3501.robot.commands.climber.RunWinchContinuous;
6
7 import edu.wpi.first.wpilibj.command.CommandGroup;
8
9 public class ToggleWinch extends CommandGroup {
10
11 public ToggleWinch() {
12 if (!Robot.getDriveTrain().isClimbing()) {
13 Robot.getDriveTrain().setClimbing(true);
14 addSequential(new RunWinchContinuous());
15
16 } else {
17 Robot.getDriveTrain().setClimbing(false);
18 addSequential(new MaintainClimbedPosition());
19 }
20 }
21
22 }