More automation and new IDs

lib/RoboLib

@@ -1 +1 @@
-Subproject commit eea76939a3cbb76aba47874adf15eaf49a8333de
+Subproject commit eeccb7531c6bddd4470d1e13804b42b279fdf7ee

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

@@ -4,11 +4,7 @@
 
 package frc.robot;
 
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.units.Unit;
 import edu.wpi.first.units.measure.Angle;
-import java.security.PublicKey;
-import java.util.Queue;
 
 import com.ctre.phoenix6.signals.InvertedValue;
 
@@ -25,6 +21,16 @@ public final class Constants {
     public static final int kDriverControllerPort = 0;
   }
 
+  public static class CameraConstanst {
+    public static final String BACK_CAMERA_NAME = "back";
+    public static final int BACK_LOCALIZATION_PIPELINE_NUMBER = 0;
+    public static final int BACK_HUB_TRACKING_PIPELINE_NUMBER = 1;
+
+    public static final String FRONT_CAMERA_NAME = "front";
+    public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;
+  
+  }
+
   public static class AdvantageKitConstants{
     public enum RobotType{
       SIM,
@@ -32,7 +38,6 @@ public final class Constants {
       REPLAY
     }
     public static final RobotType CURRENT_MODE = RobotType.REAL; 
-  
   }
   // Swerve offsets as of 3/2/26
   public static class SwerveDriveOffsets {
@@ -72,7 +77,7 @@ public final class Constants {
     public static final int DEPLOY_MOTOR_ID = 17;
     public static final boolean DEPLOY_MOTOR_INVERTED = false; // TODO: Test this
 
-    public static final int INTAKE_MOTOR_ID = 0; 
+    public static final int INTAKE_MOTOR_ID = 2; 
     public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this
   }
 
@@ -80,7 +85,7 @@ public final class Constants {
     public static final int LAUNCHER_MOTOR_ID = 2;
     public static final boolean LAUNCHER_MOTOR_INVERTED = true; // TODO: Test this
 
-    public static final int TURRET_MOTOR_ID = 0;
+    public static final int TURRET_MOTOR_ID = 3;
     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);
@@ -88,7 +93,7 @@ public final class Constants {
     public static final double HOMING_SPEED = .35;
     public static final double TURRET_GEAR_RATIO  = 0;
 
-    public static final int HOOD_SERVO_CHANNEL = 9;
+    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 Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(.5);
@@ -103,10 +108,9 @@ public final class Constants {
   
   public static class HopperConstants {
     public static final int KICKER_MOTOR_ID = 9;
-    public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
+    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.CounterClockwise_Positive;
+    public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
-    public static final double SPINDEXER_MOTOR_SPEED = .6;
   }
 }

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

@@ -79,9 +79,6 @@ public class RobotContainer {
 
     m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand());
     m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand());
-    m_driverController.b().onTrue(intakeSubsystem.revSpeedCommand());
-    m_driverController.b().onTrue(hopperSubsystem.revSpeedCommand());
-    m_driverController.b().onTrue(turretSubsystem.revSpeedCommand());
 
     intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID()));
   }

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

