package org.usfirst.frc.team3501.robot.commands.climber;
import org.usfirst.frc.team3501.robot.Robot;
-import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+import org.usfirst.frc.team3501.robot.subsystems.Climber;
import edu.wpi.first.wpilibj.command.Command;
-/**
- *
- */
public class ToggleWinch extends Command {
- DriveTrain driveTrain = Robot.getDriveTrain();
+ Climber climber = Robot.getClimber();
private double climbingSpeed;
- private double maintainPositionSpeed;
public ToggleWinch() {
- requires(driveTrain);
- climbingSpeed = driveTrain.CLIMBER_SPEED;
- maintainPositionSpeed = driveTrain.MAINTAIN_CLIMBED_POSITION;
+ requires(climber);
+ climbingSpeed = climber.CLIMBER_SPEED;
}
@Override
protected void initialize() {
+ System.out.println("toggled");
}
@Override
protected void execute() {
- if (driveTrain.shouldBeClimbing) {
- driveTrain.setMotorValues(climbingSpeed, climbingSpeed);
+ if (climber.shouldBeClimbing) {
+ climber.setCANTalonsBrakeMode(climber.COAST_MODE);
+ climber.setMotorValues(climbingSpeed);
+ System.out.println("climbing");
} else {
- driveTrain.setMotorValues(maintainPositionSpeed, maintainPositionSpeed);
+ climber.setCANTalonsBrakeMode(climber.BRAKE_MODE);
+
+ System.out.println("braked");
+ /* Not sure if should have */
+ climber.stop();
+ end();
}
}
@Override
protected void end() {
- driveTrain.shouldBeClimbing = !driveTrain.shouldBeClimbing;
+ climber.shouldBeClimbing = !climber.shouldBeClimbing;
+ System.out.println("ended");
}
@Override