finished GroundIntakePivoteReal

Author: anonymes-axolotole

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

@@ -75,5 +75,6 @@ public enum Mode {
     public static final int ARM_INTAKE_ID = 18;
     public static final int ARM_ENCODER_ID = 24;
 
+    // Intake
     public static final int GROUND_INTAKE_PIVOT_ID = 21;
 }

src/main/java/frc/robot/subsystems/arm/ArmIOReal.java

@@ -19,7 +19,7 @@ public class ArmIOReal implements ArmIO {
     TalonFX pivotMotor;
     TalonFXConfiguration pivotMotorConfig;
 
-    PositionVoltage pivotPositionControl = new PositionVoltage(0);
+    private PositionVoltage pivotPositionControl = new PositionVoltage(0);
 
     // Intake/Outtake motors & configurations
     TalonFX intakeMotor;

src/main/java/frc/robot/subsystems/groundintake/GroundIntakePivotIO.java

@@ -7,6 +7,8 @@
 
 public interface GroundIntakePivotIO {
 
+    final double MOTOR_TO_GROUND_INTAKE_PIVOT = 66.0 / 14.0; // 14.0 / 66.0;
+
     @AutoLog
     class GroundIntakePivotIOInputs {
         public double groundIntakePivotAngleDegrees;

src/main/java/frc/robot/subsystems/groundintake/GroundIntakePivotIOReal.java

@@ -3,8 +3,8 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
-
 import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Voltage;
 import frc.robot.Constants;
 
 public class GroundIntakePivotIOReal implements GroundIntakePivotIO {
@@ -16,31 +16,37 @@ public class GroundIntakePivotIOReal implements GroundIntakePivotIO {
 
     public GroundIntakePivotIOReal() {
 
-        groundIntakePivotMotor = new TalonFX(Constants.GROUNDINTAKEPIVOTE_ID);
+        groundIntakePivotMotor = new TalonFX(Constants.GROUND_INTAKE_PIVOT_ID);
     }
 
     @Override
-    public void updateState(GroundIntakePivotIOInputs inputs){
-        
+    public void updateState(GroundIntakePivotIOInputs inputs) {
+        inputs.groundIntakePivotAngleDegrees = 0.0;
+
+        inputs.pivotMotorRotations = groundIntakePivotMotor.getPosition().getValueAsDouble();
+        inputs.pivotMotorVelocityRotsPerSecond =
+                groundIntakePivotMotor.getVelocity().getValueAsDouble();
+        inputs.pivotMotorCurrent = groundIntakePivotMotor.getStatorCurrent().getValueAsDouble();
+        inputs.pivotMotorVoltage = groundIntakePivotMotor.getMotorVoltage().getValueAsDouble();
     }
 
     @Override
     public void setPivotMotorSetpoint(Angle setpoint) {
-
+        groundIntakePivotMotor.setControl(positionVoltage.withPosition(setpoint));
     }
 
     @Override
-    public void setPivotMotorVoltage(double voltage) {
-        
+    public void setPivotMotorVoltage(Voltage voltage) {
+        groundIntakePivotMotor.setVoltage(voltage.magnitude());
     }
 
     @Override
     public Angle getPivotMotorPosition() {
-
+        return groundIntakePivotMotor.getPosition().getValue();
     }
 
     @Override
     public Angle getGroundIntakePivotPosition() {
-
+        return groundIntakePivotMotor.getPosition().getValue().div(MOTOR_TO_GROUND_INTAKE_PIVOT);
     }
 }