Subsystem/turret crt

src/main/deploy/choreo/test.traj

@@ -0,0 +1,32 @@
+{
+ "name":"test",
+ "version":3,
+ "snapshot":{
+  "waypoints":[],
+  "constraints":[],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"0.8321799635887146 m", "val":0.8321799635887146}, "y":{"exp":"0.7060884237289429 m", "val":0.7060884237289429}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"4.659694671630859 m", "val":4.659694671630859}, "y":{"exp":"0.6415539979934692 m", "val":0.6415539979934692}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"2.725978374481201 m", "val":2.725978374481201}, "y":{"exp":"4.064524173736572 m", "val":4.064524173736572}, "heading":{"exp":"-33.321136223151115 mrad", "val":-0.03332113622315112}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"2.7410621643066406 m", "val":2.7410621643066406}, "y":{"exp":"5.301389694213867 m", "val":5.301389694213867}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":null,
+  "sampleType":null,
+  "waypoints":[],
+  "samples":[],
+  "splits":[]
+ },
+ "events":[]
+}

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

@@ -29,6 +29,8 @@
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Superstructure.SuperState;
+import frc.robot.components.cancoder.CANcoderIO;
+import frc.robot.components.cancoder.CANcoderIOSim;
 import frc.robot.components.canrange.CANrangeIOReal;
 import frc.robot.components.rollers.RollerIO;
 import frc.robot.components.rollers.RollerIOSim;
@@ -51,6 +53,8 @@
 import frc.robot.subsystems.shooter.HoodIOSim;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.ShooterSubsystem;
+import frc.robot.subsystems.shooter.TurretIO;
+import frc.robot.subsystems.shooter.TurretIOSim;
 import frc.robot.subsystems.shooter.TurretSubsystem;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
 import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread;
@@ -349,7 +353,18 @@ public Robot() {
                 ROBOT_MODE == RobotMode.REAL
                     ? new HoodIO(HoodIO.getCompHood(), canivore, 11)
                     : new HoodIOSim(
-                        canivore, HoodIO.getCompHood(), TurretSubsystem.HOOD_GEAR_RATIO, 11));
+                        canivore, HoodIO.getCompHood(), TurretSubsystem.HOOD_GEAR_RATIO, 11),
+                ROBOT_MODE == RobotMode.REAL
+                    // TODO id's
+                    ? new TurretIO()
+                    : new TurretIOSim(),
+                ROBOT_MODE == RobotMode.REAL
+                    ? new CANcoderIO(30, TurretSubsystem.getCancoder24tConfigs(), canivore)
+                    : new CANcoderIOSim(30, TurretSubsystem.getCancoder24tConfigs(), canivore),
+                ROBOT_MODE == RobotMode.REAL
+                    ? new CANcoderIO(30, TurretSubsystem.getCancoder26tConfigs(), canivore)
+                    : new CANcoderIOSim(30, TurretSubsystem.getCancoder26tConfigs(), canivore));
+        // TODO climber
         break;
     }
     climber =
@@ -471,10 +486,11 @@ public Robot() {
             })
         .onTrue(
             Commands.runOnce(() -> addAutos())
-                .alongWith(leds.blinkCmd(Color.kWhite, Color.kBlack, 20.0).withTimeout(1.0))
-                .ignoringDisable(true));
+                .alongWith(
+                    leds.blinkCmd(Color.kWhite, Color.kBlack, 20.0)
+                        .withTimeout(1.0)
+                        .ignoringDisable(true)));
     // TODO tbh idk if the leds will work here
