Merge pull request #388 from thedropbears/fix-windup

Author: LucienMorey

components/intake.py

@@ -115,7 +115,7 @@ def _force_retract(self):
         self.last_setpoint_update_time = wpilib.Timer.getFPGATimestamp()
 
     def on_enable(self) -> None:
-        self.retract()
+        self._force_retract()
         self.desired_output = 0.0
 
     def execute(self) -> None:

components/wrist.py

@@ -82,7 +82,6 @@ def __init__(self, mech_root: wpilib.MechanismRoot2d):
         )
 
     def on_enable(self):
-        # self.tilt_to(WristComponent.NEUTRAL_ANGLE)
         self._tilt_to(self.inclination())
         self.idle_mode = SparkMaxConfig.IdleMode.kBrake
         wrist_config = SparkMaxConfig()
@@ -156,9 +155,6 @@ def _tilt_to(self, pos: float):
     def go_to_neutral(self) -> None:
         self.tilt_to(WristComponent.NEUTRAL_ANGLE)
 
-    def reset_windup(self) -> None:
-        self.tilt_to(self.inclination())
-
     @feedback
     def at_limit(self) -> bool:
         return self.motor.getReverseLimitSwitch().get()

robot.py

@@ -348,9 +348,6 @@ def disabledPeriodic(self) -> None:
         self.chassis.update_alliance()
         self.chassis.update_odometry()
 
-        self.wrist.reset_windup()
-        self.wrist.execute()
-
         self.starboard_vision.execute()
         self.port_vision.execute()