Systems check: All good

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

@@ -75,28 +75,28 @@ public final class Constants {
   
   public static class IntakeConstants {
     public static final int DEPLOY_MOTOR_ID = 17;
-    public static final boolean DEPLOY_MOTOR_INVERTED = false; // TODO: Test this
+    public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this
 
     public static final int INTAKE_MOTOR_ID = 2; 
     public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this
   }
 
   public static class ShooterConstants {
-    public static final int LAUNCHER_MOTOR_ID = 2;
+    public static final int LAUNCHER_MOTOR_ID = 3;
-    public static final boolean LAUNCHER_MOTOR_INVERTED = true; // TODO: Test this
+    public static final boolean LAUNCHER_MOTOR_INVERTED = false;
 
-    public static final int TURRET_MOTOR_ID = 3;
+    public static final int TURRET_MOTOR_ID = 4;
     public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
     public static final int TURRET_LIMIT_CHANNEL = 0;
     public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(265);
     public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5);
     public static final double HOMING_SPEED = .35;
-    public static final double TURRET_GEAR_RATIO  = 0;
+    public static final double TURRET_GEAR_RATIO  = 1;
 
     public static final int HOOD_SERVO_CHANNEL = 0;
     public static final boolean HOOD_SERVO_INVERTED = true; // TODO: Test this
-    public static final int HOOD_ENCODER_ID = 0;
+    public static final int HOOD_ENCODER_ID = 4;
-    public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(.5);
+    public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1);
     public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90);
     public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0);
     public static final Angle DEFLECTOR_STORED_ANGLE = edu.wpi.first.units.Units.Degrees.of(0);
@@ -111,6 +111,6 @@ public final class Constants {
     public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
 
     public static final int SPINDEXER_MOTOR_ID = 8;
-    public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
+    public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
   }
 }

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

@@ -4,18 +4,11 @@
 
 package frc.robot;
 
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.auto.AutoBuilderException;
-
-import edu.wpi.first.units.Unit;
-import edu.wpi.first.units.Units;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.OperatorConstants;
-import frc.robot.commands.Autos;
+import frc.robot.commands.AutoShootCommandChassisTurret;
 import frc.robot.commands.ManualHopperCommand;
 import frc.robot.commands.ManualIntake;
 import frc.robot.commands.ManualShootCommand;
@@ -39,8 +32,6 @@ public class RobotContainer {
 
   // private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort);
 
-  private final SendableChooser<Command> autoChooser;
-
   public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController);
 
   public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
@@ -51,8 +42,6 @@ public class RobotContainer {
   
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
-    autoChooser = AutoBuilder.buildAutoChooser();
-    SmartDashboard.putData("Auto Chooser", autoChooser);
 
     // Configure the trigger bindings
     configureBindings();
@@ -80,6 +69,8 @@ public class RobotContainer {
     m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand());
     m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand());
 
+    m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, intakeSubsystem, turretSubsystem));
+
     intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID()));
   }
 
@@ -90,6 +81,6 @@ public class RobotContainer {
    */
   public Command getAutonomousCommand() {
     // An example command will be run in autonomous
-    return autoChooser.getSelected();
+    return driveSubsystem.getAutoCommand();
   }
 }

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

@@ -31,7 +31,7 @@ public class AutoShootCommandChassisTurret extends Command{
         this.intakeSubsystem = intakeSubsystem;
         this.turretSubsystem = turretSubsystem;
 
-        autoCenterController = new PIDController(0, 0, 0);
+        autoCenterController = new PIDController(0.1, 0, 0);
         autoCenterController.setTolerance(1);
         autoCenterController.setSetpoint(0);
     }
@@ -52,6 +52,8 @@ public class AutoShootCommandChassisTurret extends Command{
 
         LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME);
 
+        // driveSubsystem. autoCenterController.calculate(0);
+
         if(turretSubsystem.isLauncherUpToSpeed() && autoCenterController.atSetpoint()){
             hopperSubsystem.feedKicker();
         }

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

@@ -23,7 +23,7 @@ public class ManualHopperCommand extends Command {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    m_subsystem.spindexerOn();;
+    m_subsystem.spindexerOn();
   }
 
   // Called every time the scheduler runs while the command is scheduled.

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

