pathhandler bug, logging, barge bug

Author: ds12a

src/main/java/frc/robot/Robot.java

@@ -96,17 +96,17 @@ public void teleopInit() {
       m_autonomousCommand.cancel();
     }
     if (!m_robotContainer.hasElevatorZeroed()) m_robotContainer.zeroElevator();
-
+    else if (m_robotContainer.wasScoringCoral()) {
+      m_robotContainer.finishAuto();
+    } else {
+      m_robotContainer.stow();
+    }
+    
     m_robotContainer.setIsAuto(false);
     m_robotContainer.setIsAutoPlacing(true);
     m_robotContainer.setScoringSide(ScoreSide.LEFT);
     m_robotContainer.disableNoMotionCal();
 
-    if (m_robotContainer.wasScoringCoral()) {
-      m_robotContainer.finishAuto();
-    } else {
-      m_robotContainer.stow();
-    }
   }
 
   @Override

src/main/java/frc/robot/commands/auton/NonProcessorShallowSlowAutonCommand.java

@@ -119,6 +119,7 @@ public void reassignAlliance() {
     pathHandler.setNodeNames(NodeNames);
     pathHandler.setNodeLevels(NodeLevels);
     pathHandler.setStartNode(startNode);
+    pathHandler.setGetAlgaeLast(false);
     // pathHandler.reassignAlliance();
   }
 }

src/main/java/frc/robot/commands/auton/ProcessorShallowSlowAutonCommand.java

@@ -116,6 +116,7 @@ public void reassignAlliance() {
     pathHandler.setNodeNames(NodeNames);
     pathHandler.setNodeLevels(NodeLevels);
     pathHandler.setStartNode(startNode);
+    pathHandler.setGetAlgaeLast(false);
     // pathHandler.reassignAlliance();
   }
 }

src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java

@@ -2,6 +2,7 @@
 
 import static edu.wpi.first.units.Units.Rotations;
 
+import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
@@ -12,6 +13,7 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj.Servo;
 import frc.robot.constants.ClimbConstants;
 import frc.robot.subsystems.climb.ClimbIO.ClimbIOInputs;
@@ -36,7 +38,8 @@ public class ClimbIOServoFX implements ClimbIO {
       new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0);
   private VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(false);
 
-  StatusSignal<Angle> currPosition;
+  private StatusSignal<Angle> currPosition;
+  private StatusSignal<AngularVelocity> currVelocity;
 
   public ClimbIOServoFX() {
     logger = LoggerFactory.getLogger(this.getClass());
@@ -52,6 +55,7 @@ public ClimbIOServoFX() {
     configuratorFront.apply(ClimbConstants.getPivotArmFxConfig());
 
     currPosition = talonFxPivotArmFront.getPosition();
+    currVelocity = talonFxPivotArmFront.getVelocity();
   }
 
   // @Override
@@ -83,7 +87,9 @@ public void setPinServoPosition(double position) {
 
   @Override
   public void updateInputs(ClimbIOInputs inputs) {
-    inputs.position = currPosition.refresh().getValueAsDouble();
+    BaseStatusSignal.refreshAll(currVelocity, currPosition);
+    inputs.position = currPosition.getValueAsDouble();
+    inputs.velocity = currVelocity.getValueAsDouble();
     inputs.ratchetServoPosition = ratchetServo.getPosition();
     inputs.pinServoPosition = pinServo.getPosition();
   }

src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java

@@ -726,6 +726,7 @@ public void periodic() {
 
       case TO_STOW -> {
         if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished()) {
+          driveSubsystem.removeDriveMultiplier();
           setState(RobotStates.STOW);
         }
       }