Improved Turret shoot cmd, correct turret pose calc

src/main/java/frc/robot/Robot.java

@@ -19,7 +19,7 @@ public class Robot extends LoggedRobot {
   private final CommandScheduler m_scheduler = CommandScheduler.getInstance();
   private static Robot instance;
   private Command m_autonomousCommand;
-  private ShooterContainer m_robotContainer;
+  private RobotContainer m_robotContainer;
   private Timer disabledTimer;
   private final Timer m_alertTimer = new Timer();
   private final Alert m_canErrorAlert =
@@ -60,7 +60,7 @@ public static Robot getInstance() {
 
   @Override
   public void robotInit() {
-    m_robotContainer = new ShooterContainer();
+    m_robotContainer = new RobotContainer();
     disabledTimer = new Timer();
 
     if (isSimulation()) {

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

@@ -20,14 +20,19 @@
 import robottester.RobotTester;
 
 public class RobotContainer {
-
   private boolean m_testsAdded = false; // should only add tests once
-  private final IndexerSubsystem m_indexing = new IndexerSubsystem();
   private final CommandXboxController m_driverController = new CommandXboxController(0);
+  private final IndexerSubsystem m_indexing = new IndexerSubsystem();
   private final SwerveSubsystem m_swerve = new SwerveSubsystem(SwerveConstants.SwerveConfig.OCTANE);
   private final IntakeSubsystem m_intake = new IntakeSubsystem();
   private final ShooterSubsystem m_shooter =
-      new ShooterSubsystem()
+      new ShooterSubsystem(
+              new Trigger(
+                  () -> {
+                    return m_intake.getArmSetpoint().lt(Degrees.of(30))
+                        && m_intake.getArmInputs().position.lt(Degrees.of(16));
+                  }),
+              m_driverController.rightTrigger(0.5))
           .withRobotEstimatedPose(m_swerve::getPose)
           .withRobotFieldVelocity(m_swerve::getFieldVelocity)
           .withRobotRelativeVelocity(m_swerve::getRobotVelocity);
@@ -40,7 +45,8 @@ public class RobotContainer {
               new TurretCameraVisionIO("limelight-turret", VecBuilder.fill(0.3, 0.3, 9999999))
                   .withTurretRotationSupplier(
                       () ->
-                          new Rotation3d(Degrees.of(0), Degrees.of(0), m_shooter.getTurretAngle())))
+                          new Rotation3d(
+                              Degrees.of(0), Degrees.of(0), m_shooter.getTurretAngle().times(-1))))
           .withSpeedsSupplier(m_swerve::getFieldVelocity);
 
   private final RobotTester robotTester = new RobotTester();
@@ -59,18 +65,14 @@ public RobotContainer() {
   }
 
   private void configureBindings() {
+    m_driverController.leftTrigger(0.5).whileTrue(m_indexing.throwUp());
     m_driverController.leftBumper().toggleOnTrue(intake());
     m_driverController.rightBumper().onTrue(halfIntake());
 
-    new Trigger(
-            () -> {
-              return m_intake.getArmSetpoint().lt(Degrees.of(45))
-                  && m_intake.getArmInputs().position.lt(Degrees.of(16));
-            })
-        .onTrue(m_shooter.closeHood());
-
-    m_driverController.rightTrigger(0.5).whileTrue(m_shooter.trackHub(m_indexing));
+    m_driverController.rightTrigger(0.5).onTrue(m_shooter.trackHub(m_indexing));
     m_driverController.a().onTrue(m_shooter.closeHood());
+    m_driverController.x().onTrue(m_swerve.lockPose());
+    m_driverController.y().onTrue(m_intake.closeArm());
 
     m_swerve.configureBindings(m_driverController);
     m_intake.configureBindings(m_driverController);
@@ -81,7 +83,7 @@ private void configureBindings() {
   private void configureTests() {
     m_swerve.configureTests(robotTester);
     m_intake.configureTests(robotTester);
-    // m_shooter.configureTests(robotTester);
+    m_shooter.configureTests(robotTester);
   }
 
   public Command getAutonomousCommand() {

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

@@ -26,8 +26,8 @@ public final class Arm {
     public static final Current kStatorCurrentLimit = Amps.of(60);
 
     public static final Angle kClosedPosition = Degrees.of(6);
-    public static final Angle kOpenPosition = Degrees.of(89);
-    public static final Angle kHalfOpenPosition = Degrees.of(20);
+    public static final Angle kOpenPosition = Degrees.of(91);
+    public static final Angle kHalfOpenPosition = Degrees.of(14);
 
     public static final MechanismGearing kGearing =
         new MechanismGearing(GearBox.fromStages("5:1", "5:1", "5:1", "41:16"));

src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java

@@ -7,7 +7,7 @@
 import yams.gearing.MechanismGearing;
 import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode;
 
-public class IndexerConstants { // TODO: set real values for these constants
+public class IndexerConstants {
   public static class Transporter {
     public static final int kMotorID = 40;
     public static final double kTransportingMotorVoltage = -0.9;
@@ -21,9 +21,9 @@ public static class Transporter {
   public static class Feeder {
     public static final int kMotorID = 41;
     public static final double kFeedingMotorVoltage = 1;
-    public static final double kReversingMotorVoltage = -0.1;
+    public static final double kReversingMotorVoltage = -0.8;
     public static final double kStuckMotorVoltage = -1;
-    public static final boolean kIsInverted = true;
+    public static final boolean kIsInverted = false;
     public static final Current kStatorCurrentLimit = Amps.of(40);
     public static final MotorMode kMotorMode = MotorMode.COAST;
     public static final MechanismGearing kGearing =

src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java

@@ -30,7 +30,7 @@ public double getFeederDutyCycle() {
   }
 
   public Command throwUp() {
-    return m_feeder.reverse();
+    return m_feeder.reverse().alongWith(m_transporter.setDutyCycle(() -> -0.5));
   }
 
   public Command feederReverse() {

src/main/java/frc/robot/subsystems/shooter/HoodSubsystem.java

@@ -25,11 +25,14 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.subsystems.shooter.ShooterConstants.Hood.FeedForward;
 import frc.robot.subsystems.shooter.ShooterConstants.Hood.PID;
 import frc.robot.subsystems.shooter.ShooterConstants.Hood.PhysicalPropreties;
+import java.util.Optional;
 import org.littletonrobotics.junction.AutoLog;
 import org.littletonrobotics.junction.Logger;
+import org.msgpack.core.annotations.Nullable;
 import robottester.RobotTester;
 import robottester.TestOption;
 import yams.math.ExponentialProfilePIDController;
@@ -54,13 +57,15 @@ public static class HoodInputs {
     public double angleEncoder = 0.0;
   }
 
+  private Optional<Trigger> m_canBeOpen = Optional.empty();
+
   // private final Debouncer m_resetEncoderDebouncer = new Debouncer(0.1);
   private final SparkMax m_hoodMotor = new SparkMax(kMotorID, MotorType.kBrushless);
   // private final Encoder m_hoodEncoder =
-  //     new Encoder(
-  //         ShooterConstants.Hood.kEncoderIDA,
-  //         ShooterConstants.Hood.kEncoderIDB,
-  //         ShooterConstants.Hood.kIsEncoderInverted);
+  // new Encoder(
+  // ShooterConstants.Hood.kEncoderIDA,
+  // ShooterConstants.Hood.kEncoderIDB,
+  // ShooterConstants.Hood.kIsEncoderInverted);
   private final Timer m_resetEncoderTimer = new Timer();
 
   private final ExponentialProfilePIDController m_pid =
@@ -80,8 +85,8 @@ public static class HoodInputs {
   private final HoodInputsAutoLogged m_inputs = new HoodInputsAutoLogged();
   private boolean m_isHomed = false;
 
-  public HoodSubsystem() {
-
+  public HoodSubsystem(@Nullable Trigger canBeOpenTrigger) {
+    m_canBeOpen = Optional.ofNullable(canBeOpenTrigger);
     m_hoodMotorConfig =
         new SmartMotorControllerConfig(this)
             .withClosedLoopController(m_pid)
@@ -148,7 +153,11 @@ public Command runAngle(Angle angle) {
   }
 
   public void runAngleDirect(Angle angle) {
-    m_hoodSMC.setPosition(angle);
+    if (m_canBeOpen.isPresent() && m_canBeOpen.get().getAsBoolean()) {
+      m_hoodSMC.setPosition(Degrees.of(5));
+    } else {
+      m_hoodSMC.setPosition(angle);
+    }
   }
 
   public Angle getAngle() {
@@ -170,10 +179,11 @@ public void periodic() {
     // m_hoodSMC.setEncoderPosition(Degrees.of(getEncoderAngle()));
 
     // if (m_resetEncoderDebouncer.calculate(
-    //     m_hoodSMC.getVoltage().lte(Volts.of(0)) && m_hoodSMC.getStatorCurrent().gte(Amps.of(5))))
+    // m_hoodSMC.getVoltage().lte(Volts.of(0)) &&
+    // m_hoodSMC.getStatorCurrent().gte(Amps.of(5))))
     // {
-    //   m_hoodSMC.setVoltage(Volts.of(0));
-    //   m_hoodSMC.setEncoderPosition(Degrees.of(0));
+    // m_hoodSMC.setVoltage(Volts.of(0));
+    // m_hoodSMC.setEncoderPosition(Degrees.of(0));
     // }
   }
 
@@ -210,4 +220,8 @@ private Command homing() {
             })
         .andThen(m_hood.runTo(Degrees.of(2), Degrees.of(1)));
   }
+
+  public boolean isAtSetpoint() {
+    return m_hood.isNear(Degrees.of(m_inputs.setpoint), Degrees.of(1.2)).getAsBoolean();
+  }
 }