Merge pull request #225 from thedropbears/coral-placer-new

Coral placer new

Author: LHowe-a11y

autonomous/auto_base.py

@@ -100,7 +100,14 @@ def tracking_trajectory(self, initial_call, state_tm) -> None:
         distance = current_pose.translation().distance(final_pose.translation())
         angle_error = (final_pose.rotation() - current_pose.rotation()).radians()
 
-        if self.current_leg > 0 and not self.injector_component.has_algae():
+        if self.current_leg == 0:
+            self.coral_placer.lift()
+
+        if (
+            self.current_leg > 0
+            and not self.injector_component.has_algae()
+            and not self.coral_placer.is_executing
+        ):
             self.reef_intake.intake()
 
         if distance < self.DISTANCE_TOLERANCE and math.isclose(
@@ -144,7 +151,7 @@ def driving_to_coral(self) -> None:
     def scoring_coral(self, initial_call: bool) -> None:
         if initial_call:
             self.coral_placer.place()
-        elif not self.coral_placer.is_executing:
+        elif self.coral_placer.coral_is_scored():
             self.next_state("retreating")
 
     @state

controllers/coral_placer.py

@@ -1,17 +1,57 @@
-from magicbot import StateMachine, timed_state
+import math
 
-from components.coral_placer import CoralPlacerComponent
+from magicbot import StateMachine, state, tunable
+
+from components.chassis import ChassisComponent
+from components.wrist import WristComponent
 
 
 class CoralPlacer(StateMachine):
-    coral_placer_component: CoralPlacerComponent
+    wrist: WristComponent
+    chassis: ChassisComponent
+
+    # In degrees for tuning, converted to radians in tilt-to call
+    CORAL_PLACE_ANGLE = tunable(0.0)
+    CORAL_LOWER_ANGLE = tunable(-20.0)
+
+    RETREAT_DISTANCE = tunable(0.2)
 
     def __init__(self):
-        pass
+        self.coral_scored = False
 
-    def place(self):
+    def place(self) -> None:
+        self.engage("lowering", force=True)
+
+    def lift(self) -> None:
         self.engage()
 
-    @timed_state(duration=2.0, first=True, must_finish=True)
-    def placing(self):
-        self.coral_placer_component.place()
+    def coral_is_scored(self) -> bool:
+        return self.coral_scored
+
+    @state(first=True, must_finish=True)
+    def raising(self, initial_call: bool) -> None:
+        if initial_call:
+            self.coral_scored = False
+            self.wrist.tilt_to(math.radians(self.CORAL_PLACE_ANGLE))
+
+    @state(must_finish=True)
+    def lowering(self, initial_call: bool) -> None:
+        if initial_call:
+            self.wrist.tilt_to(math.radians(self.CORAL_LOWER_ANGLE))
+        if self.wrist.at_setpoint():
+            self.coral_scored = True
+            self.next_state("safing")
+
+    @state(must_finish=True)
+    def safing(self, initial_call: bool) -> None:
+        if initial_call:
+            self.score_pos = self.chassis.get_pose()
+
+        current_pose = self.chassis.get_pose()
+
+        distance = self.score_pos.translation().distance(current_pose.translation())
+
+        if distance >= self.RETREAT_DISTANCE:
+            self.wrist.go_to_neutral()
+            self.done()
+            return

robot.py

@@ -252,7 +252,9 @@ def testPeriodic(self) -> None:
         if self.gamepad.getRightTriggerAxis() > 0.5:
             self.algae_shooter.shoot()
         if self.gamepad.getAButton():
-            self.climber.deploy()
+            self.coral_placer.place()
+        if self.gamepad.getXButton():
+            self.coral_placer.lift()
 
         if self.gamepad.getLeftBumperButton():
             self.reef_intake.intake()
@@ -275,7 +277,7 @@ def testPeriodic(self) -> None:
         # Components
         self.chassis.execute()
         self.climber.execute()
-        self.coral_placer_component.execute()
+        # self.coral_placer_component.execute()
         self.shooter_component.execute()
         self.injector_component.execute()
         if self.gamepad.getStartButton():