Merge branch 'testRoboReveal' of https://github.com/Team4388/2022NoWayHome into testRoboReveal

src/main/java/frc4388/robot/Constants.java

@@ -174,6 +174,10 @@ public final class Constants {
     public static final int ELBOW_ID = 31;
     public static final int GYRO_ID = 14;
 
+    public static final double INPUT_MULTIPLIER = 0.7;
+    public static final double ELBOW_SOFT_LIMIT_TOLERANCE = 20000.0;
+    public static final double SHOULDER_SOFT_LIMIT_TOLERANCE = 12000.0;
+  
     // TODO Update this stuff too
     public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm
     public static final double LOWER_ARM_LENGTH = 27;
@@ -192,10 +196,10 @@ public final class Constants {
     public static final double ELBOW_GB_RATIO = 1.d;
     public static final double SHOULDER_GB_RATIO = 1.d;
 
-    public static final double SHOULDER_SOFT_LIMIT_FORWARD = 53869;
+    public static final double SHOULDER_FORWARD_SOFT_LIMIT = 53869;
-    public static final double SHOULDER_SOFT_LIMIT_REVERSE = 0;
+    public static final double SHOULDER_REVERSE_SOFT_LIMIT = 0;
-    public static final double ELBOW_SOFT_LIMIT_FORWARD = 281717;
+    public static final double ELBOW_FORWARD_SOFT_LIMIT = 281717;
-    public static final double ELBOW_SOFT_LIMIT_REVERSE = 0;
+    public static final double ELBOW_REVERSE_SOFT_LIMIT = 0;
 
     // PID Constants
     public static final int SHOULDER_SLOT_IDX = 0;

src/main/java/frc4388/robot/RobotContainer.java

@@ -61,7 +61,6 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
 import frc4388.robot.subsystems.Claws;
-import frc4388.robot.commands.RunClaw;
 import frc4388.robot.subsystems.ClimberRewrite;
 import frc4388.robot.subsystems.Claws.ClawType;
 import frc4388.robot.Constants.OIConstants;
@@ -69,6 +68,7 @@ import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch;
+import frc4388.robot.commands.ClimberCommands.RunClaw;
 // import frc4388.robot.commands.ButtonBoxCommands.TurretManual;
 import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
 import frc4388.robot.commands.ShooterCommands.AimToCenter;
@@ -158,9 +158,9 @@ public class RobotContainer {
     /* Default Commands */
 
     // moves climber in xy space with two-axis input from the operator controller
-    m_robotClimber.setDefaultCommand(
+    // m_robotClimber.setDefaultCommand(
-      new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getRightX() * 0.7, -getOperatorController().getRightY() * 0.7), 
+    //   new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getRightX() * 0.7, -getOperatorController().getRightY() * 0.7), 
-      m_robotClimber));
+    //   m_robotClimber));
 
 
     // IK command
@@ -206,8 +206,8 @@ public class RobotContainer {
         new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), 
         m_robotTurret).withName("Turret runTurretWithInput defaultCommand"));
 
-    // m_robotHood.setDefaultCommand(
+    m_robotHood.setDefaultCommand(
-    //    new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood));
+       new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood));
     // m_robotTurret.setDefaultCommand(
     //     new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
 
@@ -318,8 +318,22 @@ public class RobotContainer {
       .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender))
       .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
     
-    // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
+    new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
-    //     .whileHeld(new TurretManual(m_robotTurret));
+
+      .whenPressed(new InstantCommand(() -> m_robotTurret.setDefaultCommand(null)))
+      .whenPressed(new InstantCommand(() -> m_robotHood.setDefaultCommand(null)))
+      .whenPressed(new InstantCommand(() -> m_robotClimber.setDefaultCommand(
+        new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getLeftY(), -getOperatorController().getRightY()), m_robotClimber))))
+
+      .whenReleased(new InstantCommand(() -> m_robotClimber.setDefaultCommand(null)))
+      .whenReleased(new InstantCommand(() -> m_robotTurret.setDefaultCommand(
+        new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret))))
+      .whenReleased(new InstantCommand(() -> m_robotHood.setDefaultCommand(
+        new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood))));
+
+      // .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER))
+      // .whenReleased(new InstantCommand(() -> this.currentMode = CurrentMode.TURRET));
+      // .whenReleased(EnableClimber()));
       
     // control turret manual mode
     // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
