Cleanup and controls changes for event

team422 · May 31, 2024 · 94c1427 · 94c1427
1 parent ef0c6d3
commit 94c1427
Show file tree

Hide file tree

Showing 11 changed files with 160 additions and 40 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -101,6 +101,28 @@ public static final class ShooterConstants {
         new LoggedTunableNumber("Shooter Left Flywheel kV", 0.27);
     public static final SimpleMotorFeedforward kLeftFlywheelFeedforward =
         new SimpleMotorFeedforward(kLeftFlywheelKs.get(), kLeftFlywheelKv.get());
+
+    // setpoints for ImmerseCon (firing into crowd)
+    public static final LoggedTunableNumber kLeftFlywheelLowVelocity =
+        new LoggedTunableNumber("Shooter Left Flywheel Low Velocity", 9.0);
+    public static final LoggedTunableNumber kRightFlywheelLowVelocity =
+        new LoggedTunableNumber("Shooter Right Flywheel Low Velocity", 11.0);
+    public static final LoggedTunableNumber kPivotLowAngle =
+        new LoggedTunableNumber("Shooter Pivot Low Angle", 20);
+
+    public static final LoggedTunableNumber kLeftFlywheelMidVelocity =
+        new LoggedTunableNumber("Shooter Left Flywheel Mid Velocity", 13.0);
+    public static final LoggedTunableNumber kRightFlywheelMidVelocity =
+        new LoggedTunableNumber("Shooter Right Flywheel Mid Velocity", 15.0);
+    public static final LoggedTunableNumber kPivotMidAngle =
+        new LoggedTunableNumber("Shooter Pivot Mid Angle", 35);
+
+    public static final LoggedTunableNumber kLeftFlywheelHighVelocity =
+        new LoggedTunableNumber("Shooter Left Flywheel High Velocity", 19.0);
+    public static final LoggedTunableNumber kRightFlywheelHighVelocity =
+        new LoggedTunableNumber("Shooter Right Flywheel High Velocity", 21.0);
+    public static final LoggedTunableNumber kPivotHighAngle =
+        new LoggedTunableNumber("Shooter Pivot High Angle", 55);
   }
 
   public static final class IntakeConstants {

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -13,13 +13,13 @@
 
 package frc.robot;
 
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.Constants.IndexerConstants;
 import frc.robot.Constants.IntakeConstants;
 import frc.robot.Constants.Ports;
 import frc.robot.Constants.ShooterConstants;
+import frc.robot.RobotState.RobotAction;
 import frc.robot.commands.DriveCommands;
 import frc.robot.oi.DriverControls;
 import frc.robot.oi.DriverControlsPS5;
@@ -157,21 +157,42 @@ private void configureButtonBindings() {
             m_driverControls::getDriveRotation));
 
     m_driverControls
-        .revShooter()
+        .revShooterLow()
         .onTrue(
             Commands.runOnce(
                 () -> {
-                  m_shooter.setFlywheelVelocity(17.0, 20.0);
-                  m_shooter.setPivotAngle(Rotation2d.fromDegrees(50));
-                  // deploy intake so shooter pivot doesn't run into it
-                  m_intake.setPivotAngle(IntakeConstants.kDeployedAngle);
+                  m_robotState.setCurAction(RobotAction.REV_SHOOTER_LOW);
                 }))
         .onFalse(
             Commands.runOnce(
                 () -> {
-                  m_shooter.setFlywheelVelocity(0.0, 0.0);
-                  m_shooter.setPivotAngle(ShooterConstants.kHomeAngle);
-                  m_intake.setPivotAngle(IntakeConstants.kHomeAngle);
+                  m_robotState.setCurAction(RobotAction.STOW);
+                }));
+
+    m_driverControls
+        .revShooterMid()
+        .onTrue(
+            Commands.runOnce(
+                () -> {
+                  m_robotState.setCurAction(RobotAction.REV_SHOOTER_MID);
+                }))
+        .onFalse(
+            Commands.runOnce(
+                () -> {
+                  m_robotState.setCurAction(RobotAction.STOW);
+                }));
+
+    m_driverControls
+        .revShooterHigh()
+        .onTrue(
+            Commands.runOnce(
+                () -> {
+                  m_robotState.setCurAction(RobotAction.REV_SHOOTER_HIGH);
+                }))
+        .onFalse(
+            Commands.runOnce(
+                () -> {
+                  m_robotState.setCurAction(RobotAction.STOW);
                 }));
 
     m_driverControls
