Testing changes

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

@@ -117,7 +117,7 @@ public static final class IntakeConstants {
     public static final LoggedTunableNumber kIdleVoltage =
         new LoggedTunableNumber("Intake Idle Voltage", 0.0);
     public static final LoggedTunableNumber kIntakeVoltage =
-        new LoggedTunableNumber("Intake Voltage", -7.0);
+        new LoggedTunableNumber("Intake Voltage", -10.0);
     public static final LoggedTunableNumber kOuttakeVoltage =
         new LoggedTunableNumber("Outtake Voltage", 7.0);
 
@@ -133,16 +133,24 @@ public static final class KickerConstants {
     public static final LoggedTunableNumber kIdleVoltage =
         new LoggedTunableNumber("Kicker Idle Voltage", 0.0);
     public static final LoggedTunableNumber kIntakingVoltage =
-        new LoggedTunableNumber("Kicker Intake Voltage", -7.0);
+        new LoggedTunableNumber("Kicker Intake Voltage", -8.0);
+    public static final LoggedTunableNumber kIndexingVoltage =
+        new LoggedTunableNumber("Kicker Intake Voltage", -8.0);
+    public static final LoggedTunableNumber kReversingVoltage =
+        new LoggedTunableNumber("Kicker Reverse Voltage", 8.0);
     public static final LoggedTunableNumber kShootingVoltage =
-        new LoggedTunableNumber("Kicker Shooting Voltage", -7.0);
+        new LoggedTunableNumber("Kicker Shooting Voltage", -10.0);
     public static final LoggedTunableNumber kEjectingVoltage =
-        new LoggedTunableNumber("Kicker Eject Voltage", 7.0);
+        new LoggedTunableNumber("Kicker Eject Voltage", 8.0);
 
     public static final LoggedTunableNumber kShootingTimeout =
         new LoggedTunableNumber("Kicker Shooting Timeout", 1.0);
     public static final LoggedTunableNumber kIntakeTimeout =
         new LoggedTunableNumber("Kicker Shooting Timeout", 1.0);
+    public static final LoggedTunableNumber kIndexingTimeout =
+        new LoggedTunableNumber("Kicker Indexing Timeout", 0.1);
+    public static final LoggedTunableNumber kReverseTimeout =
+        new LoggedTunableNumber("Kicker Reverse Timeout", 0.5);
 
     // Simulation constants
     public static final DCMotor kSimGearbox = DCMotor.getNEO(1);
@@ -180,6 +188,17 @@ public static final class ShooterConstants {
     public static final LoggedTunableNumber kAmpBottomVelocity =
         new LoggedTunableNumber("Amp Bottom Velocity", 7.0);
 
+    public static final LoggedTunableNumber kSubwooferTopVelocity =
+        new LoggedTunableNumber("Subwoofer Top Velocity", 60.0);
+    public static final LoggedTunableNumber kSubwooferBottomVelocity =
+        new LoggedTunableNumber("Subwoofer Bottom Velocity", 40.0);
+
+    public static final boolean kManualControl = false;
+    public static final LoggedTunableNumber kManualTopVelocity =
+        new LoggedTunableNumber("Manual Top Velocity", 0.0);
+    public static final LoggedTunableNumber kManualBottomVelocity =
+        new LoggedTunableNumber("Manual Bottom Velocity", 0.0);
+
     // Simulation constants
     public static final DCMotor kSimTopGearbox = DCMotor.getNEO(1);
     public static final DCMotor kSimBottomGearbox = DCMotor.getNEO(1);

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

@@ -26,7 +26,7 @@
 import frc.robot.commands.DriveCommands;
 import frc.robot.commands.auto.AutoFactory;
 import frc.robot.oi.DriverControls;
-import frc.robot.oi.DriverControlsXbox;
+import frc.robot.oi.DriverControlsPS5;
 import frc.robot.oi.OperatorControls;
 import frc.robot.oi.OperatorControlsXbox;
 import frc.robot.subsystems.aprilTagVision.AprilTagVision;
@@ -163,8 +163,8 @@ private void configureCommands() {
 
   /** Configure the controllers. */
   private void configureControllers() {
-    // m_driverControls = new DriverControlsPS5(0);
-    m_driverControls = new DriverControlsXbox(0);
+    m_driverControls = new DriverControlsPS5(0);
+    // m_driverControls = new DriverControlsXbox(0);
     m_operatorControls = new OperatorControlsXbox(5);
   }
 
@@ -189,8 +189,8 @@ private void configureButtonBindings() {
 
     m_driverControls
         .runIntake()
-        .onTrue(
-            Commands.runOnce(
+        .whileTrue(
+            Commands.run(
                 () -> {
                   m_robotState.updateRobotAction(RobotAction.kIntake);
                 }))
@@ -284,6 +284,19 @@ private void configureButtonBindings() {
                   }
                 }));
 
+    m_driverControls
+        .subwooferShot()
+        .onTrue(
+            Commands.runOnce(
+                () -> {
+                  m_robotState.updateRobotAction(RobotAction.kSubwooferShot);
+                }))
+        .onFalse(
+            Commands.runOnce(
+                () -> {
+                  m_robotState.setDefaultAction();
+                }));
+
     m_operatorControls
         .runIntake()
         .whileTrue(

src/main/java/frc/robot/RobotState.java

@@ -5,6 +5,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.Constants.KickerConstants;
+import frc.robot.Constants.ShooterConstants;
 import frc.robot.subsystems.aprilTagVision.AprilTagVision;
 import frc.robot.subsystems.aprilTagVision.AprilTagVision.VisionObservation;
 import frc.robot.subsystems.drive.Drive;
@@ -40,6 +41,7 @@ public enum RobotAction {
     kIntake,
     kRevAndAlign,
     kRevNoAlign,
+    kSubwooferShot,
     kVomitting,
     kFeeding,
     kAmpLineup,
@@ -66,6 +68,7 @@ private RobotState(
     periodicHash.put(RobotAction.kVomitting, () -> {});
     periodicHash.put(RobotAction.kRevAndAlign, this::revAndAlignPeriodic);
     periodicHash.put(RobotAction.kRevNoAlign, this::revNoAlignPeriodic);
+    periodicHash.put(RobotAction.kSubwooferShot, this::subwooferShotPeriodic);
     periodicHash.put(RobotAction.kFeeding, this::feedingPeriodic);
     periodicHash.put(RobotAction.kAmpLineup, this::ampLineupPeriodic);
 
@@ -155,6 +158,12 @@ public void autoShootPeriodic() {
     }
   }
 
+  public void subwooferShotPeriodic() {
+    m_shooter.setDesiredVelocity(
+        ShooterConstants.kSubwooferTopVelocity.get(),
+        ShooterConstants.kSubwooferBottomVelocity.get());
+  }
+
   public void feedingPeriodic() {
     Pose2d currPose = getEstimatedPose();
     ShooterPosition position = m_shooterMath.calculateFeedingShooter(currPose);
@@ -193,6 +202,7 @@ public void updateRobotAction(RobotAction newAction) {
 
         break;
 
+      case kSubwooferShot:
       case kRevNoAlign:
         m_intake.updateState(IntakeState.kIdle);
         m_shooter.updateState(ShooterState.kRevving);

src/main/java/frc/robot/commands/DriveCommands.java

@@ -64,6 +64,7 @@ public static Command joystickDrive(
           boolean isFlipped =
               DriverStation.getAlliance().isPresent()
                   && DriverStation.getAlliance().get() == Alliance.Red;
+          System.out.println();
           drive.setDesiredChassisSpeeds(
               ChassisSpeeds.fromFieldRelativeSpeeds(
                   linearVelocity.getX() * DriveConstants.kMaxLinearSpeed,

src/main/java/frc/robot/oi/DriverControls.java

@@ -26,4 +26,6 @@ public interface DriverControls {
   public Trigger amp();
 
   public Trigger cancelAmpLineup();
+
+  public Trigger subwooferShot();
 }

src/main/java/frc/robot/oi/DriverControlsPS5.java

@@ -22,7 +22,7 @@ public double getStrafe() {
 
   @Override
   public double getTurn() {
-    return -m_controller.getRightX();
+    return m_controller.getRightX();
   }
 
   @Override
@@ -64,4 +64,9 @@ public Trigger amp() {
   public Trigger cancelAmpLineup() {
     return new Trigger(() -> Math.abs(m_controller.getRightX()) > 0.3);
   }
+
+  @Override
+  public Trigger subwooferShot() {
+    return m_controller.cross();
+  }
 }

src/main/java/frc/robot/oi/DriverControlsXbox.java

@@ -64,4 +64,9 @@ public Trigger amp() {
   public Trigger cancelAmpLineup() {
     return new Trigger(() -> Math.abs(m_controller.getRightX()) > 0.3);
   }
+
+  @Override
+  public Trigger subwooferShot() {
+    return m_controller.a();
+  }
 }

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -63,7 +63,7 @@ public enum DriveProfiles {
   private ChassisSpeeds m_desiredChassisSpeeds = new ChassisSpeeds();
 
   private Rotation2d m_desiredHeading = new Rotation2d();
-  private PIDController m_headingController = new PIDController(3.5, 0, 0.09);
+  private PIDController m_headingController = new PIDController(0.1, 0, 0.09);
 
   private Rotation2d m_rawGyroRotation = new Rotation2d();
   private SwerveModulePosition[] m_lastModulePositions = // For delta tracking

src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java

@@ -70,25 +70,25 @@ public ModuleIOTalonFX(int index) {
         m_driveTalon = new TalonFX(Ports.kFrontLeftDrive, Ports.kCanivoreName);
         m_turnTalon = new TalonFX(Ports.kFrontLeftTurn, Ports.kCanivoreName);
         m_cancoder = new CANcoder(Ports.kFrontLeftCancoder, Ports.kCanivoreName);
-        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
+        absoluteEncoderOffset = new Rotation2d(1.617); // MUST BE CALIBRATED
         break;
       case 1:
         m_driveTalon = new TalonFX(Ports.kFrontRightDrive, Ports.kCanivoreName);
         m_turnTalon = new TalonFX(Ports.kFrontRightTurn, Ports.kCanivoreName);
         m_cancoder = new CANcoder(Ports.kFrontRightCancoder, Ports.kCanivoreName);
-        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
+        absoluteEncoderOffset = new Rotation2d(2.750); // MUST BE CALIBRATED
         break;
       case 2:
         m_driveTalon = new TalonFX(Ports.kBackLeftDrive, Ports.kCanivoreName);
         m_turnTalon = new TalonFX(Ports.kBackLeftTurn, Ports.kCanivoreName);
         m_cancoder = new CANcoder(Ports.kBackLeftCancoder, Ports.kCanivoreName);
-        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
+        absoluteEncoderOffset = new Rotation2d(-0.948); // MUST BE CALIBRATED
         break;
       case 3:
         m_driveTalon = new TalonFX(Ports.kBackRightDrive, Ports.kCanivoreName);
         m_turnTalon = new TalonFX(Ports.kBackRightTurn, Ports.kCanivoreName);
         m_cancoder = new CANcoder(Ports.kBackRightCancoder, Ports.kCanivoreName);
-        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
+        absoluteEncoderOffset = new Rotation2d(2.049); // MUST BE CALIBRATED
         break;
       default:
         throw new RuntimeException("Invalid module index");

src/main/java/frc/robot/subsystems/kicker/Kicker.java

@@ -14,10 +14,14 @@ public class Kicker extends SubsystemBase {
 
   private Timer m_shootTimeout = new Timer();
   private Timer m_intakeTimeout = new Timer();
+  private Timer m_indexTimeout = new Timer();
+  private Timer m_reverseTimeout = new Timer();
 
   public enum KickerState {
     kIdle,
     kIntaking,
+    kIndexing,
+    kReversing,
     kShooting,
     kVomitting
   }
@@ -29,6 +33,8 @@ public Kicker(KickerIO io) {
     HashMap<Enum<?>, Runnable> periodicHash = new HashMap<>();
     periodicHash.put(KickerState.kIdle, this::idlePeriodic);
     periodicHash.put(KickerState.kIntaking, this::intakingPeriodic);
+    periodicHash.put(KickerState.kIndexing, this::indexingPeriodic);
+    periodicHash.put(KickerState.kReversing, this::reversingPeriodic);
     periodicHash.put(KickerState.kShooting, this::shootingPeriodic);
     periodicHash.put(KickerState.kVomitting, this::vomittingPeriodic);
     m_profiles = new SubsystemProfiles(KickerState.class, periodicHash, KickerState.kIdle);
@@ -53,7 +59,25 @@ private void intakingPeriodic() {
 
     if (m_io.hasGamePiece() || m_intakeTimeout.hasElapsed(KickerConstants.kIntakeTimeout.get())) {
       updateState(KickerState.kIdle);
-      m_io.setVoltage(KickerConstants.kIdleVoltage.get());
+      idlePeriodic();
+    }
+  }
+
+  private void indexingPeriodic() {
+    m_io.setVoltage(KickerConstants.kIndexingVoltage.get());
+
+    if (m_io.hasGamePiece() || m_indexTimeout.hasElapsed(KickerConstants.kIndexingVoltage.get())) {
+      updateState(KickerState.kReversing);
+      reversingPeriodic();
+    }
+  }
+
+  private void reversingPeriodic() {
+    m_io.setVoltage(KickerConstants.kReversingVoltage.get());
+
+    if (m_reverseTimeout.hasElapsed(KickerConstants.kReverseTimeout.get())) {
+      updateState(KickerState.kIdle);
+      idlePeriodic();
     }
   }
 
@@ -77,6 +101,12 @@ public void updateState(KickerState state) {
       case kIntaking:
         m_intakeTimeout.restart();
         break;
+      case kIndexing:
+        m_indexTimeout.restart();
+        break;
+      case kReversing:
+        m_reverseTimeout.restart();
+        break;
       case kShooting:
         m_shootTimeout.restart();
         break;
@@ -91,4 +121,5 @@ public void updateState(KickerState state) {
   // Mr. Wood better than u - Aahil
   // I love God and Jesus - Patty lin
   // James and Tommy better than you guys cause you're stinky and rotund like a clock
+  // Ya'll need to get a life :) - also add a v8 with a turbo and a supercharger
 }

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

@@ -53,6 +53,12 @@ public Shooter(
 
   @Override
   public void periodic() {
+    if (ShooterConstants.kManualControl) {
+      updateState(ShooterState.kRevving);
+      setDesiredVelocity(
+          ShooterConstants.kManualTopVelocity.get(), ShooterConstants.kManualBottomVelocity.get());
+    }
+
     m_io.updateInputs(m_inputs);
 
     m_profiles.getPeriodicFunction().run();