turret

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

@@ -159,12 +159,12 @@ public RobotContainer() {
   private void configureBindings() {
     m_operatorController
         .rightBumper("Intake")
-        .whileTrue(intake.intakeIn().alongWith(spindex.goToShooter()));
+        .whileTrue(intake.intakeIn().alongWith(spindex.goToShooter()).alongWith(turret.setPosition(TurretConstants.kIntakePreset)));
     m_operatorController
         .rightTrigger("Shooter")
         .onTrue(
             shooter.setVelocity(
-                ShooterConstants.kShooterSpeakerRPS, ShooterConstants.kShooterSpeakerRPS));
+                ShooterConstants.kShooterSpeakerRPS, ShooterConstants.kShooterFollowerSpeakerRPS));
     m_operatorController.a("feed").onTrue(spindex.feedNoteToShooter());
     m_operatorController.x("sub").onTrue(pivotShooter.setSub());
     m_operatorController.y("zero").onTrue(pivotShooter.setPosition(0));

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

@@ -24,7 +24,7 @@ public final class ShooterConstants {
   public static MotorOutputConfigs motorOutputConfigs =
       new MotorOutputConfigs()
           .withNeutralMode(NeutralModeValue.Brake)
-          .withInverted(InvertedValue.Clockwise_Positive);
+          .withInverted(InvertedValue.CounterClockwise_Positive);
   public static TalonFXConfiguration motorConfigs =
       new TalonFXConfiguration()
           .withSlot0(
@@ -55,15 +55,15 @@ public final class ShooterConstants {
                   .withPeakReverseTorqueCurrent(80));
   public static TalonFXConfiguration followerMotorConfigs =
       motorConfigs.withMotorOutput(
-          motorOutputConfigs.withInverted(InvertedValue.CounterClockwise_Positive));
+          motorOutputConfigs.withInverted(InvertedValue.Clockwise_Positive));
 
-  public static double kShooterSpeakerRPS = 42;
-  public static double kShooterFollowerSpeakerRPS = 45; // really 80
+  public static double kShooterSpeakerRPS = -30;
+  public static double kShooterFollowerSpeakerRPS = 60; // really 80
 
-  public static double kShooterSubwooferRPS = 60;
+  public static double kShooterSubwooferRPS = -60;
   public static double kShooterFollowerSubwooferRPS = 70;
 
-  public static double kShooterFeederRPS = 42;
+  public static double kShooterFeederRPS = -42;
   public static double kShooterFollowerFeederRPS = 45;
 
   public static final class SimulationConstants {

src/main/java/frc/robot/subsystems/spindex/Spindex.java

@@ -95,9 +95,13 @@ public Command goToShooter() {
   }
 
   public Command feedNoteToShooter() {
-    return setShooterFeederVoltage(SpindexConstants.shooterFeederVoltage)
-        .until(() -> !beamBreakIOAutoLogged.beamBroken)
-        .finallyDo(shooterFeederIO::off);
+    return setVoltage(SpindexConstants.spindexMotorVoltage, SpindexConstants.shooterFeederVoltage)
+//        .until(() -> !beamBreakIOAutoLogged.beamBroken)
+            .finallyDo(
+                    () -> {
+                      spindexIO.off();
+                      shooterFeederIO.off();
+                    });
   }
 
   public Command goToAmpevator() {

src/main/java/frc/robot/subsystems/spindex/SpindexConstants.java

@@ -13,7 +13,7 @@
 
 public class SpindexConstants {
   public static final int spindexMotorID = 50;
-  public static final double spindexMotorVoltage = -7;
+  public static final double spindexMotorVoltage = 7;
   public static TalonFXConfiguration spindexMotorConfigs =
       new TalonFXConfiguration()
           .withSlot0(new Slot0Configs().withKS(0).withKV(0.1).withKP(1).withKI(0).withKD(0))

src/main/java/frc/robot/subsystems/turret/EncoderIOCancoder.java

@@ -21,7 +21,7 @@ public class EncoderIOCancoder implements EncoderIO {
   private final StatusSignal<Double> encoderVelocity;
 
   public EncoderIOCancoder(int canCoderID) {
-    encoder = new CANcoder(canCoderID, "mani");
+    encoder = new CANcoder(canCoderID);
     PhoenixUtil.applyCancoderConfig(
         encoder, TurretConstants.canCoderConfig, TurretConstants.flashConfigRetries);
 

src/main/java/frc/robot/subsystems/turret/Turret.java

@@ -83,8 +83,7 @@ public Command setPositionFieldRelative(Rotation2d position, CommandSwerveDrivet
     return this.run(
         () ->
             turretIO.setPosition(
-                position.minus(swerve.getState().Pose.getRotation()).getRotations()
-                    / TurretConstants.gearRatio));
+                position.minus(swerve.getState().Pose.getRotation()).getRotations()));
   }
 
   /**
@@ -93,7 +92,7 @@ public Command setPositionFieldRelative(Rotation2d position, CommandSwerveDrivet
    */
   public Command setPosition(Rotation2d position) {
     return this.runOnce(
-        () -> turretIO.setPosition(position.getRotations() / TurretConstants.gearRatio));
+        () -> turretIO.setPosition(position.getRotations()));
   }
 
   /**
@@ -156,12 +155,6 @@ public Command shakeHead() {
    */
   public Command reset() {
     return this.runOnce(
-        () -> {
-          if (turretIOInputs.turretMotorPosition > TurretConstants.kForwardLimit) {
-            turretIO.setPosition(0);
-          } else if (turretIOInputs.turretMotorPosition < TurretConstants.kReverseLimit) {
-            turretIO.setPosition(0);
-          }
-        });
+        () -> turretIO.setPosition(0));
   }
 }

src/main/java/frc/robot/subsystems/turret/TurretConstants.java

@@ -8,6 +8,7 @@
 package frc.robot.subsystems.turret;
 
 import com.ctre.phoenix6.configs.*;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -16,8 +17,6 @@ public final class TurretConstants {
 
   public static int kCanCoderID1 = 41;
   public static int kCanCoderID2 = 42;
-
-  public static final double gearRatio = 10.0 / 133; // TODO: Set gear ratio
   public static final CANcoderConfiguration canCoderConfig =
       new CANcoderConfiguration().withMagnetSensor(new MagnetSensorConfigs().withMagnetOffset(0));
   public static final double updateFrequency = 50.0;
@@ -28,18 +27,19 @@ public final class TurretConstants {
   public static final double followTagI = 0;
   public static final double followTagD = 0;
 
-  public static final double kForwardLimit = 69; // TODO: Set limit
-  public static final double kReverseLimit = -69; // TODO: Set limit
+  public static final double kForwardLimit = 32; // TODO: Set limit
+  public static final double kReverseLimit = -1; // TODO: Set limit
 
   public static final Rotation2d kSubPreset = Rotation2d.fromRotations(0);
+  public static final Rotation2d kIntakePreset = Rotation2d.fromRotations(16.5);
 
   public static final TalonFXConfiguration motorConfigs = // TODO: Set configs
       new TalonFXConfiguration()
           .withSlot0(
               new Slot0Configs()
                   .withKS(0)
-                  .withKV(0.05)
-                  .withKP(1)
+                  .withKV(0)
+                  .withKP(20)
                   .withKI(0)
                   .withKD(0) // Original 0.145
               )
@@ -49,21 +49,15 @@ public final class TurretConstants {
                   .withInverted(InvertedValue.Clockwise_Positive))
           .withMotionMagic(
               new MotionMagicConfigs()
-                  .withMotionMagicAcceleration(100)
+                  .withMotionMagicAcceleration(1600)
                   .withMotionMagicCruiseVelocity(100)
-                  .withMotionMagicJerk(420))
+                  .withMotionMagicJerk(3200))
           .withCurrentLimits(
               new CurrentLimitsConfigs()
                   .withStatorCurrentLimitEnable(true)
-                  .withStatorCurrentLimit(60))
-          .withSoftwareLimitSwitch(
-              new SoftwareLimitSwitchConfigs()
-                  .withForwardSoftLimitEnable(true)
-                  .withForwardSoftLimitThreshold(kForwardLimit)
-                  .withReverseSoftLimitEnable(true)
-                  .withReverseSoftLimitThreshold(kReverseLimit));
+                  .withStatorCurrentLimit(60));
 
-  public static final boolean kUseMotionMagic = false;
+  public static final boolean kUseMotionMagic = true;
 
   public static int flashConfigRetries = 5;
 

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

@@ -13,6 +13,7 @@ public interface TurretIO {
   @AutoLog
   public static class TurretIOInputs {
     public double turretMotorVoltage = 0.0;
+    public double turretMotorRotorPos = 0.0;
     public double turretMotorPosition = 0.0;
     public double turretMotorStatorCurrent = 0.0;
     public double turretMotorSupplyCurrent = 0.0;

src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java

@@ -23,6 +23,7 @@ public class TurretIOTalonFX implements TurretIO {
       new MotionMagicExpoVoltage(0).withSlot(0);
 
   private final StatusSignal<Double> turretMotorVoltage = turretMotor.getMotorVoltage();
+  private final StatusSignal<Double> turretMotorRotorPos = turretMotor.getRotorPosition();
   private final StatusSignal<Double> turretMotorPosition = turretMotor.getPosition();
   private final StatusSignal<Double> turretMotorStatorCurrent = turretMotor.getStatorCurrent();
   private final StatusSignal<Double> turretMotorSupplyCurrent = turretMotor.getSupplyCurrent();
@@ -33,12 +34,13 @@ public class TurretIOTalonFX implements TurretIO {
   public TurretIOTalonFX() {
     var motorConfig = TurretConstants.motorConfigs;
     ClosedLoopGeneralConfigs closedLoopGeneralConfigs = new ClosedLoopGeneralConfigs();
-    closedLoopGeneralConfigs.ContinuousWrap = true;
+    closedLoopGeneralConfigs.ContinuousWrap = false;
     motorConfig.ClosedLoopGeneral = closedLoopGeneralConfigs;
     PhoenixUtil.applyMotorConfigs(turretMotor, motorConfig, TurretConstants.flashConfigRetries);
 
     BaseStatusSignal.setUpdateFrequencyForAll(
         TurretConstants.updateFrequency,
+        turretMotorRotorPos,
         turretMotorVoltage,
         turretMotorPosition,
         turretMotorStatorCurrent,
@@ -53,12 +55,14 @@ public void updateInputs(TurretIOInputs inputs) {
     BaseStatusSignal.refreshAll(
         turretMotorVoltage,
         turretMotorPosition,
+        turretMotorRotorPos,
         turretMotorStatorCurrent,
         turretMotorSupplyCurrent,
         turretMotorTemperature,
         turretMotorReferenceSlope);
     inputs.turretMotorVoltage = turretMotorVoltage.getValueAsDouble();
     inputs.turretMotorPosition = turretMotorPosition.getValueAsDouble();
+    inputs.turretMotorRotorPos = turretMotorRotorPos.getValueAsDouble();
     inputs.turretMotorStatorCurrent = turretMotorStatorCurrent.getValueAsDouble();
     inputs.turretMotorSupplyCurrent = turretMotorSupplyCurrent.getValueAsDouble();
     inputs.turretMotorTemperature = turretMotorTemperature.getValueAsDouble();