@@ -0,0 +1,75 @@
+package frc.robot.commands;
+
+import java.lang.constant.Constable;
+
+import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
+import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers.LimelightTarget_Fiducial;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.HopperSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.TurretSubsystem;
+
+public class AutoShootCommandChassisTurret extends Command{
+    
+    // For Chassis Control
+    private DriveSubsystem driveSubsystem;
+    
+    private HopperSubsystem hopperSubsystem;
+    private IntakeSubsystem intakeSubsystem;
+    private TurretSubsystem turretSubsystem;
+
+    private PIDController autoCenterController;
+
+    public AutoShootCommandChassisTurret(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, IntakeSubsystem intakeSubsystem, TurretSubsystem turretSubsystem)
+    {
+        this.driveSubsystem = driveSubsystem;
+        this.hopperSubsystem = hopperSubsystem;
+        this.intakeSubsystem = intakeSubsystem;
+        this.turretSubsystem = turretSubsystem;
+
+        autoCenterController = new PIDController(0, 0, 0);
+        autoCenterController.setTolerance(1);
+        autoCenterController.setSetpoint(0);
+    }
+
+    @Override
+    public void initialize() {
+        turretSubsystem.setLauncher(2500);
+        hopperSubsystem.spindexerOn();
+    }
+
+    @Override
+    public void execute() {
+        // Need to compute shoot speed and angle
+
+        // Get apriltags
+        // Change to HUB tracking pipeline
+        LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_PIPELINE_NUMBER);
+
+        LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME);
+
+        if(turretSubsystem.isLauncherUpToSpeed() && autoCenterController.atSetpoint()){
+            hopperSubsystem.feedKicker();
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        turretSubsystem.setLauncher(0);
+
+        hopperSubsystem.stopKicker();
+        hopperSubsystem.spindexerOff();
+
+        LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER);
+    }
+
+}

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();;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -31,19 +31,19 @@ public class ManualHopperCommand extends Command {
   public void execute() {
     if(m_xboxController.getLeftTriggerAxis()>.5 && m_xboxController.getRightTriggerAxis()>.2)
     {
-      m_subsystem.feed();
+      m_subsystem.feedKicker();
     }
     else
     {
-      m_subsystem.stop();
+      m_subsystem.stopKicker();
     }
     if(m_xboxController.getRightBumper())
     {
-      m_subsystem.spin(Constants.HopperConstants.SPINDEXER_MOTOR_SPEED);
+      m_subsystem.spindexerOn();
     }
     if(m_xboxController.getLeftBumper())
     {
-      m_subsystem.spin(0);
+      m_subsystem.spindexerOff();
     }
   }
 

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

@@ -28,26 +28,20 @@ public class HopperSubsystem extends TestableSubsystem {
     SetMotorDirection(kickerMotor, Constants.HopperConstants.KICKER_MOTOR_DIRECTION);
   } 
 
-  public void feed() {kickerMotor.set(0.5);}
+  public void feedKicker() {kickerMotor.set(0.5);}
 
-  public void reverse() {kickerMotor.set(-0.5);} 
+  public void reverseKicker() {kickerMotor.set(-0.5);} 
 
-  public void stop() {kickerMotor.set(0);}
+  public void stopKicker() {kickerMotor.stopMotor();}
 
-  public void spin(double speed) {spindexerMotor.set(speed);}
+  public void spindexerOn() { spindexerMotor.set(0.5);}
 
-  public Command revSpeedCommand()
+  public void spindexerOff() { spindexerMotor.stopMotor();}
-  {
-    return this.runOnce(() -> reverse());
-  }
-
-  @PostTest(name = "A friendly name", enabled = true)
-  public void myFailingPostTest(){
-    assertThat(true).isFalse();
-  }
 
   @PostTest(enabled = true)
-  public void myPassingPostTest(){
+  public void hopperSystem_CANDevicesConnected(){
-    assertThat("Hello World").endWith("World");
+    assertThat(kickerMotor.isConnected()).isTrue().as("Kicker motor not connected!");
+    assertThat(spindexerMotor.isConnected()).isTrue().as("Spindexer motor not connected!");
   }
+
 }

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

@@ -61,4 +61,10 @@ public class IntakeSubsystem extends TestableSubsystem {
   {
     return this.runOnce(() -> setSpeed(.5));
   }
+
+  @PostTest(enabled = true)
+  public void IntakeSubsystem_CANDevicesConnected(){
+    assertThat(intakeMotor.isConnected()).isTrue().as("Intake motor not connected!");
+    assertThat(deployMotor.isAlive()).isTrue().as("Deploy controller not connected!");
+  }
 }

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

@@ -5,24 +5,28 @@
 package frc.robot.subsystems;
 
 import org.fairportrobotics.frc.posty.TestableSubsystem;
+import org.fairportrobotics.frc.posty.test.PostTest;
+import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
 
-import com.ctre.phoenix6.controls.ControlRequest;
-import com.ctre.phoenix6.controls.DutyCycleOut;
-import com.ctre.phoenix6.controls.PositionDutyCycle;
 import com.ctre.phoenix6.controls.PositionVoltage;
-import com.ctre.phoenix6.controls.VelocityDutyCycle;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
-import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.revrobotics.PersistMode;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 import frc.robot.Constants;
-import static org.fairportrobotics.frc.robolib.motors.Utils.*;
-
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.Velocity;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -38,21 +42,46 @@ public class TurretSubsystem extends TestableSubsystem {
   private DigitalInput turretLimitSwitch;
   private TalonFX turretMotor;
   private SparkMax launcherMotor;
+  private SparkClosedLoopController launcherControler;
+
   private Servo hood;
   private Angle limitPos;
   private Angle limitNeg;
   private boolean turretGoingPos;
 
   public TurretSubsystem() {
-    turretLimitSwitch = new DigitalInput(Constants.ShooterConstants.TURRET_LIMIT_CHANNEL);
-    //turretMotor = new TalonFX(Constants.ShooterConstants.TURRET_MOTOR_ID);
-    //SetMotorDirection(turretMotor, Constants.ShooterConstants.TURRET_MOTOR_DIRECTION);
 
+    // Turret configuration
+    turretLimitSwitch = new DigitalInput(Constants.ShooterConstants.TURRET_LIMIT_CHANNEL);
+    turretMotor = new TalonFX(Constants.ShooterConstants.TURRET_MOTOR_ID);
+    turretMotor.getConfigurator().apply(
+      new TalonFXConfiguration()
+        .withSlot0(
+          new Slot0Configs()
+            .withKP(1.0)
+            .withKI(0)
+            .withKD(0)
+        )
+        .withMotorOutput(
+          new MotorOutputConfigs()
+            .withInverted(InvertedValue.Clockwise_Positive)
+        )
+        .withCurrentLimits(
+          new CurrentLimitsConfigs()
+        )
+    );
+
+    // Launcher configuration
     launcherMotor = new SparkMax(Constants.ShooterConstants.LAUNCHER_MOTOR_ID, MotorType.kBrushless);
     SparkMaxConfig config = new SparkMaxConfig()
-      .apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO_2);
+      .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.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
+    launcherMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    launcherControler = launcherMotor.getClosedLoopController();
 
+    // Hood configuration
     hood = new Servo(Constants.ShooterConstants.HOOD_SERVO_CHANNEL);
     currentState = TurretState.INIT;
     limitNeg = Constants.ShooterConstants.LIMIT_AXIMUTH_NEG;
@@ -60,7 +89,11 @@ public class TurretSubsystem extends TestableSubsystem {
     turretGoingPos = false;
   }
 
-  public void setLauncher(double speed) {launcherMotor.set(speed);}
+  public void setLauncher(double speed) {launcherControler.setSetpoint(speed, ControlType.kVelocity);}
+
+  public boolean isLauncherUpToSpeed() {
+    return launcherControler.isAtSetpoint();
+  }
 
   public void turretControl(double position)
   {
@@ -108,8 +141,7 @@ public class TurretSubsystem extends TestableSubsystem {
 
   public Angle getTurretAngle()
   {
-    // return turretMotor.getPosition().refresh().getValue().times(Constants.ShooterConstants.TURRET_GEAR_RATIO);
+    return turretMotor.getPosition().refresh().getValue().times(Constants.ShooterConstants.TURRET_GEAR_RATIO);
-    return Units.Degrees.of(0.0);
   }
 
   @Override
@@ -147,4 +179,11 @@ public class TurretSubsystem extends TestableSubsystem {
       //turretMotor.set(0);
     }*/
   }
+
+  @PostTest()
+  public void TurretSubsystem_CANDevicesConnected(){
+    assertThat(launcherMotor.getDeviceId()).isGreaterThan(0).as("Launcher motor not connected!");
+    assertThat(turretMotor.isConnected()).isTrue().as("Turret motor not connected!");
+  }
+
 }