Subsystem/comp turret

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

@@ -6,6 +6,7 @@
 
 import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.SignalLogger;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.sim.TalonFXSimState.MotorType;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -45,6 +46,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;
@@ -277,7 +280,12 @@ public Robot() {
       case COMP:
         indexer = new SpindexerSubsystem();
         intake = new LintakeSubsystem();
-        shooter = new TurretSubsystem();
+        shooter = 
+        new TurretSubsystem(
+          ROBOT_MODE == RobotMode.REAL
+          ? new TurretIO(TurretIO.getTurretConfiguration(), canivore)
+          : new TurretIOSim(TurretIO.getTurretConfiguration(), canivore)
+        );
         climber = new ClimberSubsystem(); // TODO climber
         break;
     }

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

@@ -119,6 +119,10 @@ public Command spit() {
         }); // TODO: TUNE HOOD POS AND FLYWHEEL VELOCITY
   }
 
+  public Command setHoodPositionCommand(Supplier<Rotation2d> hoodPosition) {
+    return this.run(() -> hoodIO.setHoodPosition(hoodPosition.get()));
+  }
+
   @Override
   public void periodic() {
     hoodIO.updateInputs(hoodInputs);
@@ -135,7 +139,7 @@ public Command runHoodSysid() {
             .until(
                 () ->
                     hoodInputs.hoodPositionRotations.getDegrees()
-                        > (HOOD_MAX_ROTATION.getDegrees() - 5)), // Stop before endstop
+                        > (HOOD_MAX_ROTATION.getDegrees() - 5)),
         hoodSysid
             .quasistatic(Direction.kReverse)
             .until(

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

@@ -0,0 +1,123 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems.shooter;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+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.geometry.Rotation2d;
+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;
+
+/** Add your docs here. */
+public class TurretIO {
+  @AutoLog
+  public static class TurretIOInputs {
+    public Rotation2d turretPositionRotations = new Rotation2d();
+    public double turretStatorCurrentAmps = 0.0;
+    public double velocityRotationsPerSec  = 0.0;
+    public double turretSupplyCurrentAmps = 0.0;
+    public double turretVoltage = 0.0;
+    public double turretTempC = 0.0;
+  }
+
+  protected TalonFX turretMotor;
+  private final BaseStatusSignal turretPositionRotations;
+  private final StatusSignal<Voltage> turretVoltage;
+  private final StatusSignal<Current> turretStatorCurrent;
+  private final StatusSignal<Current> turretSupplyCurrent;
+  private final StatusSignal<Temperature> turretTemp;
+  private final StatusSignal<AngularVelocity> turretAngularVelocity;
+  private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true);
+  private PositionVoltage positionVoltage = new PositionVoltage(0.0).withEnableFOC(true);
+  private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true);
+
+  public TurretIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) {
+    turretMotor = new TalonFX(11, canbus);
+    turretMotor.getConfigurator().apply(TurretIO.getTurretConfiguration());
+    turretPositionRotations = turretMotor.getPosition();
+    turretAngularVelocity = turretMotor.getVelocity();
+    turretVoltage = turretMotor.getMotorVoltage();
+    turretStatorCurrent = turretMotor.getStatorCurrent();
+    turretSupplyCurrent = turretMotor.getSupplyCurrent();
+    turretTemp = turretMotor.getDeviceTemp();
+
+    BaseStatusSignal.setUpdateFrequencyForAll(
+        50.0,
+        turretPositionRotations,
+        turretAngularVelocity,
+        turretVoltage,
+        turretStatorCurrent,
+        turretSupplyCurrent,
+        turretTemp);
+    turretMotor.optimizeBusUtilization();
+  }
+
+  public static TalonFXConfiguration getTurretConfiguration() {
+    TalonFXConfiguration config = new TalonFXConfiguration();
+
+    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+    config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+
+    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+    config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+
+    config.Feedback.SensorToMechanismRatio = TurretSubsystem.TURRET_GEAR_RATIO;
+
+    // config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; Potentially need, maybe not tho.
+
+    config.Slot0.kS = 0.0;
+    config.Slot0.kG = 0.0;
+    config.Slot0.kV = 1.1;
+    config.Slot0.kP = 5.0;
+    config.Slot0.kD = 0.0;
+
+    config.CurrentLimits.StatorCurrentLimit = 80.0;
+    config.CurrentLimits.StatorCurrentLimitEnable = true;
+    config.CurrentLimits.SupplyCurrentLimit = 60.0;
+
+    return config;
+  }
+
+  public void setTurretVoltage(double turretVoltage) {
+    turretMotor.setControl(voltageOut.withOutput(turretVoltage));
+  }
+
+  public void setTurretPosition(Rotation2d turretPosition) {
+    turretMotor.setControl(positionVoltage.withPosition(turretPosition.getRotations()));
+  }
+
+  public void setTurretVelocity(double turretVelocity) {
+    turretMotor.setControl(velocityVoltage.withVelocity(turretVelocity));
+  }
+
+  public void updateInputs(TurretIOInputs inputs) {
+    BaseStatusSignal.refreshAll(
+        turretPositionRotations,
+        turretVoltage,
+        turretStatorCurrent,
+        turretSupplyCurrent,
+        turretTemp);
+
+    inputs.turretPositionRotations =
+        Rotation2d.fromRadians(turretPositionRotations.getValueAsDouble());
+    inputs.turretVoltage = turretVoltage.getValueAsDouble();
+    inputs.turretStatorCurrentAmps = turretStatorCurrent.getValueAsDouble();
+    inputs.turretSupplyCurrentAmps = turretSupplyCurrent.getValueAsDouble();
+    inputs.turretTempC = turretTemp.getValueAsDouble();
+  }
+}

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

