Climber changes during Sac Day 1 (#105)

* Fix angles for climbing

* Use atan for climber angle

* use pitch instead of accel calculations

* change rails to toggle

* changes to controls requested by drive team

* lol

* change slow climb formula

the goal of this new formula is to find a set voltage/joystick value
where the robot will be more or less supported statically by the
elevator, and use that as the neutral value of the manipulator

Author: kevinzwang

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));
 

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);
 

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; }
+}