@@ -27,17 +27,17 @@ public class ManualShootCommand extends Command {
   public void execute() {
     if(m_controller.getRightTriggerAxis()>.8)
     {
-      m_subsystem.setLauncher(1);
+      m_subsystem.setLauncher(5000);
       m_controller.setRumble(RumbleType.kBothRumble, 1);
     }
     else if(m_controller.getRightTriggerAxis()>.5)
     {
-      m_subsystem.setLauncher(.7);
+      m_subsystem.setLauncher(3000);
       m_controller.setRumble(RumbleType.kBothRumble, .67);
     }
     else if(m_controller.getRightTriggerAxis()>.2)
     {
-      m_subsystem.setLauncher(.5);
+      m_subsystem.setLauncher(2000);
       m_controller.setRumble(RumbleType.kBothRumble, .33);
     }
     else

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

@@ -169,8 +169,9 @@ public class DriveSubsystem extends TestableSubsystem {
                     },
                     this);
 
-            autoChooser = AutoBuilder.buildAutoChooser();
+            // autoChooser = AutoBuilder.buildAutoChooser();
-            SmartDashboard.putData("Auto Chooser", autoChooser);
+            // autoChooser.addOption("Do Nothing", Commands.none());
+            // SmartDashboard.putData("Auto Chooser", autoChooser);
 
         } catch (Exception e) {
             e.printStackTrace();
@@ -191,7 +192,8 @@ public class DriveSubsystem extends TestableSubsystem {
     }
 
     public Command getAutoCommand(){
-        return autoChooser.getSelected();
+        return Commands.none();
+        // return autoChooser.getSelected();
     }
 
     @PostTest()

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

@@ -7,8 +7,10 @@ package frc.robot.subsystems;
 import org.fairportrobotics.frc.posty.TestableSubsystem;
 import org.fairportrobotics.frc.posty.test.PostTest;
 
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.hardware.TalonFX;
 
+import edu.wpi.first.cscore.VideoSource.ConnectionStrategy;
 import edu.wpi.first.wpilibj2.command.Command;
 
 import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
@@ -22,10 +24,10 @@ public class HopperSubsystem extends TestableSubsystem {
   /** Creates a new HopperSubsystem. */
   public HopperSubsystem() {
     spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID);
-    SetMotorDirection(spindexerMotor, Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION);
+    spindexerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION));
 
     kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID);
-    SetMotorDirection(kickerMotor, Constants.HopperConstants.KICKER_MOTOR_DIRECTION);
+    kickerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION));
   } 
 
   public void feedKicker() {kickerMotor.set(0.5);}
@@ -40,8 +42,8 @@ public class HopperSubsystem extends TestableSubsystem {
 
   @PostTest(enabled = true)
   public void hopperSystem_CANDevicesConnected(){
-    assertThat(kickerMotor.isConnected()).isTrue().as("Kicker motor not connected!");
+    assertThat(kickerMotor.isConnected()).as("Kicker motor not connected!").isTrue();
-    assertThat(spindexerMotor.isConnected()).isTrue().as("Spindexer motor not connected!");
+    assertThat(spindexerMotor.isConnected()).as("Spindexer motor not connected!").isTrue();
   }
 
 }

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

@@ -64,7 +64,7 @@ public class IntakeSubsystem extends TestableSubsystem {
 
   @PostTest(enabled = true)
   public void IntakeSubsystem_CANDevicesConnected(){
-    assertThat(intakeMotor.isConnected()).isTrue().as("Intake motor not connected!");
+    assertThat(intakeMotor.isConnected()).as("Intake motor not connected!").isTrue();
-    assertThat(deployMotor.isAlive()).isTrue().as("Deploy controller not connected!");
+    assertThat(deployMotor.isAlive()).as("Deploy controller not connected!").isTrue();
   }
 }

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

@@ -6,6 +6,8 @@ package frc.robot.subsystems;
 
 import org.fairportrobotics.frc.posty.TestableSubsystem;
 import org.fairportrobotics.frc.posty.test.PostTest;
+import org.littletonrobotics.junction.Logger;
+
 import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
 
 import com.ctre.phoenix6.controls.PositionVoltage;
@@ -38,6 +40,8 @@ public class TurretSubsystem extends TestableSubsystem {
     MANUAL;
   }
 
+  private final boolean LOGGING = true;
+
   private TurretState currentState;
   private DigitalInput turretLimitSwitch;
   private TalonFX turretMotor;
@@ -76,7 +80,7 @@ public class TurretSubsystem extends TestableSubsystem {
     SparkMaxConfig config = new SparkMaxConfig()
       .apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO);
     config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED);
-    config.closedLoop.p(1).i(0).d(0).outputRange(0, 5000);
+    config.closedLoop.p(1).i(0).d(0.1).outputRange(0, 5000);
     config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
     launcherMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
     launcherControler = launcherMotor.getClosedLoopController();
@@ -158,6 +162,12 @@ public class TurretSubsystem extends TestableSubsystem {
       default:
         break;
     }
+
+    if(LOGGING)
+    {
+      Logger.recordOutput("Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
+    }
+
   }
 
   private void periodicHoming()
@@ -182,8 +192,8 @@ public class TurretSubsystem extends TestableSubsystem {
 
   @PostTest()
   public void TurretSubsystem_CANDevicesConnected(){
-    assertThat(launcherMotor.getDeviceId()).isGreaterThan(0).as("Launcher motor not connected!");
+    assertThat(launcherMotor.getDeviceId()).as("Launcher motor not connected!").isGreaterThan(0);
-    assertThat(turretMotor.isConnected()).isTrue().as("Turret motor not connected!");
+    assertThat(turretMotor.isConnected()).as("Turret motor not connected!").isTrue();
   }
 
 }