@@ -0,0 +1,46 @@
+package frc.robot.subsystems.shooter;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.sim.ChassisReference;
+import com.ctre.phoenix6.sim.TalonFXSimState;
+import com.ctre.phoenix6.sim.TalonFXSimState.MotorType;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+
+public class TurretIOSim extends TurretIO{
+    DCMotorSim turretphysicssSim =
+    new DCMotorSim(
+        LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX44Foc(1), 0.001, TurretSubsystem.TURRET_GEAR_RATIO), 
+        DCMotor.getKrakenX44Foc(1));
+
+TalonFXSimState motorSim;
+final double simLoopPeriod = 0.002;
+Notifier simNotifier;
+double lastSimTime = 0.0;
+
+public TurretIOSim(TalonFXConfiguration configuration, CANBus canBus){
+    super(configuration, canBus);
+motorSim = turretMotor.getSimState();
+motorSim.setMotorType(MotorType.KrakenX44);
+motorSim.Orientation = ChassisReference.Clockwise_Positive;
+simNotifier = new Notifier(() -> {
+    double currentTime = Utils.getCurrentTimeSeconds();
+    double deltaTime = currentTime - lastSimTime;
+    lastSimTime = currentTime;
+motorSim.setSupplyVoltage(RobotController.getBatteryVoltage());
+turretphysicssSim.setInputVoltage(motorSim.getMotorVoltage());
+turretphysicssSim.update(deltaTime);
+motorSim.setRawRotorPosition(turretphysicssSim.getAngularPositionRotations() * turretphysicssSim.getGearing());
+motorSim.setRotorVelocity(turretphysicssSim.getAngularVelocityRPM()/60 * turretphysicssSim.getGearing());
+
+});
+simNotifier.startPeriodic(simLoopPeriod);
+
+}
+}

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

@@ -5,42 +5,69 @@
 package frc.robot.subsystems.shooter;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import java.util.function.Supplier;
+import org.littletonrobotics.junction.Logger;
+
 
 /** Pivoting hooded shooter (turret). !! COMP !! */
 public class TurretSubsystem extends SubsystemBase implements Shooter {
+  public static double TURRET_GEAR_RATIO = (12.0 / 42.0) * (16.0 / 32.0) * (10.0 / 85.0);
+  public TurretIO turretIO;
+  public TurretIOInputsAutoLogged turretIOInputs = new TurretIOInputsAutoLogged();
+
   /** Creates a new TurretSubsystem. */
-  public TurretSubsystem() {}
+  public TurretSubsystem(TurretIO turretIO) {
+    this.turretIO = turretIO;
+  }
 
   @Override
   public void periodic() {
+    turretIO.updateInputs(turretIOInputs);
+    Logger.processInputs("Shooter/Turret", turretIOInputs);
     // This method will be called once per scheduler run
   }
 
   @Override
   public Command shoot(Supplier<Pose2d> robotPoseSupplier) {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'shoot'");
+    return this.run(
+        () -> {
+          turretIO.setTurretPosition(new Rotation2d());
+          // TODO:Find the reall number
+
+        });
   }
 
   @Override
   public Command feed(Supplier<Pose2d> robotPoseSupplier, Supplier<Pose2d> feedTarget) {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'feed'");
+    return this.run(
+        () -> {
+          turretIO.setTurretPosition(new Rotation2d());
+          // TODO:Find the reall number
+
+        });
   }
 
   @Override
   public Command rest() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'rest'");
+    return this.run(
+        () -> {
+          turretIO.setTurretPosition(new Rotation2d());
+          // TODO:Find the reall number
+
+        });
   }
 
   @Override
   public Command spit() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'spit'");
+    return this.run(
+        () -> {
+          turretIO.setTurretPosition(new Rotation2d());
+          // TODO:Find the reall number
+
+        });
   }
 
   @Override
@@ -63,7 +90,11 @@ public Command zeroHood() {
 
   @Override
   public Command testShoot() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'testShoot'");
+    return this.run(
+        () -> {
+          turretIO.setTurretPosition(new Rotation2d());
+          // TODO:Find the reall number
+
+        });
   }
 }