@@ -407,7 +421,6 @@ public class RobotContainer {
    * Finally, adds the existing path files to the auto chooser
    */
   private void autoInit() {
-
     try {
       WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(),
           StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY,

src/main/java/frc4388/robot/commands/ClimberCommands/RunClaw.java

@@ -2,7 +2,7 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-package frc4388.robot.commands;
+package frc4388.robot.commands.ClimberCommands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.ClawConstants;

src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java

@@ -2,7 +2,7 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-package frc4388.robot.commands.climber;
+package frc4388.robot.commands.ClimberCommands;
 
 import org.opencv.core.Point;
 

src/main/java/frc4388/robot/subsystems/ClimberRewrite.java

@@ -37,6 +37,8 @@ public class ClimberRewrite extends SubsystemBase {
   private Point tPoint;
 
   private boolean groundRelative;
+  private double shoulderSpeedLimiter;
+  private double elbowSpeedLimiter;
 
   /** Creates a new ClimberRewrite. */
   public ClimberRewrite(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_Pigeon2 gyro, boolean _groundRelative) {
@@ -55,14 +57,14 @@ public class ClimberRewrite extends SubsystemBase {
     // m_shoulder.setSelectedSensorPosition(((ClimberConstants.SHOULDER_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO);
     // m_elbow.setSelectedSensorPosition(((ClimberConstants.ELBOW_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO);
 
-    m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_FORWARD);
+    m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT);
     m_elbow.configForwardSoftLimitEnable(true);
     // m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE);
     // m_elbow.configReverseSoftLimitEnable(true);
 
-    m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_FORWARD);
+    m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT);
     m_shoulder.configForwardSoftLimitEnable(true);
-    m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE);
+    m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT);
     m_shoulder.configReverseSoftLimitEnable(false);
 
     m_shoulder.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
@@ -78,6 +80,7 @@ public class ClimberRewrite extends SubsystemBase {
       m_gyro = gyro;
     
     groundRelative = _groundRelative;
+    this.elbowSpeedLimiter = 1.0;
   }
 
   public void setClimberGains() {
@@ -129,8 +132,8 @@ public class ClimberRewrite extends SubsystemBase {
   }
 
   public void setMotors(double shoulderOutput, double elbowOutput) {
-    m_shoulder.set(shoulderOutput);
+    m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER * this.shoulderSpeedLimiter);
-    m_elbow.set(elbowOutput);
+    m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER * this.elbowSpeedLimiter);
   }
 
   public double[] getJointAngles() {
@@ -179,6 +182,40 @@ public class ClimberRewrite extends SubsystemBase {
     SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition());
     // double[] jointAngles = getTargetJointAngles(tPoint, 0.d);
     // setJointAngles(jointAngles);
+
+    // * speed limiting near ELBOW soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output).
+    double currentElbowPos = this.m_elbow.getSelectedSensorPosition();
+    double forwardElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT);
+    double reverseElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_REVERSE_SOFT_LIMIT);
+
+    if (forwardElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) {
+      this.elbowSpeedLimiter = 0.15 + (forwardElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE));
+    }
+
+    if (reverseElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) {
+      this.elbowSpeedLimiter = 0.15 + (reverseElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE));
+    }
+
+    if ((forwardElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) && (reverseElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)) {
+      this.elbowSpeedLimiter = 1.0;
+    }
+
+    // * speed limiting near SHOULDER soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output).
+    double currentShoulderPos = this.m_shoulder.getSelectedSensorPosition();
+    double forwardShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT);
+    double reverseShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT);
+
+    if (forwardShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) {
+      this.shoulderSpeedLimiter = 0.15 + (forwardShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE));
+    }
+
+    if (reverseShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) {
+      this.shoulderSpeedLimiter = 0.15 + (reverseShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE));
+    }
+
+    if ((forwardShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) && (reverseShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)) {
+      this.shoulderSpeedLimiter = 1.0;
+    }
   }
 
   /**

src/main/java/frc4388/robot/subsystems/Hood.java

@@ -68,19 +68,16 @@ public class Hood extends SubsystemBase {
     double reverseDistance = Math.abs(currentPos - ShooterConstants.HOOD_REVERSE_SOFT_LIMIT);
 
     if (forwardDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
-      this.speedLimiter = 0.2 + (forwardDistance * 0.05);
+      this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
     }
 
     if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
-      this.speedLimiter = 0.2 + (reverseDistance * 0.05);
+      this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
     }
 
     if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) {
       this.speedLimiter = 1.0;
     }
-
-    double hoodCurrent = m_angleAdjusterMotor.getOutputCurrent();
-
   }
 
     /**

src/main/java/frc4388/robot/subsystems/Turret.java

@@ -152,11 +152,11 @@ public class Turret extends SubsystemBase {
     double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT);
 
     if (forwardDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) {
-      this.speedLimiter = 0.2 + (forwardDistance * 0.05);
+      this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE));
     }
 
     if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) {
-      this.speedLimiter = 0.2 + (reverseDistance * 0.05);
+      this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE));
     }
 
     if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) {