fixed pid

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

@@ -66,7 +66,7 @@ public static class PivotConstants {
     // less than this angle, we won't be able to shoot a note
     public static final Rotation2d minShootingAngle = Rotation2d.fromDegrees(25);
 
-    public static final double p = 0.00005, i = 0, d = 0;
+    public static final double p = 0.5, i = 0, d = 0;
   }
 
   public static class ShooterConstants {
@@ -75,7 +75,7 @@ public static class ShooterConstants {
     public static final int bottomShooterCANId = 17;
 
     public static final double shootingPower = -0.75;
-    public static final double conveyorPower = 0.6;
+    public static final double conveyorPower = 0.35;
   }
 
   public static class HangConstants {

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

@@ -60,7 +60,7 @@ public void robotInit() {
     switch (Constants.currentMode) {
       case REAL:
         // Running on a real robot, log to a USB stick ("/U/logs")
-        Logger.addDataReceiver(new WPILOGWriter());
+        // Logger.addDataReceiver(new WPILOGWriter());
         Logger.addDataReceiver(new NT4Publisher());
         break;
 

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

@@ -14,6 +14,8 @@
 package frc.robot;
 
 import com.pathplanner.lib.auto.AutoBuilder;
+
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -57,6 +59,13 @@ public class RobotContainer {
   public RobotContainer() {
     switch (Constants.currentMode) {
       case REAL:
+            //   drive =
+            // new Drive(
+            //     new GyroIO() {},
+            //     new ModuleIOSim(),
+            //     new ModuleIOSim(),
+            //     new ModuleIOSim(),
+            //     new ModuleIOSim());
         drive =
             new Drive(
                 new GyroIOPigeon2(true),
@@ -150,7 +159,7 @@ private void configureButtonBindings() {
 
     pivot.setDefaultCommand(
         PivotCommands.basicOperatorControl(
-            pivot, () -> operator.getRightX() - operator.getLeftX()));
+            pivot, () -> MathUtil.applyDeadband(operator.getRightX() - operator.getLeftX(), 0.1)));
 
     hang.setDefaultCommand(hang.operatorControl(() -> 
       operator.getRightTriggerAxis() - operator.getLeftTriggerAxis()));

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

@@ -15,7 +15,7 @@ public static Command basicOperatorControl(Pivot pivot, DoubleSupplier rotationS
         () -> {
           double delta = MathUtil.applyDeadband(rotationSupplier.getAsDouble(), 0.1, 1);
 
-          var target = pivot.getCurrentPosition().plus(Rotation2d.fromDegrees(delta));
+          var target = pivot.getCurrentPosition().plus(Rotation2d.fromDegrees(delta*4));
 
           pivot.setTargetPosition(target);
         },

src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java

@@ -24,7 +24,7 @@
 
 /** IO implementation for Pigeon2 */
 public class GyroIOPigeon2 implements GyroIO {
-  private final Pigeon2 pigeon = new Pigeon2(20);
+  private final Pigeon2 pigeon = new Pigeon2(0);
   private final StatusSignal<Double> yaw = pigeon.getYaw();
   private final Queue<Double> yawPositionQueue;
   private final StatusSignal<Double> yawVelocity = pigeon.getAngularVelocityZWorld();
@@ -34,7 +34,7 @@ public GyroIOPigeon2(boolean phoenixDrive) {
     pigeon.getConfigurator().setYaw(0.0);
     yaw.setUpdateFrequency(Module.ODOMETRY_FREQUENCY);
     yawVelocity.setUpdateFrequency(100.0);
-    pigeon.optimizeBusUtilization();
+    pigeon.optimizeBusUtilization(1.0);
     yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(pigeon, pigeon.getYaw());
   }
 

src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java

@@ -116,6 +116,7 @@ public ModuleIOTalonFX(int index) {
         turnCurrent);
     driveTalon.optimizeBusUtilization();
     turnTalon.optimizeBusUtilization();
+    cancoder.optimizeBusUtilization(1.0);
   }
 
   @Override

src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java

@@ -37,7 +37,7 @@ public class PhoenixOdometryThread extends Thread {
       new ReentrantLock(); // Prevents conflicts when registering signals
   private BaseStatusSignal[] signals = new BaseStatusSignal[0];
   private final List<Queue<Double>> queues = new ArrayList<>();
-  private boolean isCANFD = false;
+  private boolean isCANFD = true;
 
   private static PhoenixOdometryThread instance = null;
 

src/main/java/frc/robot/subsystems/pivot/Pivot.java

@@ -27,6 +27,9 @@ public void setTargetPosition(Rotation2d target) {
         || target.getRadians() > PivotConstants.pivotEnd.getRadians()) {
       throw new IllegalArgumentException("Target position is out of bounds");
     }
+
+    Logger.recordOutput("Pivot/TargetThing", target);
+
     io.setTargetPosition(target);
   }
 

src/main/java/frc/robot/subsystems/pivot/PivotIO.java

@@ -9,6 +9,7 @@ public static class PivotIOInputs {
     public Rotation2d pivotCurentAngle = new Rotation2d();
     public Rotation2d pivotTargetAngle = new Rotation2d();
     public double motorCurrent = 0.0;
+    public double motorVoltage = 0.0;
   }
 
   default void updateInputs(PivotIOInputs inputs) {}

src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java

@@ -4,9 +4,12 @@
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.PositionDutyCycle;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+
+// import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
 import frc.robot.Constants.PivotConstants;
 
@@ -18,8 +21,10 @@ public class PivotIOReal implements PivotIO {
   double zeroPosition;
   Rotation2d targetPosition;
   TalonFXConfiguration config;
+  PositionDutyCycle request;
 
   private final StatusSignal<Double> current;
+  private final StatusSignal<Double> voltage;
 
   public PivotIOReal() {
     pivotMotor = new TalonFX(PivotConstants.pivotCANId);
@@ -41,28 +46,32 @@ public PivotIOReal() {
     // in rotations!!
     position = pivotMotor.getPosition();
     current = pivotMotor.getTorqueCurrent();
+    voltage = pivotMotor.getMotorVoltage();
 
-    BaseStatusSignal.setUpdateFrequencyForAll(20.0, position, current);
+    BaseStatusSignal.setUpdateFrequencyForAll(100.0, position, current, voltage);
     pivotMotor.optimizeBusUtilization(1.0);
 
+    request = new PositionDutyCycle(position.getValueAsDouble()).withSlot(0);
+
     // PivotConstants.pivotStart
     zeroPosition = position.getValueAsDouble();
     targetPosition = angleFromEncoder(zeroPosition);
   }
 
   public void updateInputs(PivotIOInputs inputs) {
-    BaseStatusSignal.refreshAll(position);
+    BaseStatusSignal.refreshAll(position, current,voltage);
     double currentPosition = position.getValueAsDouble();
 
     inputs.pivotCurentAngle = angleFromEncoder(currentPosition);
     inputs.pivotTargetAngle = targetPosition;
     inputs.motorCurrent = current.getValueAsDouble();
+    inputs.motorVoltage = voltage.getValueAsDouble();
   }
 
   /** sets the target position where 0 is facing towards the back of the robot */
   public void setTargetPosition(Rotation2d target) {
     targetPosition = target;
-    pivotMotor.setPosition(encoderFromAngle(target));
+    pivotMotor.setControl(request.withPosition(encoderFromAngle(target)));
   }
 
   private Rotation2d angleFromEncoder(double encoderTicks) {