Merge branch 'main' of https://github.com/Team334/R2024

src/main/java/frc/robot/Constants.java

@@ -65,7 +65,7 @@ public static class Speeds {
     public static final double SHOOTER_ANGLE_MAX_SPEED = 0.3;
     public static final double ELEVATOR_MAX_SPEED = 0.75;
 
-    public static final double INTAKE_FEED_SPEED = 0.5; // TODO: Get this
+    public static final double INTAKE_FEED_SPEED = 0.6; // TODO: Get this
     public static final double OUTTAKE_FEED_SPEED = -0.4;
 
     public static final double INTAKE_ACTUATE_MAX_SPEED = 0.3;

src/main/java/frc/robot/RobotContainer.java

@@ -143,10 +143,10 @@ private void configureBindings() {
     _operatorController.triangle().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.OUTTAKE));
     _operatorController.cross().whileTrue(new FeedActuate(_intakeSubsystem, FeedMode.INTAKE));
 
-    // test for the presets
-    _driveController.square().onTrue(
-      new AutonShoot(_shooterSubsystem, _elevatorSubsystem, _ledSubsystem, _swerveSubsystem, _intakeSubsystem)
-    );
+    _operatorController.R2().whileTrue(
+      Commands.runOnce(_intakeSubsystem::disableReverseSoftLimit, _intakeSubsystem).andThen(
+        Commands.run(() -> _intakeSubsystem.actuate(-0.3), _intakeSubsystem).handleInterrupt(_intakeSubsystem::resetReverseSoftLimit)
+    ));
 
     // driver bindings
     _driveController.R1().onTrue(Commands.runOnce(() -> _swerveSubsystem.fieldOriented = !_swerveSubsystem.fieldOriented, _swerveSubsystem));

src/main/java/frc/robot/commands/shooter/AutoAim.java

@@ -160,10 +160,6 @@ public void execute() {
       Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED
     );
 
-    _reachedSwerveHeading = _headingController.atSetpoint();
-    _reachedShooterAngle = _shooter.atDesiredAngle();
-    _reachedElevatorHeight = _elevator.atDesiredHeight();
-
     if (_reachedSwerveHeading) rotationVelocity = 0;
 
     SmartDashboard.putNumber("Y", _desiredSwerveHeading);
@@ -184,6 +180,10 @@ public void execute() {
 
     _shooter.setAngle(_desiredShooterAngle);
     _elevator.setHeight(_desiredElevatorHeight);
+
+    _reachedSwerveHeading = _headingController.atSetpoint();
+    _reachedShooterAngle = _shooter.atDesiredAngle();
+    _reachedElevatorHeight = _elevator.atDesiredHeight();
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/subsystems/IntakeSubsystem.java

@@ -16,6 +16,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.Constants.PID;
+import frc.robot.Constants.Speeds;
 import frc.robot.utils.configs.NeoConfig;
 
 /**
@@ -93,6 +94,19 @@ public boolean isFeedMode(FeedMode feedMode) {
 
   public boolean isActuatorState(ActuatorState actuatorState) {
     return _actuatorState == actuatorState;
+  /**
+   * Disables the reverse soft limit of the actuator.
+   */
+  public void disableReverseSoftLimit() {
+    _actuatorMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
+  }
+
+  /**
+   * Resets the reverse soft limit (and encoder) of the actuator.
+   */
+  public void resetReverseSoftLimit() {
+    _actuatorEncoder.setPosition(0);
+    _actuatorMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
   }
 
   /**
@@ -111,45 +125,42 @@ public double getActuator() {
     return _actuatorEncoder.getPosition();
   }
 
+  /**
+   * Actuate the intake at a given speed. Speed gets clamped.
+   */
+  public void actuate(double speed) {
+    _actuatorMotor.set(MathUtil.clamp(
+      speed,
+      -Speeds.INTAKE_ACTUATE_MAX_SPEED,
+      Speeds.INTAKE_ACTUATE_MAX_SPEED
+    ));
+  }
+
   /**
    * Set the actuator's state. MUST BE CALLED REPEATEDLY.
    *
    * @param actuatorState
    *            The state to set the actuator to.
    */
-  public void actuate(ActuatorState actuatorState) {
-    double out = 0;
-
+  public void actuate(ActuatorState actuatorState) {    
     _actuatorState = actuatorState;
 
     switch (actuatorState) {
       case STOWED :
-        out = MathUtil.clamp(
-          _actuatorController.calculate(getActuator(), Constants.Encoders.INTAKE_STOWED),
-          -Constants.Speeds.INTAKE_ACTUATE_MAX_SPEED,
-          Constants.Speeds.INTAKE_ACTUATE_MAX_SPEED
-        );
+        actuate(_actuatorController.calculate(getActuator(), Constants.Encoders.INTAKE_STOWED));
         break;
 
       case OUT :
-        out = MathUtil.clamp(
-          _actuatorController.calculate(getActuator(), Constants.Encoders.INTAKE_OUT),
-          -Constants.Speeds.INTAKE_ACTUATE_MAX_SPEED,
-          Constants.Speeds.INTAKE_ACTUATE_MAX_SPEED
-        );
+        actuate(_actuatorController.calculate(getActuator(), Constants.Encoders.INTAKE_OUT));
         break;
 
       case NONE:
-        out = 0;
+        actuate(0);
         break;
 
       default:
         break;
     }
-
-    // System.out.println(out);
-    SmartDashboard.putNumber("OUT", out);
-    _actuatorMotor.set(out);
   }
 
   // public void setAngle(double angle){
@@ -195,6 +206,7 @@ public void periodic() {
     // This method will be called once per scheduler run
     SmartDashboard.putNumber("ACTUATOR ENCODER", _actuatorEncoder.getPosition());
     SmartDashboard.putData("ACTUATOR PID", _actuatorController);
+    SmartDashboard.putNumber("ACTUATOR OUT", _actuatorMotor.get());
 
     // SmartDashboard.putBoolean("NOTE SAFETY", noteSafety());
 

src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java

@@ -86,9 +86,9 @@ public class SwerveDriveSubsystem extends SubsystemBase {
       Constants.Physical.SWERVE_KINEMATICS, getHeadingRaw(),
       new SwerveModulePosition[]{_frontLeft.getPosition(), _frontRight.getPosition(), _backRight.getPosition(),
           _backLeft.getPosition()},
-      new Pose2d(), VecBuilder.fill(0.01, 0.01, 0.01), VecBuilder.fill(0.9, 0.9, 1.2));
+      new Pose2d(), VecBuilder.fill(0.01, 0.01, 0.01), VecBuilder.fill(0.9, 0.9, 1.8)); // note: LL heading std was 0.9 
 
-  // OTHER POSSIBLE STD VALUES:
+  // OTHER POSSIBLE STD DEV VALUES:
   // VecBuilder.fill(0.006, 0.006, 0.007), VecBuilder.fill(0.52, 0.52, 1.35)
   // VecBuilder.fill(0.006, 0.006, 0.007), VecBuilder.fill(0.5, 0.5, 1.3)
 
@@ -173,6 +173,7 @@ public void periodic() {
     if (_visionSubsystem.isValid()) { // TODO: make sure this works
       Optional<Pose2d> visionBotpose = _visionSubsystem.getBotpose();
       if (visionBotpose.isPresent()) {
+        // Pose2d updateBotpose = new Pose2d(visionBotpose.get().getTranslation(), getHeading()); // use gyro heading only?
         _estimator.addVisionMeasurement(visionBotpose.get(), _visionSubsystem.getLatency());
       }
     }