-
     // Add autos when first connecting to DS
     new Trigger(
             () ->

src/main/java/frc/robot/Superstructure.java

@@ -284,6 +284,7 @@ private void addCommands() {
         intake.rest(),
         indexer.rest(),
         shooter.score(
+            () -> FeedTargets.getFeedTarget(feedTarget).getPose(),
             () ->
                 AutoAim.getCompensatedSOTMShotData(
                     swerve.getPose(),
@@ -296,6 +297,7 @@ private void addCommands() {
         intake.rest(),
         indexer.kick(),
         shooter.score(
+            () -> FeedTargets.getFeedTarget(feedTarget).getPose(),
             () ->
                 AutoAim.getCompensatedSOTMShotData(
                     swerve.getPose(),
@@ -308,6 +310,7 @@ private void addCommands() {
         intake.rest(),
         indexer.kick(),
         shooter.score(
+            () -> FeedTargets.getFeedTarget(feedTarget).getPose(),
             () ->
                 AutoAim.getCompensatedSOTMShotData(
                     swerve.getPose(),
@@ -319,6 +322,7 @@ private void addCommands() {
         intake.intake(),
         indexer.kick(),
         shooter.score(
+            () -> FeedTargets.getFeedTarget(feedTarget).getPose(),
             () ->
                 AutoAim.getCompensatedSOTMShotData(
                     swerve.getPose(),

src/main/java/frc/robot/components/cancoder/CANcoderIO.java

@@ -4,17 +4,41 @@
 
 package frc.robot.components.cancoder;
 
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.hardware.CANcoder;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.measure.Angle;
 import org.littletonrobotics.junction.AutoLog;
 
 // for cancoders that aren't on a swerve module (eg arm, intake)
-public interface CANcoderIO {
-  // TODO wherever you use this, create an alert for connected or not
+public class CANcoderIO {
+
   @AutoLog
   public static class CANcoderIOInputs {
     public boolean connected = false;
     public Rotation2d cancoderPositionRotations = new Rotation2d();
   }
 
-  public void updateInputs(CANcoderIOInputs inputs);
+  protected final CANcoder cancoder;
+
+  private final StatusSignal<Angle> cancoderAbsolutePositionRotations;
+
+  public CANcoderIO(int cancoderID, CANcoderConfiguration config, CANBus canbus) {
+    cancoder = new CANcoder(cancoderID, canbus);
+    cancoderAbsolutePositionRotations = cancoder.getAbsolutePosition();
+    BaseStatusSignal.setUpdateFrequencyForAll(50.0, cancoderAbsolutePositionRotations);
+    cancoder.getConfigurator().apply(config);
+    cancoder.optimizeBusUtilization();
+  }
+
+  public void updateInputs(CANcoderIOInputs inputs) {
+    BaseStatusSignal.refreshAll(cancoderAbsolutePositionRotations);
+
+    inputs.connected = BaseStatusSignal.isAllGood(cancoderAbsolutePositionRotations);
+    inputs.cancoderPositionRotations =
+        Rotation2d.fromRotations(cancoderAbsolutePositionRotations.getValueAsDouble());
+  }
 }

src/main/java/frc/robot/components/cancoder/CANcoderIOSim.java

@@ -0,0 +1,27 @@
+package frc.robot.components.cancoder;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.sim.CANcoderSimState;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public class CANcoderIOSim extends CANcoderIO {
+  private final CANcoderSimState sim;
+
+  private double positionRotations;
+
+  public CANcoderIOSim(int cancoderID, CANcoderConfiguration config, CANBus canbus) {
+    super(cancoderID, config, canbus);
+    this.sim = cancoder.getSimState();
+  }
+
+  public void setSimValues(double positionRotations) {
+    this.positionRotations = positionRotations;
+  }
+
+  public void updateInputs(CANcoderIOInputs inputs) {
+    sim.setRawPosition(positionRotations);
+
+    inputs.cancoderPositionRotations = Rotation2d.fromRotations(positionRotations);
+  }
+}

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

@@ -18,7 +18,7 @@ public interface Shooter extends Subsystem {
    * Sets hood angle and flywheel velocity based on distance from hub from the shot map + current
    * pose
    */
-  public Command score(Supplier<ShotData> shotDataSupplier);
+  public Command score(Supplier<Pose2d> robotPoseSupplier, Supplier<ShotData> shotDataSupplier);
 
   /**
    * Sets hood angle and flywheel velocity based on distance from hub from the feed map + current

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

@@ -86,7 +86,7 @@ public Command testShoot() {
         });
   }
 
-  public Command score(Supplier<ShotData> shotDataSupplier) {
+  public Command score(Supplier<Pose2d> robotPoseSupplier, Supplier<ShotData> shotDataSupplier) {
     return this.run(
         () -> {
           hoodSetpoint = shotDataSupplier.get().hoodAngle();

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

@@ -0,0 +1,121 @@
+package frc.robot.subsystems.shooter;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Temperature;
+import edu.wpi.first.units.measure.Voltage;
+import org.littletonrobotics.junction.AutoLog;
+import org.littletonrobotics.junction.AutoLogOutput;
+
+public class TurretIO {
+  public static double TURRET_GEAR_RATIO = (42.0 / 12.0) * (32.0 / 16.0) * (85.0 / 10.0);
+
+  public static double CANCODER_ONE_TO_TURRET_GEAR_RATIO = (24.0 / 32.0) * (10.0 / 85.0);
+
+  // idk
+  public static Rotation2d TURRET_MIN_ROTATIONS = Rotation2d.fromRotations(0.0);
+  public static Rotation2d TURRET_MAX_ROTATIONS = Rotation2d.fromRotations(0.8);
+
+  // todo ID?
+  protected final TalonFX motor = new TalonFX(40, "*");
+
+  @AutoLog
+  public static class TurretIOInputs {
+    public double angularVelocityRotationsPerSec = 0.0;
+    public Rotation2d positionRotations = new Rotation2d();
+    public double statorCurrentAmps = 0.0;
+    public double supplyCurrentAmp = 0.0;
+    public double voltage = 0.0;
+    public double tempCelsius = 0.0;
+  }
+
+  private final StatusSignal<AngularVelocity> angularVelocityRotationsPerSec = motor.getVelocity();
+  private final StatusSignal<Angle> positionRotations = motor.getPosition();
+  private final StatusSignal<Current> supplyCurrentAmps = motor.getSupplyCurrent();
+  private final StatusSignal<Current> statorCurrentAmps = motor.getStatorCurrent();
+  private final StatusSignal<Voltage> voltage = motor.getMotorVoltage();
+  private final StatusSignal<Temperature> tempCelcius = motor.getDeviceTemp();
+
+  private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true);
+  private MotionMagicVoltage motionMagic = new MotionMagicVoltage(0.0);
+
+  // todo
+  private Rotation2d turretSetpoint = Rotation2d.kZero;
+
+  public TurretIO() {
+
+    final TalonFXConfiguration config = new TalonFXConfiguration();
+
+    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+    config.Feedback.SensorToMechanismRatio = TURRET_GEAR_RATIO;
+    config.CurrentLimits.StatorCurrentLimit = 80.0;
+    config.CurrentLimits.StatorCurrentLimitEnable = true;
+    config.CurrentLimits.SupplyCurrentLimit = 60.0;
+
+    config.Slot0.kS = 0;
+    config.Slot0.kG = 0;
+    config.Slot0.kV = 0;
+    config.Slot0.kP = 0;
+    config.Slot0.kD = 0;
+
+    motor.getConfigurator().apply(config);
+
+    BaseStatusSignal.setUpdateFrequencyForAll(
+        50.0,
+        angularVelocityRotationsPerSec,
+        positionRotations,
+        voltage,
+        statorCurrentAmps,
+        supplyCurrentAmps,
+        tempCelcius);
+    motor.optimizeBusUtilization();
+  }
+
+  public void updateInputs(TurretIOInputs inputs) {
+    BaseStatusSignal.refreshAll(
+        positionRotations,
+        angularVelocityRotationsPerSec,
+        voltage,
+        statorCurrentAmps,
+        supplyCurrentAmps,
+        tempCelcius);
+
+    inputs.positionRotations = Rotation2d.fromRotations(positionRotations.getValueAsDouble());
+    inputs.angularVelocityRotationsPerSec = angularVelocityRotationsPerSec.getValueAsDouble();
+    inputs.voltage = voltage.getValueAsDouble();
+    inputs.statorCurrentAmps = statorCurrentAmps.getValueAsDouble();
+    inputs.supplyCurrentAmp = supplyCurrentAmps.getValueAsDouble();
+    inputs.tempCelsius = tempCelcius.getValueAsDouble();
+  }
+
+  public void setTurretPosition(Rotation2d positionAngle) {
+    turretSetpoint = positionAngle;
+    motor.setControl(
+        motionMagic.withPosition(
+            MathUtil.clamp(
+                positionAngle.getRotations(),
+                TURRET_MIN_ROTATIONS.getRotations(),
+                TURRET_MAX_ROTATIONS.getRotations())));
+  }
+
+  public void resetTurretPosition(Rotation2d turretRotation) {
+    motor.setPosition(turretRotation.getRotations());
+  }
+
+  @AutoLogOutput(key = "Shooter/Turret/Setpoint")
+  public Rotation2d getTurretSetpoint() {
+    return turretSetpoint;
+  }
+}