track more stuff

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

@@ -59,10 +59,12 @@ public static class PivotConstants {
 
     // the mechanically stopped beginning of the pivot is at a negative angle from the ground
     // 0 degrees is parallel to the ground and facing the same direction as the back of the robot
-    public static Rotation2d pivotStart = Rotation2d.fromRadians(-0.13969988112131737);
-    public static Rotation2d pivotEnd = Rotation2d.fromDegrees(129.056487);
+    public static final Rotation2d pivotStart = Rotation2d.fromRadians(-0.13969988112131737);
+    public static final Rotation2d pivotEnd = Rotation2d.fromDegrees(129.056487);
+    /** spot where the intake conveyor can take a note from the intake */
+    public static final Rotation2d intakePosition = Rotation2d.fromDegrees(25);
     // less than this angle, we won't be able to shoot a note
-    public static Rotation2d minShootingAngle = Rotation2d.fromDegrees(25);
+    public static final Rotation2d minShootingAngle = Rotation2d.fromDegrees(25);
 
     public static final double p = 0.00005, i = 0, d = 0;
   }

src/main/java/frc/robot/subsystems/hang/HangIO.java

@@ -7,6 +7,9 @@ public interface HangIO {
   public static class HangIOInputs {
     public double mainCurrent = 0.0;
     public double followerCurrent = 0.0;
+
+    public double mainPosition = 0.0;
+    public double followerPosition = 0.0;
   }
 
   default void updateInputs(HangIOInputs inputs) {}

src/main/java/frc/robot/subsystems/hang/HangIOReal.java

@@ -16,6 +16,8 @@ public class HangIOReal implements HangIO {
 
   StatusSignal<Double> leaderCurrent;
   StatusSignal<Double> followerCurrent;
+  StatusSignal<Double> leaderPosition;
+  StatusSignal<Double> followerPosition;
 
   public HangIOReal() {
 
@@ -35,9 +37,11 @@ public HangIOReal() {
 
     leaderCurrent = leader.getTorqueCurrent();
     followerCurrent = follower.getTorqueCurrent();
+    leaderPosition = leader.getPosition();
+    followerPosition = follower.getPosition();
 
     // don't really need this to update that often b/c it's just for data visualization
-    BaseStatusSignal.setUpdateFrequencyForAll(100.0, leaderCurrent, followerCurrent);
+    BaseStatusSignal.setUpdateFrequencyForAll(100.0, leaderCurrent, followerCurrent, followerPosition, leaderPosition);
     leader.optimizeBusUtilization(1.0);
     follower.optimizeBusUtilization(1.0);
 

src/main/java/frc/robot/subsystems/intake/IntakeIO.java

@@ -6,6 +6,7 @@ public interface IntakeIO {
   @AutoLog
   public static class IntakeIOInputs {
     public double current = 0.0;
+    public double voltage = 0.0;
   }
 
   public default void updateInputs(IntakeIOInputs inputs) {}

src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java

@@ -9,11 +9,14 @@ public class IntakeIOReal implements IntakeIO {
 
   public IntakeIOReal() {
     motor = new CANSparkMax(IntakeConstants.intakeCANId, MotorType.kBrushless);
+
+    motor.setSmartCurrentLimit(25);
   }
 
   @Override
   public void updateInputs(IntakeIO.IntakeIOInputs inputs) {
     inputs.current = motor.getOutputCurrent();
+    inputs.voltage = motor.getBusVoltage();
   }
 
   @Override

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

@@ -40,7 +40,6 @@ public PivotIOReal() {
 
     // in rotations!!
     position = pivotMotor.getPosition();
-
     current = pivotMotor.getTorqueCurrent();
 
     BaseStatusSignal.setUpdateFrequencyForAll(20.0, position, current);