Initial commit

unilabos/devices/motor/Grasp.py

@@ -0,0 +1,153 @@
+import time
+from serial import Serial 
+from threading import Thread
+
+from unilabos.device_comms.universal_driver import UniversalDriver
+
+class EleGripper(UniversalDriver):
+    @property
+    def status(self) -> str:
+        return f"spin_pos: {self.rot_data[0]}, grasp_pos: {self.gri_data[0]}, spin_v: {self.rot_data[1]}, grasp_v: {self.gri_data[1]}, spin_F: {self.rot_data[2]}, grasp_F: {self.gri_data[2]}"
+
+    def __init__(self,port,baudrate=115200, pos_error=-11, id = 9):
+        self.ser = Serial(port,baudrate)
+        self.pos_error = pos_error
+
+        self.success = False
+
+        # [pos, speed, force]
+        self.gri_data = [0,0,0]
+        self.rot_data = [0,0,0]
+
+        self.id = id
+
+        self.init_gripper()
+        self.wait_for_gripper_init()
+
+        t = Thread(target=self.data_loop)
+        t.start()
+
+        self.gripper_move(0,255,255)
+        self.rotate_move_abs(0,255,255)
+
+    def modbus_crc(self, data: bytes) -> bytes:
+        crc = 0xFFFF
+        for pos in data:
+            crc ^= pos
+            for _ in range(8):
+                if (crc & 0x0001) != 0:
+                    crc >>= 1
+                    crc ^= 0xA001
+                else:
+                    crc >>= 1
+        return crc.to_bytes(2, byteorder='little')
+
+    def send_cmd(self, id, fun, address, data:str):
+        data_len = len(bytes.fromhex(data))
+        data_ = f"{id:02X} {fun} {address} {data_len//2:04X} {data_len:02X} {data}"
+        data_bytes = bytes.fromhex(data_)
+        data_with_checksum = data_bytes + self.modbus_crc(data_bytes)
+        self.ser.write(data_with_checksum)
+        time.sleep(0.01)
+        self.ser.read(8)
+
+    def read_address(self, id , address, data_len):
+        data = f"{id:02X} 03 {address} {data_len:04X}"
+        data_bytes = bytes.fromhex(data)
+        data_with_checksum = data_bytes + self.modbus_crc(data_bytes)
+        self.ser.write(data_with_checksum)
+        time.sleep(0.01)
+        res_len = 5+data_len*2
+        res = self.ser.read(res_len)
+        return res
+
+    def init_gripper(self):
+        self.send_cmd(self.id,'10','03 E8','00 01')
+        self.send_cmd(self.id,'10','03 E9','00 01')
+
+    def gripper_move(self, pos, speed, force):
+        self.send_cmd(self.id,'10', '03 EA', f"{speed:02x} {pos:02x} {force:02x} 01")
+
+    def rotate_move_abs(self, pos, speed, force):
+        pos += self.pos_error
+        if pos < 0:
+            pos = (1 << 16) + pos
+        self.send_cmd(self.id,'10', '03 EC', f"{(pos):04x} {force:02x} {speed:02x} 0000 00 01")
+
+    # def rotate_move_rel(self, pos, speed, force):
+    #     if pos < 0:
+    #         pos = (1 << 16) + pos
+    #     print(f'{pos:04x}')
+    #     self.send_cmd(self.id,'10', '03 EC', f"0000 {force:02x} {speed:02x} {pos:04x} 00 02")
+
+    def wait_for_gripper_init(self):
+        res = self.read_address(self.id, "07 D0", 1)
+        res_r = self.read_address(self.id, "07 D1", 1)
+        while res[4] == 0x08 or res_r[4] == 0x08:
+            res = self.read_address(self.id, "07 D0", 1)
+            res_r = self.read_address(self.id, "07 D1", 1)
+            time.sleep(0.1)
+
+    def wait_for_gripper(self):
+        while self.gri_data[1] != 0:
+            time.sleep(0.1)
+
+    def wait_for_rotate(self):
+        while self.rot_data[1] != 0:
+            time.sleep(0.1)
+
+    def data_reader(self):
+        res_g = self.read_address(self.id, "07 D2", 2)
+        res_r = self.read_address(self.id, "07 D4", 4)
+        int32_value = (res_r[3] << 8) | res_r[4]
+        if int32_value > 0x7FFF:
+            int32_value -= 0x10000  
+        self.gri_data = (int(res_g[4]), int(res_g[3]), int(res_g[5]))
+        self.rot_data = (int32_value, int(res_r[5]), int(res_r[6]))
+
+    def data_loop(self):
+        while True:
+            self.data_reader()
+            time.sleep(0.1)
+
+
+    def node_gripper_move(self, cmd:str):
+        self.success = False
+        
+        try:
+            cmd_list = [int(x) for x in cmd.replace(' ','').split(',')]
+            self.gripper_move(*cmd_list)
+            self.wait_for_gripper()
+        except Exception as e:
+            raise e
+
+        self.success = True
+
+    def node_rotate_move(self, cmd:str):
+        self.success = False
+
+        try:
+            cmd_list = [int(x) for x in cmd.replace(' ','').split(',')]
+            self.rotate_move_abs(*cmd_list)
+            self.wait_for_rotate()
+        except Exception as e:
+            raise e
+        
+        self.success = True
+
+    def move_and_rotate(self, spin_pos, grasp_pos, spin_v, grasp_v, spin_F, grasp_F):
+        self.gripper_move(grasp_pos, grasp_v, grasp_F)
+        self.wait_for_gripper()
+        self.rotate_move_abs(spin_pos, spin_v, spin_F)
+        self.wait_for_rotate()
+
+if __name__ == "__main__":
+    gripper = EleGripper("COM12")
+    gripper.init_gripper()
+    gripper.wait_for_gripper_init()
+    gripper.gripper_move(210,127,255)
+    gripper.wait_for_gripper()
+    gripper.rotate_move_abs(135,10,255)
+    gripper.data_reader()
+    print(gripper.rot_data)
+