Update logic so that aimer locks when intaking and only when intaking

team401 · Oct 26, 2024 · 9e97a9f · 9e97a9f
1 parent 1d0fbc8
commit 9e97a9f
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 11 deletions.
diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java
@@ -60,6 +60,7 @@ public class ScoringConstants {
 
     public static final double aimMaxAngleRotations = 0.361328 - 0.184570;
     public static final double aimMinAngleRotations = -0.217285;
+    public static final double aimLockVoltage = -0.5;
 
     public static final double aimAngleTolerance = 0.015;
 

diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java
@@ -245,14 +245,18 @@ public void applyOutputs(AimerOutputs outputs) {
 
         request = motionMagicVoltage;
 
-        if (lockNegativeAtHome && !override) {
-            talonFXConfigs.CurrentLimits.StatorCurrentLimit =
-                    ScoringConstants.aimerCurrentLimit / 5;
-            aimerMotor.getConfigurator().apply(talonFXConfigs);
-            request = new VoltageOut(-0.05);
+        if (lockNegativeAtHome
+                && getEncoderPosition()
+                        < ScoringConstants.aimMinAngleRotations
+                                + ScoringConstants.intakeAngleToleranceRotations
+                && !override) {
+            // talonFXConfigs.CurrentLimits.StatorCurrentLimit =
+            //        ScoringConstants.aimerCurrentLimit / 5;
+            // aimerMotor.getConfigurator().apply(talonFXConfigs);
+            request = new VoltageOut(ScoringConstants.aimLockVoltage);
         } else {
             talonFXConfigs.CurrentLimits.StatorCurrentLimit = ScoringConstants.aimerCurrentLimit;
-            aimerMotor.getConfigurator().apply(talonFXConfigs);
+            // aimerMotor.getConfigurator().apply(talonFXConfigs);
             motionMagicVoltage.withPosition(goalAngleRot);
             request = motionMagicVoltage;
         }

diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java
@@ -183,11 +183,8 @@ private void idle() {
     }
 
     private void intake() {
-        if (!aimerAtIntakePosition()) {
-            aimerIo.setAimAngleRot(ScoringConstants.aimMinAngleRotations);
-        } else {
-            aimerIo.setNegativeHomeLockMode(true);
-        }
+        aimerIo.setAimAngleRot(ScoringConstants.aimMinAngleRotations);
+        aimerIo.setNegativeHomeLockMode(true);
 
         if (!hasNote()) {
             shooterIo.setKickerVolts(ScoringConstants.kickerIntakeVolts);
@@ -509,6 +506,8 @@ public void enabledInit() {
 
     @Override
     public void periodic() {
+        aimerIo.setNegativeHomeLockMode(false); // This should be false in all states but intake
+        // Intake will set it to true later in the loop.
 
         if (!SmartDashboard.containsKey("Aimer Offset")) {
             SmartDashboard.putNumber("Aimer Offset", ScoringConstants.aimerStaticOffset);