From 0460ad023da20690e81e3b59ecf3eebf7b011a33 Mon Sep 17 00:00:00 2001
From: Rohan Godha <rgodha24@brophybroncos.org>
Date: Mon, 26 Feb 2024 13:13:07 -0700
Subject: [PATCH] track more stuff

---
 src/main/java/frc/robot/Constants.java                    | 8 +++++---
 src/main/java/frc/robot/subsystems/hang/HangIO.java       | 3 +++
 src/main/java/frc/robot/subsystems/hang/HangIOReal.java   | 6 +++++-
 src/main/java/frc/robot/subsystems/intake/IntakeIO.java   | 1 +
 .../java/frc/robot/subsystems/intake/IntakeIOReal.java    | 3 +++
 src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java | 1 -
 6 files changed, 17 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 478f3f5..d860f22 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/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;
   }
diff --git a/src/main/java/frc/robot/subsystems/hang/HangIO.java b/src/main/java/frc/robot/subsystems/hang/HangIO.java
index 1caa8ef..692219a 100644
--- a/src/main/java/frc/robot/subsystems/hang/HangIO.java
+++ b/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) {}
diff --git a/src/main/java/frc/robot/subsystems/hang/HangIOReal.java b/src/main/java/frc/robot/subsystems/hang/HangIOReal.java
index 9eb27f5..18b7168 100644
--- a/src/main/java/frc/robot/subsystems/hang/HangIOReal.java
+++ b/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);
 
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
index ecf8915..dd432f2 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
+++ b/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) {}
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
index 89cfb3d..332106a 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
+++ b/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
diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java
index 605bc5a..3450f6b 100644
--- a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java
+++ b/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);