diff --git a/Robot2019/src/main/java/frc/robot/OI.java b/Robot2019/src/main/java/frc/robot/OI.java index 16d0328..f4fb838 100644 --- a/Robot2019/src/main/java/frc/robot/OI.java +++ b/Robot2019/src/main/java/frc/robot/OI.java @@ -18,6 +18,7 @@ import frc.robot.commands.IntakeHatch; import frc.robot.commands.ManualClimb; import frc.robot.commands.NormalDrive; +import frc.robot.commands.SlowClimb; import frc.robot.commands.ResetWobble; import frc.robot.commands.SetArcadeOrTank; import frc.robot.commands.SlowDrive; @@ -48,6 +49,7 @@ public class OI { JoystickButton climberRailBtn; JoystickButton autoClimbBtn; JoystickButton manualClimbBtn; + JoystickButton slowClimbBtn; JoystickButton toggleCameraBtn; JoystickButton wobbleDriveBtn; JoystickButton cycleLightBtn; @@ -92,6 +94,10 @@ public class OI { manualClimbBtn = new JoystickButton(manipulator, Manip.RT_rTrigger); manualClimbBtn.toggleWhenPressed(new ManualClimb(climber, manipulator, lights)); + + slowClimbBtn = new JoystickButton(manipulator, Manip.LB_lShoulder); + slowClimbBtn.whileHeld(new SlowClimb()); + toggleCameraBtn = new JoystickButton(leftJoy, 2); toggleCameraBtn.whenPressed(new ToggleCamera(driveCamera, hatchCamera, cameraServer)); diff --git a/Robot2019/src/main/java/frc/robot/commands/ManualClimb.java b/Robot2019/src/main/java/frc/robot/commands/ManualClimb.java index f7037a9..0c5d2a3 100644 --- a/Robot2019/src/main/java/frc/robot/commands/ManualClimb.java +++ b/Robot2019/src/main/java/frc/robot/commands/ManualClimb.java @@ -39,6 +39,9 @@ public ManualClimb(Climber climber, Joystick manip, Lights lights) { if (!SmartDashboard.containsKey("Climber Drive Limit")) { SmartDashboard.putNumber("Climber Drive Limit", 0); } + if (!SmartDashboard.containsKey("Slow Climb Neutral")) { + SmartDashboard.putNumber("Slow Climb Neutral", -0.5); + } } // Called just before this Command runs the first time @@ -56,16 +59,22 @@ protected void execute() { if (!SmartDashboard.getBoolean("Outreach Mode", false)) { double climbSpeed = manip.getRawAxis(climbJoyAxis); - // if (climbSpeed > 0 && climber.getEncDistance() >= climbDist) { - // climbSpeed = 0; - // } else if (climbSpeed < 0 && climber.getEncDistance() <= retractDist) { - // climbSpeed = 0; - // } - - // climber soft stop to prevent bindinng - if (climber.getEncDistance() > SmartDashboard.getNumber("Climber Max Height", 25) && (-climbSpeed) > SmartDashboard.getNumber("Climber Drive Limit", 0)) { - climbSpeed = -SmartDashboard.getNumber("Climber Drive Limit", 0); - } + if (SmartDashboard.getBoolean("Slow Climb", false)) { + double neutral = SmartDashboard.getNumber("Slow Climb Neutral", -0.5); + // this part doesn't make any immediate intuitive sense but trust me it is the correct formula + climbSpeed = climbSpeed * (1 + neutral) + neutral; + } + + // if (climbSpeed > 0 && climber.getEncDistance() >= climbDist) { + // climbSpeed = 0; + // } else if (climbSpeed < 0 && climber.getEncDistance() <= retractDist) { + // climbSpeed = 0; + // } + + // climber soft stop to prevent bindinng + if (climber.getEncDistance() > SmartDashboard.getNumber("Climber Max Height", 25) && (-climbSpeed) > SmartDashboard.getNumber("Climber Drive Limit", 0)) { + climbSpeed = -SmartDashboard.getNumber("Climber Drive Limit", 0); + } climber.runClimber(climbSpeed); diff --git a/Robot2019/src/main/java/frc/robot/commands/SlowClimb.java b/Robot2019/src/main/java/frc/robot/commands/SlowClimb.java new file mode 100644 index 0000000..0972433 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/SlowClimb.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.command.Command; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Add your docs here. + */ +public class SlowClimb extends Command { + /** + * Add your docs here. + */ + public SlowClimb() { + super(); + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called once when the command executes + @Override + protected void initialize() { + SmartDashboard.putBoolean("Slow Climb", true); + } + + @Override + protected void end() { + SmartDashboard.putBoolean("Slow Climb", false); + } + + @Override + protected void interrupted() { + end(); + } + + @Override + protected boolean isFinished() { return false; } +}