From 9dc69158f74215de20fd5fdcee299020e8f2b88b Mon Sep 17 00:00:00 2001 From: Cindy Zhang Date: Thu, 9 Feb 2017 19:42:15 -0800 Subject: [PATCH] implement ToggleWinch command --- .../usfirst/frc/team3501/robot/Constants.java | 2 - src/org/usfirst/frc/team3501/robot/OI.java | 8 +-- .../robot/commandgroups/ToggleWinch.java | 22 --------- .../commands/climber/RunWinchContinuous.java | 11 +++-- .../robot/commands/climber/ToggleWinch.java | 49 +++++++++++++++++++ .../team3501/robot/subsystems/DriveTrain.java | 27 +++------- 6 files changed, 62 insertions(+), 57 deletions(-) delete mode 100644 src/org/usfirst/frc/team3501/robot/commandgroups/ToggleWinch.java create mode 100644 src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java index a1a8cb0..3567e2f 100644 --- a/src/org/usfirst/frc/team3501/robot/Constants.java +++ b/src/org/usfirst/frc/team3501/robot/Constants.java @@ -45,8 +45,6 @@ public class Constants { public static final Value HIGH_GEAR = DoubleSolenoid.Value.kForward; public static final Value LOW_GEAR = DoubleSolenoid.Value.kReverse; - public static final double CLIMBER_SPEED = 5; - // MOTOR CONTROLLERS public static final int FRONT_LEFT = 1; public static final int FRONT_RIGHT = 3; diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java index 05fb72d..f878e7c 100644 --- a/src/org/usfirst/frc/team3501/robot/OI.java +++ b/src/org/usfirst/frc/team3501/robot/OI.java @@ -1,6 +1,6 @@ package org.usfirst.frc.team3501.robot; -import org.usfirst.frc.team3501.robot.commandgroups.ToggleWinch; +import org.usfirst.frc.team3501.robot.commands.climber.ToggleWinch; import org.usfirst.frc.team3501.robot.commands.driving.ToggleGear; import org.usfirst.frc.team3501.robot.commands.intake.ReverseIntakeContinuous; import org.usfirst.frc.team3501.robot.commands.intake.RunIntakeContinuous; @@ -57,12 +57,6 @@ public class OI { toggleWinch = new JoystickButton(leftJoystick, Constants.OI.TOGGLE_WINCH_PORT); - /* - * if (!Robot.getDriveTrain().isClimbing()) { toggleWinch.whenPressed(new - * RunWinchContinuous()); Robot.getDriveTrain().setClimbing(true); } else { - * toggleWinch.whenPressed(new MaintainClimbedPosition()); - * Robot.getDriveTrain().setClimbing(false); } - */ toggleWinch.whenPressed(new ToggleWinch()); } diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/ToggleWinch.java b/src/org/usfirst/frc/team3501/robot/commandgroups/ToggleWinch.java deleted file mode 100644 index 3766aee..0000000 --- a/src/org/usfirst/frc/team3501/robot/commandgroups/ToggleWinch.java +++ /dev/null @@ -1,22 +0,0 @@ -package org.usfirst.frc.team3501.robot.commandgroups; - -import org.usfirst.frc.team3501.robot.Robot; -import org.usfirst.frc.team3501.robot.commands.climber.MaintainClimbedPosition; -import org.usfirst.frc.team3501.robot.commands.climber.RunWinchContinuous; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -public class ToggleWinch extends CommandGroup { - - public ToggleWinch() { - if (!Robot.getDriveTrain().isClimbing()) { - Robot.getDriveTrain().setClimbing(true); - addSequential(new RunWinchContinuous()); - - } else { - Robot.getDriveTrain().setClimbing(false); - addSequential(new MaintainClimbedPosition()); - } - } - -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java index 6527799..210199d 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java +++ b/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java @@ -1,6 +1,7 @@ package org.usfirst.frc.team3501.robot.commands.climber; import org.usfirst.frc.team3501.robot.Robot; +import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; import edu.wpi.first.wpilibj.command.Command; @@ -17,6 +18,8 @@ import edu.wpi.first.wpilibj.command.Command; * */ public class RunWinchContinuous extends Command { + DriveTrain driveTrain = Robot.getDriveTrain(); + private double climbingSpeed; /** * See JavaDoc comment in class for details @@ -25,19 +28,17 @@ public class RunWinchContinuous extends Command { * value range is from -1 to 1 */ public RunWinchContinuous() { - requires(Robot.getDriveTrain()); + requires(driveTrain); + climbingSpeed = driveTrain.CLIMBER_SPEED; } @Override protected void initialize() { - Robot.getDriveTrain().setMotorValues( - Robot.getDriveTrain().getClimbingSpeed(), - Robot.getDriveTrain().getClimbingSpeed()); } @Override protected void execute() { - + Robot.getDriveTrain().setMotorValues(climbingSpeed, climbingSpeed); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java b/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java new file mode 100644 index 0000000..3e60406 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java @@ -0,0 +1,49 @@ +package org.usfirst.frc.team3501.robot.commands.climber; + +import org.usfirst.frc.team3501.robot.Robot; +import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class ToggleWinch extends Command { + DriveTrain driveTrain = Robot.getDriveTrain(); + private double climbingSpeed; + private double maintainPositionSpeed; + + public ToggleWinch() { + requires(driveTrain); + climbingSpeed = driveTrain.CLIMBER_SPEED; + maintainPositionSpeed = driveTrain.MAINTAIN_CLIMBED_POSITION; + } + + @Override + protected void initialize() { + } + + @Override + protected void execute() { + if (driveTrain.shouldBeClimbing) { + driveTrain.setMotorValues(climbingSpeed, climbingSpeed); + } else { + driveTrain.setMotorValues(maintainPositionSpeed, maintainPositionSpeed); + } + } + + @Override + protected boolean isFinished() { + return false; + } + + @Override + protected void end() { + driveTrain.shouldBeClimbing = !driveTrain.shouldBeClimbing; + } + + @Override + protected void interrupted() { + end(); + } +} diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index 7463b86..b0113e8 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -21,8 +21,11 @@ public class DriveTrain extends Subsystem { public static final int ENCODER_PULSES_PER_REVOLUTION = 256; public static final double INCHES_PER_PULSE = WHEEL_DIAMETER * Math.PI / ENCODER_PULSES_PER_REVOLUTION; - public static final int MAINTAIN_CLIMBED_POSITION = 0; - public static final int TIME_TO_CLIMB_FOR = 0; + + public static final double MAINTAIN_CLIMBED_POSITION = 0; + public static final double TIME_TO_CLIMB_FOR = 0; + public static final double CLIMBER_SPEED = 0; + private static DriveTrain driveTrain; private final CANTalon frontLeft, frontRight, rearLeft, rearRight; @@ -32,8 +35,7 @@ public class DriveTrain extends Subsystem { private ADXRS450_Gyro imu; - private boolean isClimbing; - private static double CLIMBER_SPEED;; + public boolean shouldBeClimbing; private DriveTrain() { // MOTOR CONTROLLERS @@ -63,8 +65,6 @@ public class DriveTrain extends Subsystem { rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.MODULE_NUMBER, Constants.DriveTrain.RIGHT_GEAR_PISTON_FORWARD, Constants.DriveTrain.RIGHT_GEAR_PISTON_REVERSE); - - CLIMBER_SPEED = Constants.DriveTrain.CLIMBER_SPEED; } public static DriveTrain getDriveTrain() { @@ -81,7 +81,6 @@ public class DriveTrain extends Subsystem { frontRight.set(-right); rearRight.set(-right); - this.isClimbing = true; } public void joystickDrive(final double thrust, final double twist) { @@ -90,7 +89,6 @@ public class DriveTrain extends Subsystem { public void stop() { setMotorValues(0, 0); - this.isClimbing = false; } public double getLeftMotorVal() { @@ -189,17 +187,4 @@ public class DriveTrain extends Subsystem { protected void initDefaultCommand() { setDefaultCommand(new JoystickDrive()); } - - public boolean isClimbing() { - return this.isClimbing; - } - - public void setClimbing(boolean isClimbing) { - this.isClimbing = isClimbing; - } - - public double getClimbingSpeed() { - return this.CLIMBER_SPEED; - } - } -- 2.30.2