added plate crane set speed method

AD-SDL · Jun 25, 2024 · 2820074 · 2820074
1 parent eac2338
commit 2820074
Show file tree

Hide file tree

Showing 2 changed files with 47 additions and 103 deletions.
diff --git a/src/platecrane_driver/platecrane_driver.py b/src/platecrane_driver/platecrane_driver.py
@@ -9,6 +9,9 @@
     SerialPort,  # use when running through WEI REST clients
 )
 
+# from serial_port import SerialPort      # use when running through the driver
+# from resource_defs import locations, plate_definitions
+# from resource_types import PlateResource
 # from serial_port import SerialPort      # use when running through the driver
 # from resource_defs import locations, plate_definitions
 # from resource_types import PlateResource
@@ -389,6 +392,8 @@ def pick_plate_safe_approach(
         Returns:
             None
         """
+        print("PICK PLATE SAFE APPROACH CALLED")
+
 
         # open the gripper
         self.gripper_open()
@@ -402,11 +407,21 @@ def pick_plate_safe_approach(
             Y=current_pos[3],
         )
 
+        # Rotate gripper
+        current_pos = self.get_position()
+        self.move_joint_angles(
+            R=current_pos[0], 
+            Z=current_pos[1],
+            P=locations[source].joint_angles[2], 
+            Y=current_pos[3],
+        )
+
+
         # Lower z axis to safe_approach_z height
         current_pos = self.get_position()
         self.move_joint_angles(
-            R=current_pos[0],
-            Z=locations[plate_type].safe_approach_height,
+            R=current_pos[0], 
+            Z=locations[source].safe_approach_height,
             P=current_pos[2],
             Y=current_pos[3],
         )
@@ -416,7 +431,7 @@ def pick_plate_safe_approach(
         self.move_joint_angles(
             R=current_pos[0],
             Z=current_pos[1],
-            P=locations[source].joint_angles[2],
+            P=current_pos[2], 
             Y=locations[source].joint_angles[3],
         )
 
@@ -451,6 +466,7 @@ def pick_plate_safe_approach(
         )
 
         # move rest of joints to neutral location
+        self.move_tower_neutral()
         self.move_arm_neutral()
 
     def place_plate_safe_approach(
@@ -478,6 +494,15 @@ def place_plate_safe_approach(
             Y=current_pos[3],
         )
 
+        # Rotate gripper to correct orientation 
+        current_pos = self.get_position()
+        self.move_joint_angles(
+            R=current_pos[0], 
+            Z=current_pos[1],
+            P=locations[target].joint_angles[2], 
+            Y=current_pos[3],
+        )
+
         # Lower z axis to safe_approach_z height
         current_pos = self.get_position()
         self.move_joint_angles(
@@ -487,12 +512,12 @@ def place_plate_safe_approach(
             Y=current_pos[3],
         )
 
-        # extend arm over plate and rotate gripper to correct orientation
+        # extend arm over plate
         current_pos = self.get_position()
         self.move_joint_angles(
             R=current_pos[0],
             Z=current_pos[1],
-            P=locations[target].joint_angles[2],
+            P=current_pos[2], 
             Y=locations[target].joint_angles[3],
         )
 
@@ -505,7 +530,6 @@ def place_plate_safe_approach(
             Y=current_pos[3],
         )
 
-        time.sleep(2)
         self.gripper_open()
 
         # Back away using safe approach path

diff --git a/src/platecrane_driver/resource_defs.py b/src/platecrane_driver/resource_defs.py
@@ -1,103 +1,23 @@
-"""Resource Definitions for the PlateCrane in BIO 350"""
-
-# from resource_types import Location, PlateResource  # when testing through driver
-from platecrane_driver.resource_types import (
-    Location,  # through WEI
-    PlateResource,
-)
+# from resource_types import Location, PlateResource  # through driver
+from platecrane_driver.resource_types import Location, PlateResource  # through WEI
 
 # Locations accessible by the PlateCrane EX. [R (base), Z (vertical axis), P (gripper rotation), Y (arm extension)]
 locations = {
-    "Safe": Location(
-        name="Safe",
-        joint_angles=[182220, 2500, 460, -308],
-        location_type="nest",
-        safe_approach_height=0,
-    ),
-    "Stack1": Location(
-        name="Stack1",
-        joint_angles=[164672, -32703, 472, 5389],
-        location_type="stack",
-        safe_approach_height=0,
-    ),
-    "Stack2": Location(
-        name="Stack2",
-        joint_angles=[182220, -32703, 460, 5420],
-        location_type="stack",
-        safe_approach_height=0,
-    ),
-    "Stack3": Location(
-        name="Stack3",
-        joint_angles=[199708, -32703, 514, 5484],
-        location_type="stack",
-        safe_approach_height=0,
-    ),
-    "Stack4": Location(
-        name="Stack4",
-        joint_angles=[217401, -32703, 546, 5473],
-        location_type="stack",
-        safe_approach_height=0,
-    ),
-    "Stack5": Location(
-        name="Stack5",
-        joint_angles=[235104, -32703, 532, 5453],
-        location_type="stack",
-        safe_approach_height=0,
-    ),
-    "LidNest1": Location(
-        name="LidNest1",
-        joint_angles=[168355, -31800, 484, -306],
-        location_type="nest",
-        safe_approach_height=0,
-    ),
-    "LidNest2": Location(
-        name="LidNest2",
-        joint_angles=[199805, -31800, 484, -306],
-        location_type="nest",
-        safe_approach_height=0,
-    ),
-    "LidNest3": Location(
-        name="LidNest3",
-        joint_angles=[231449, -31800, 484, -306],
-        location_type="nest",
-        safe_approach_height=0,
-    ),
-    "Solo.Position1": Location(
-        name="Solo.Position1",
-        joint_angles=[36703, -27951, -1000, 3630],
-        location_type="nest",
-        safe_approach_height=0,
-    ),
-    "Solo.Position2": Location(
-        name="Solo.Position2",
-        joint_angles=[53182, -27951, -413, 834],
-        location_type="nest",
-        safe_approach_height=0,
-    ),
-    "Hidex.Nest": Location(
-        name="Hidex.Nest",
-        joint_angles=[102327, -31090, -5830, 2378],
-        location_type="nest",
-        safe_approach_height=-27033,
-    ),
-    "Sealer.Nest": Location(
-        name="Sealer.Nest",
-        joint_angles=[117305, 920, -4766, 4550],
-        location_type="nest",
-        safe_approach_height=0,
-    ),
-    "Peeler.Nest": Location(
-        name="Peeler.Nest",
-        joint_angles=[292635, -31058, -4521, 4235],
-        location_type="nest",
-        safe_approach_height=0,
-    ),
-    "Liconic.Nest": Location(
-        name="Liconic.Nest",
-        joint_angles=[265603, -19900, -5413, 4953],
-        location_type="nest",
-        safe_approach_height=0,
-    ),
+    "Safe": Location(name="Safe", joint_angles=[182220, 2500, 460, -308], location_type="nest", safe_approach_height=0),
+    "Stack1": Location(name="Stack1", joint_angles=[164672, -32703, 472, 5389], location_type="stack", safe_approach_height=0),
+    "Stack2": Location(name="Stack2", joint_angles=[182220, -32703, 460, 5420], location_type="stack", safe_approach_height=0),
+    "Stack3": Location(name="Stack3", joint_angles=[199708, -32703, 514, 5484], location_type="stack", safe_approach_height=0),
+    "Stack4": Location(name="Stack4", joint_angles=[217401, -32703, 546, 5473], location_type="stack", safe_approach_height=0),
+    "Stack5": Location(name="Stack5", joint_angles=[235104, -32703, 532, 5453], location_type="stack", safe_approach_height=0),
+    "LidNest1": Location(name="LidNest1", joint_angles=[168355, -31800, 484, -306], location_type="nest", safe_approach_height=0),
+    "LidNest2": Location(name="LidNest2", joint_angles=[199805, -31800, 484, -306], location_type="nest", safe_approach_height=0),
+    "LidNest3": Location(name="LidNest3", joint_angles=[231449, -31800, 484, -306], location_type="nest", safe_approach_height=0),
+    "Solo.Position1": Location(name="Solo.Position1", joint_angles=[36703, -27951, -1000, 3630], location_type="nest", safe_approach_height=0),
+    "Solo.Position2": Location(name="Solo.Position2", joint_angles=[53182, -27951, -413, 834], location_type="nest", safe_approach_height=0),
+    "Hidex.Nest": Location(name="Hidex.Nest", joint_angles=[102327, -31090, -5840, 2389], location_type="nest", safe_approach_height=-27033),
+    "Sealer.Nest": Location(name="Sealer.Nest", joint_angles=[117412, 920, -4766, 4514], location_type="nest", safe_approach_height=0), 
+    "Peeler.Nest": Location(name="Peeler.Nest", joint_angles=[292635, -31008, -4521, 4235], location_type="nest", safe_approach_height=0),
+    "Liconic.Nest": Location(name="Liconic.Nest", joint_angles=[265563, -19800, -5413, 4978], location_type="nest", safe_approach_height=0),
 }
 
 # Dimensions of labware used on the BIO_Workcells