From 7bca08a907264209e25f927fdbfc02b79282592d Mon Sep 17 00:00:00 2001
From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com>
Date: Wed, 18 Feb 2026 10:38:39 -0800
Subject: [PATCH] Update Hood.java

---
 src/main/java/frc/robot/subsystems/hood/Hood.java | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java
index e164243..07581ef 100644
--- a/src/main/java/frc/robot/subsystems/hood/Hood.java
+++ b/src/main/java/frc/robot/subsystems/hood/Hood.java
@@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.Constants;
 import frc.robot.constants.IdConstants;
+import yams.gearing.GearBox;
 
 public class Hood extends SubsystemBase implements HoodIO{
     private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
@@ -84,7 +85,7 @@ public class Hood extends SubsystemBase implements HoodIO{
     }
 
 	/**
-	 * Sets the setpoint position and velocity of the hood
+	 * Sets the setpoint position and velocity of the hood. Call in command execute.
 	 * @param angle
 	 * @param velocityRadPerSec
 	 */
@@ -93,6 +94,13 @@ public class Hood extends SubsystemBase implements HoodIO{
 		goalVelocityRadPerSec = velocityRadPerSec;
 	}
 
+	/**
+	 * @return Position of turret in degrees
+	 */
+	public double getPositionDeg(){
+		return Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO;
+	}
+
     @Override
     public void periodic() {
 		updateInputs();
@@ -116,10 +124,12 @@ public class Hood extends SubsystemBase implements HoodIO{
 		double motorGoalRotations = Units.radiansToRotations(setpointRad) * GEAR_RATIO;
 
 		//Clamp the setpoint rotations
-		motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(HoodConstants.MIN_ANGLE) * GEAR_RATIO, Units.degreesToRotations(HoodConstants.MAX_ANGLE) * GEAR_RATIO);
+		motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.radiansToRotations(MIN_ANGLE_RAD) * GEAR_RATIO, Units.radiansToRotations(MAX_ANGLE_RAD) * GEAR_RATIO);
 		
+		// Multiply goal velocity by kV
 		double velocityCompensation = goalVelocityRadPerSec * FEEDFORWARD_KV;
 
+		// Set control with feedforward
 		motor.setControl(mmVoltageRequest
 			.withPosition(motorGoalRotations)
 			.withFeedForward(velocityCompensation));
-- 
2.39.5