Merge pull request #299 from thedropbears/trap-profile

Always work on your traps, fellas!

Author: LucienMorey

components/intake.py

@@ -1,5 +1,4 @@
 import math
-import time
 
 import wpilib
 from magicbot import feedback, tunable
@@ -67,23 +66,37 @@ def __init__(self, intake_mech_root: wpilib.MechanismRoot2d) -> None:
 
         self.motor_encoder = self.arm_motor.getEncoder()
 
-        self.desired_angle = IntakeComponent.RETRACTED_ANGLE
-
-        self.last_setpoint_update_time = time.monotonic()
+        self.desired_state = TrapezoidProfile.State(
+            IntakeComponent.RETRACTED_ANGLE, 0.0
+        )
+        self.last_setpoint_update_time = wpilib.Timer.getFPGATimestamp()
+        self.initial_state = TrapezoidProfile.State(self.position(), self.velocity())
 
     def intake(self):
         if not math.isclose(
-            self.desired_angle, IntakeComponent.DEPLOYED_ANGLE, abs_tol=0.1
+            self.desired_state.position, IntakeComponent.DEPLOYED_ANGLE, abs_tol=0.1
         ):
-            self.desired_angle = IntakeComponent.DEPLOYED_ANGLE
-            self.last_setpoint_update_time = time.monotonic()
+            self.desired_state = TrapezoidProfile.State(
+                IntakeComponent.DEPLOYED_ANGLE, 0.0
+            )
+            self.last_setpoint_update_time = wpilib.Timer.getFPGATimestamp()
+            self.initial_state = TrapezoidProfile.State(
+                self.position(), self.velocity()
+            )
 
         self.desired_output = self.intake_output
 
     def retract(self):
-        if not math.isclose(self.desired_angle, self.RETRACTED_ANGLE, abs_tol=0.1):
-            self.desired_angle = IntakeComponent.RETRACTED_ANGLE
-            self.last_setpoint_update_time = time.monotonic()
+        if not math.isclose(
+            self.desired_state.position, self.RETRACTED_ANGLE, abs_tol=0.1
+        ):
+            self.desired_state = TrapezoidProfile.State(
+                IntakeComponent.RETRACTED_ANGLE, 0.0
+            )
+            self.initial_state = TrapezoidProfile.State(
+                self.position(), self.velocity()
+            )
+            self.last_setpoint_update_time = wpilib.Timer.getFPGATimestamp()
 
     @feedback
     def raw_encoder(self) -> float:
@@ -104,18 +117,18 @@ def execute(self) -> None:
 
         self.desired_output = 0.0
 
-        desired_state = self.motion_profile.calculate(
-            time.monotonic() - self.last_setpoint_update_time,
-            TrapezoidProfile.State(self.position(), self.velocity()),
-            TrapezoidProfile.State(self.desired_angle, 0.0),
+        tracked_state = self.motion_profile.calculate(
+            wpilib.Timer.getFPGATimestamp() - self.last_setpoint_update_time,
+            self.initial_state,
+            self.desired_state,
         )
-        ff = self.arm_ff.calculate(desired_state.position, desired_state.velocity)
+        ff = self.arm_ff.calculate(tracked_state.position, tracked_state.velocity)
 
         if not math.isclose(
-            self.desired_angle, self.position(), abs_tol=math.radians(5)
+            self.desired_state.position, self.position(), abs_tol=math.radians(5)
         ):
             self.arm_motor.setVoltage(
-                self.pid.calculate(self.position(), desired_state.position) + ff
+                self.pid.calculate(self.position(), tracked_state.position) + ff
             )
         else:
             self.arm_motor.setVoltage(0.0)

components/wrist.py

@@ -1,5 +1,4 @@
 import math
-import time
 
 import wpilib
 from magicbot import feedback
@@ -65,9 +64,12 @@ def __init__(self, mech_root: wpilib.MechanismRoot2d):
 
         self.motor_encoder = self.motor.getEncoder()
 
-        self.desired_angle = WristComponent.NEUTRAL_ANGLE
+        self.desired_state = TrapezoidProfile.State(WristComponent.NEUTRAL_ANGLE, 0.0)
 
-        self.last_setpoint_update_time = time.monotonic()
+        self.last_setpoint_update_time = wpilib.Timer.getFPGATimestamp()
+        self.initial_state = TrapezoidProfile.State(
+            self.inclination(), self.current_velocity()
+        )
 
     def on_enable(self):
         self.tilt_to(WristComponent.NEUTRAL_ANGLE)
@@ -117,15 +119,21 @@ def current_velocity(self) -> float:
 
     @feedback
     def at_setpoint(self) -> bool:
-        return abs(self.desired_angle - self.inclination()) < WristComponent.TOLERANCE
+        return (
+            abs(self.desired_state.position - self.inclination())
+            < WristComponent.TOLERANCE
+        )
 
     def tilt_to(self, pos: float) -> None:
         clamped_angle = clamp(pos, self.MAXIMUM_DEPRESSION, self.MAXIMUM_ELEVATION)
 
         # If the new setpoint is within the tolerance we wouldn't move anyway
-        if abs(clamped_angle - self.desired_angle) > self.TOLERANCE:
-            self.desired_angle = clamped_angle
-            self.last_setpoint_update_time = time.monotonic()
+        if abs(clamped_angle - self.desired_state.position) > self.TOLERANCE:
+            self.desired_state = TrapezoidProfile.State(clamped_angle, 0.0)
+            self.last_setpoint_update_time = wpilib.Timer.getFPGATimestamp()
+            self.initial_state = TrapezoidProfile.State(
+                self.inclination(), self.current_velocity()
+            )
 
     def go_to_neutral(self) -> None:
         self.tilt_to(WristComponent.NEUTRAL_ANGLE)
@@ -134,15 +142,15 @@ def reset_windup(self) -> None:
         self.tilt_to(self.inclination())
 
     def execute(self) -> None:
-        desired_state = self.wrist_profile.calculate(
-            time.monotonic() - self.last_setpoint_update_time,
-            TrapezoidProfile.State(self.inclination(), self.current_velocity()),
-            TrapezoidProfile.State(self.desired_angle, 0.0),
+        tracked_state = self.wrist_profile.calculate(
+            wpilib.Timer.getFPGATimestamp() - self.last_setpoint_update_time,
+            self.initial_state,
+            self.desired_state,
         )
-        ff = self.wrist_ff.calculate(desired_state.position, desired_state.velocity)
+        ff = self.wrist_ff.calculate(tracked_state.position, tracked_state.velocity)
 
         self.motor.setVoltage(
-            self.pid.calculate(self.inclination(), desired_state.position) + ff
+            self.pid.calculate(self.inclination(), tracked_state.position) + ff
         )
 
         self.wrist_ligament.setAngle(self.inclination_deg())