@@ -184,16 +205,12 @@ private void configureButtonBindings() {
         .onTrue(
             Commands.runOnce(
                 () -> {
-                  m_indexer.setFeederVoltage(IndexerConstants.kFeederVoltage);
-                  m_intake.setRollerVoltage(IntakeConstants.kDeployRollerVoltage);
-                  m_intake.setPivotAngle(IntakeConstants.kDeployedAngle);
+                  m_robotState.setCurAction(RobotAction.INTAKE);
                 }))
         .onFalse(
             Commands.runOnce(
                 () -> {
-                  m_indexer.setFeederVoltage(0.0);
-                  m_intake.setRollerVoltage(0.0);
-                  m_intake.setPivotAngle(IntakeConstants.kHomeAngle);
+                  m_robotState.setCurAction(RobotAction.STOW);
                 }));
   }
 

diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java
@@ -3,6 +3,7 @@
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.numbers.N1;
@@ -14,18 +15,32 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
+import frc.robot.Constants.IndexerConstants;
+import frc.robot.Constants.IntakeConstants;
+import frc.robot.Constants.ShooterConstants;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.indexer.Indexer;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.led.Led;
 import frc.robot.subsystems.led.Led.LedState;
 import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.Shooter.ShooterPosition;
 import java.util.HashMap;
 import java.util.Map;
 import org.littletonrobotics.junction.Logger;
 
 public class RobotState {
 
+  public static enum RobotAction {
+    STOW,
+    INTAKE,
+    REV_SHOOTER_LOW,
+    REV_SHOOTER_MID,
+    REV_SHOOTER_HIGH
+  }
+
+  private RobotAction m_curAction = RobotAction.STOW;
+
   private static RobotState instance = null;
 
   // subsystems
@@ -138,6 +153,55 @@ public void updateRobotState() {
       updateComponents();
     }
 
+    if (edu.wpi.first.wpilibj.RobotState.isDisabled()) {
+      updatePrematchCheck();
+    }
+  }
+
+  public void setCurAction(RobotAction action) {
+    if (m_curAction == action) {
+      return;
+    }
+    m_curAction = action;
+    switch (m_curAction) {
+      case STOW:
+        m_shooter.setShooter(new ShooterPosition(ShooterConstants.kHomeAngle, 0.0, 0.0));
+        m_intake.setPivotAngle(IntakeConstants.kHomeAngle);
+        m_intake.setRollerVoltage(0.0);
+        break;
+      case INTAKE:
+        m_intake.setPivotAngle(IntakeConstants.kDeployedAngle);
+        m_intake.setRollerVoltage(IntakeConstants.kDeployRollerVoltage);
+        m_indexer.setFeederVoltage(IndexerConstants.kFeederVoltage);
+        break;
+      case REV_SHOOTER_LOW:
+        m_shooter.setShooter(
+            new ShooterPosition(
+                Rotation2d.fromDegrees(ShooterConstants.kPivotLowAngle.get()),
+                ShooterConstants.kLeftFlywheelLowVelocity.get(),
+                ShooterConstants.kRightFlywheelLowVelocity.get()));
+        m_intake.setPivotAngle(IntakeConstants.kDeployedAngle);
+        break;
+      case REV_SHOOTER_MID:
+        m_shooter.setShooter(
+            new ShooterPosition(
+                Rotation2d.fromDegrees(ShooterConstants.kPivotMidAngle.get()),
+                ShooterConstants.kLeftFlywheelMidVelocity.get(),
+                ShooterConstants.kRightFlywheelMidVelocity.get()));
+        m_intake.setPivotAngle(IntakeConstants.kDeployedAngle);
+        break;
+      case REV_SHOOTER_HIGH:
+        m_shooter.setShooter(
+            new ShooterPosition(
+                Rotation2d.fromDegrees(ShooterConstants.kPivotHighAngle.get()),
+                ShooterConstants.kLeftFlywheelHighVelocity.get(),
+                ShooterConstants.kRightFlywheelHighVelocity.get()));
+        m_intake.setPivotAngle(IntakeConstants.kDeployedAngle);
+        break;
+    }
+  }
+
+  public void updatePrematchCheck() {
     boolean prematchReady = true;
     for (String key : kPrematchCheckValues.keySet()) {
       Logger.recordOutput("PreMatchCheck/" + key, kPrematchCheckValues.get(key));

diff --git a/src/main/java/frc/robot/oi/DriverControls.java b/src/main/java/frc/robot/oi/DriverControls.java
@@ -9,7 +9,11 @@ public interface DriverControls {
 
   public double getDriveRotation();
 
-  public Trigger revShooter();
+  public Trigger revShooterLow();
+
+  public Trigger revShooterMid();
+
+  public Trigger revShooterHigh();
 
   public Trigger runKicker();
 

diff --git a/src/main/java/frc/robot/oi/DriverControlsPS5.java b/src/main/java/frc/robot/oi/DriverControlsPS5.java
@@ -26,10 +26,20 @@ public double getDriveRotation() {
   }
 
   @Override
-  public Trigger revShooter() {
+  public Trigger revShooterLow() {
+    return m_controller.circle();
+  }
+
+  @Override
+  public Trigger revShooterMid() {
     return m_controller.L2();
   }
 
+  @Override
+  public Trigger revShooterHigh() {
+    return m_controller.square();
+  }
+
   @Override
   public Trigger runKicker() {
     return m_controller.R1();

diff --git a/src/main/java/frc/robot/oi/DriverControlsXbox.java b/src/main/java/frc/robot/oi/DriverControlsXbox.java
@@ -26,10 +26,20 @@ public double getDriveRotation() {
   }
 
   @Override
-  public Trigger revShooter() {
+  public Trigger revShooterLow() {
+    return m_controller.b();
+  }
+
+  @Override
+  public Trigger revShooterMid() {
     return m_controller.leftTrigger();
   }
 
+  @Override
+  public Trigger revShooterHigh() {
+    return m_controller.x();
+  }
+
   @Override
   public Trigger runKicker() {
     return m_controller.rightBumper();

diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOFalcon.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOFalcon.java
@@ -2,6 +2,7 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DigitalInput;
@@ -59,11 +60,11 @@ public void updateInputs(IndexerInputs inputs) {
 
   @Override
   public void setKickerVoltage(double voltage) {
-    m_kickerMotor.setVoltage(voltage);
+    m_kickerMotor.setControl(new VoltageOut(voltage));
   }
 
   @Override
   public void setFeederVoltage(double voltage) {
-    m_feederMotor.setVoltage(voltage);
+    m_feederMotor.setControl(new VoltageOut(voltage));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOKraken.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOKraken.java
@@ -2,6 +2,7 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.math.util.Units;
 
@@ -29,6 +30,6 @@ public void updateInputs(RollerInputs inputs) {
 
   @Override
   public void setVoltage(double voltage) {
-    m_motor.setVoltage(voltage);
+    m_motor.setControl(new VoltageOut(voltage));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -19,18 +19,8 @@
 
 public class Shooter extends SubsystemBase {
   /** Represents the desired state of the shooter, including flywheel and pivot measurements */
-  public static class ShooterPosition {
-    public Rotation2d pivotAngle;
-    public double leftFlywheelVelocity;
-    public double rightFlywheelVelocity;
-
-    public ShooterPosition(
-        Rotation2d pivotAngle, double leftFlywheelVelocity, double rightFlywheelVelocity) {
-      this.pivotAngle = pivotAngle;
-      this.leftFlywheelVelocity = leftFlywheelVelocity;
-      this.rightFlywheelVelocity = rightFlywheelVelocity;
-    }
-  }
+  public static record ShooterPosition(
+      Rotation2d pivotAngle, double leftFlywheelVelocity, double rightFlywheelVelocity) {}
 
   private ShooterPivotIO m_pivotIO;
   private FlywheelIO m_flywheelIO;
@@ -205,13 +195,13 @@ public Command setFlywheelVelocityCommand(double leftVelocity, double rightVeloc
     return Commands.runOnce(() -> setFlywheelVelocity(leftVelocity, rightVelocity));
   }
 
-  public void setShooter(ShooterPosition state) {
-    setPivotAngle(state.pivotAngle);
-    setFlywheelVelocity(state.leftFlywheelVelocity, state.rightFlywheelVelocity);
+  public void setShooter(ShooterPosition position) {
+    setPivotAngle(position.pivotAngle());
+    setFlywheelVelocity(position.leftFlywheelVelocity(), position.rightFlywheelVelocity());
   }
 
-  public Command setShooterCommand(ShooterPosition state) {
-    return Commands.runOnce(() -> setShooter(state));
+  public Command setShooterCommand(ShooterPosition position) {
+    return Commands.runOnce(() -> setShooter(position));
   }
 
   public Rotation2d getPivotAngle() {

diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOKraken.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOKraken.java
@@ -78,6 +78,6 @@ private double rotationsPerSecondToMeterPerSecond(double rotationsPerSecond) {
   @Override
   public void setVoltage(double leftVoltage, double rightVoltage) {
     m_leftMotor.setControl(new VoltageOut(leftVoltage));
-    m_rightMotor.setVoltage(rightVoltage);
+    m_rightMotor.setControl(new VoltageOut(rightVoltage));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/pivot/ShooterPivotIOFalcon.java b/src/main/java/frc/robot/subsystems/shooter/pivot/ShooterPivotIOFalcon.java
@@ -3,6 +3,7 @@
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
@@ -66,7 +67,7 @@ public void updateInputs(ShooterPivotInputs inputs) {
 
   @Override
   public void setVoltage(double voltage) {
-    m_leader.setVoltage(voltage);
+    m_leader.setControl(new VoltageOut(voltage));
   }
 
   @Override