Merge pull request #59 from DeepBlueRobotics/manual-climb

add manual climb

Author: dfang314

Robot2019/src/main/java/frc/robot/OI.java

@@ -15,6 +15,7 @@
 import frc.robot.commands.Climb;
 import frc.robot.commands.EjectCargo;
 import frc.robot.commands.IntakeOnlyCargo;
+import frc.robot.commands.ManualClimb;
 import frc.robot.commands.NormalDrive;
 import frc.robot.commands.ResetWobble;
 import frc.robot.commands.SetArcadeOrTank;
@@ -42,7 +43,8 @@ public class OI {
   JoystickButton toggleHatchBtn;
   JoystickButton cargoIntakeBtn, cargoEjectBtn;
   JoystickButton climberRailBtn;
-  JoystickButton climbBtn;
+  JoystickButton autoClimbBtn;
+  JoystickButton manualClimbBtn;
   JoystickButton toggleCameraBtn;
   JoystickButton wobbleDriveBtn;
   JoystickButton cycleLightBtn;
@@ -66,19 +68,22 @@ public class OI {
     normDriveBtn = new JoystickButton(leftJoy, 3);
     normDriveBtn.whileHeld(new NormalDrive());
 
-    toggleHatchBtn = new JoystickButton(manipulator, Manip.X); // TODO: set ports to correct values
+    toggleHatchBtn = new JoystickButton(manipulator, Manip.X);
     toggleHatchBtn.whenPressed(new ToggleHatch(hp));
 
-    cargoIntakeBtn = new JoystickButton(manipulator, Manip.A); // TODO: set ports to correct values
+    cargoIntakeBtn = new JoystickButton(manipulator, Manip.A);
     cargoIntakeBtn.whenPressed(new IntakeOnlyCargo(cargo, hp, dt));
-    cargoEjectBtn = new JoystickButton(manipulator, Manip.B); // TODO: set ports to correct values
+    cargoEjectBtn = new JoystickButton(manipulator, Manip.B);
     cargoEjectBtn.whenPressed(new EjectCargo(cargo));
 
-    climberRailBtn = new JoystickButton(manipulator, Manip.LB_lShoulder); // TODO: confirm button number
+    climberRailBtn = new JoystickButton(manipulator, Manip.LB_lShoulder);
     climberRailBtn.whenPressed(new ActuateClimberRails(climber));
 
-    climbBtn = new JoystickButton(manipulator, Manip.Y); // TODO: confirm button number
-    climbBtn.whenPressed(new Climb(climber, dt, leftJoy));
+    autoClimbBtn = new JoystickButton(manipulator, Manip.RT_rTrigger);
+    autoClimbBtn.toggleWhenPressed(new Climb(climber, dt, leftJoy));
+
+    manualClimbBtn = new JoystickButton(manipulator, Manip.LT_lTrigger);
+    manualClimbBtn.toggleWhenPressed(new ManualClimb(climber, dt, leftJoy, rightJoy));
 
     toggleCameraBtn = new JoystickButton(leftJoy, 2);
     toggleCameraBtn.whenPressed(new ToggleCamera(driveCamera, hatchCamera, cameraServer));

Robot2019/src/main/java/frc/robot/commands/ManualClimb.java

@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* 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.Joystick;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.subsystems.Climber;
+import frc.robot.subsystems.Drivetrain;
+
+public class ManualClimb extends Command {
+  private Climber climber;
+  private Drivetrain dt;
+  private Joystick leftJoy, rightJoy;
+  
+  final private double retractDist = 0;
+  final private double climbDist = 24.46; // In inches
+
+  public ManualClimb(Climber climber, Drivetrain dt, Joystick leftJoy, Joystick rightJoy) {
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+    requires(climber);
+    requires(dt);
+
+    this.climber = climber;
+    this.dt = dt;
+    this.leftJoy = leftJoy;
+    this.rightJoy = rightJoy;
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    SmartDashboard.putNumber("Current Angle", 0);
+    SmartDashboard.putNumber("Max Angle", 0);
+    SmartDashboard.putNumber("Min Angle", 0);
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    double climbSpeed = -leftJoy.getY();
+    double driveSpeed = -rightJoy.getY();
+
+    if (climbSpeed > 0 && climber.getEncDistance() >= climbDist) {
+      climbSpeed = 0;
+    } else if (climbSpeed < 0 && climber.getEncDistance() <= retractDist) {
+      climbSpeed = 0;
+    }
+    climber.runClimber(climbSpeed);
+    dt.drive(driveSpeed, driveSpeed);
+
+    double angle = climber.getAngle();
+    SmartDashboard.putNumber("Current Angle", angle);
+    if (angle > SmartDashboard.getNumber("Max Angle", 0)) {
+      SmartDashboard.putNumber("Max Angle", angle);
+    }
+    if (angle < SmartDashboard.getNumber("Min Angle", 0)) {
+      SmartDashboard.putNumber("Min Angle", angle);
+    }
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    return false;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+    climber.stopClimber();
+    dt.stop();
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() {
+  }
+}

Robot2019/src/main/java/frc/robot/subsystems/Climber.java

@@ -36,6 +36,7 @@ public Climber(VictorSP motor, Encoder enc, AHRS ahrs, DoubleSolenoid pistons) {
     double pulseFraction = 1.0 / 256;
     double pitchDiameter = 1.790; // https://www.vexrobotics.com/35-sprockets.html#Drawing
     enc.setDistancePerPulse(pulseFraction * Math.PI * pitchDiameter);
+    enc.setReverseDirection(true);
     enc.reset();
   }
 
@@ -51,17 +52,24 @@ public void stopClimber() {
     motor.stopMotor();
   }
 
-  // We are erring on the side of changing directions too much
+  public double getAngle() {
+    double rawAngle = Math.atan2(ahrs.getRawAccelZ(), ahrs.getRawAccelX());
+    double angle;
+    if (rawAngle > 0) {
+      angle = rawAngle - Math.PI;
+    } else {
+      angle = rawAngle + Math.PI;
+    }
+    return angle * 180 / Math.PI;
+  }
+
+  //We are erring on the side of changing directions too much
   public boolean needToClimb() {
-    double angle = Math.atan2(ahrs.getRawAccelZ(), ahrs.getRawAccelX());
-    angle *= 180 / Math.PI;
-    return angle < maxTilt && enc.getDistance() < maxDist;
+    return getAngle() < maxTilt && enc.getDistance() < maxDist;
   }
 
   public boolean canDrop() {
-    double angle = Math.atan2(ahrs.getRawAccelZ(), ahrs.getRawAccelX());
-    angle *= 180 / Math.PI;
-    return angle > minTilt && enc.getDistance() > minDist;
+    return getAngle() > minTilt && enc.getDistance() > minDist;
   }
 
   public double getEncDistance() {