Initial commit

README.md

+TicTacToe mit PhantomX-Roboter

phantomController.py

+import serial
+import time
+import numpy as np
+
+def connect():
+    ser = serial.Serial('/dev/ttyUSB0',38400,timeout=None)
+    time.sleep(10)      # Nach dem Öffnen 10 Sekunden warten
+    ser.write('connect\n'.encode())
+    ser.readline().decode()
+    return ser
+
+def move_pose(ser,w,gripServo,duration):
+    BASE_MIN = -2.6112
+    BASE_MAX = 2.6061
+    SHOULDER_MIN = -1.5708
+    SHOULDER_MAX = 1.5708
+    ELBOW_MIN = -2.0043
+    ELBOW_MAX = 1.5555
+    WRIST_MIN = -1.6218
+    WRIST_MAX = 1.6779
+    WROT_MIN = -2.6112
+    WROT_MAX = 2.6061
+    GRIP_MIN = 0
+    GRIP_MAX = 511
+    DURATION_MIN = 250
+    DURATION_MAX = 65535
+    
+    ###################### Code-Teil von Shefqet Grdlay
+    v = np.zeros(5)
+    d = [111.6, 0, 0, 0, 155]
+    a = [0, 148.22, 147, 0, 0]
+    v[0] = np.arctan2(w[1], w[0])
+    v234 = np.arctan2(-(np.cos(v[0]) * w[3] + np.sin(v[0]) * w[4]), -w[5])
+    b1 = np.cos(v[0]) * w[0] + np.sin(v[0]) * w[1] + d[4] * np.sin(v234)
+    b2 = d[0] - d[4] * np.cos(v234) - w[2]
+
+    v[2] = np.arccos((np.square(np.linalg.norm([[b1], [b2]])) - np.square(a[1]) - np.square(a[2])) / (2 * a[1] * a[2]))
+    v[1] = np.arctan2((a[1] + a[2] * np.cos(v[2])) * b2 - a[2] * np.sin(v[2]) * b1,
+                      (a[1] + a[2] * np.cos(v[2])) * b1 + a[2] * np.sin(v[2]) * b2)
+
+    v[3] = v234 - v[1] - v[2]
+    v[4] = np.pi * np.log(np.sqrt(np.square(w[3]) + np.square(w[4]) + np.square(w[5])))
+    v[1] = v[1] + 1.315014068517125
+    v[2] = v[2] - 1.314998850007427
+    v[3] = v[3] + 1.570781108285199
+
+    ######################
+    
+    # Überprüfung der zulässigen Werte
+    if v[0] < BASE_MIN:
+        return "False"
+    if v[0] > BASE_MAX:
+        return "False"
+    if v[1] < SHOULDER_MIN:
+        return "False"
+    if v[1] > SHOULDER_MAX:
+        return "False"
+    if v[2] < ELBOW_MIN:
+        return "False"
+    if v[2] > ELBOW_MAX:
+        return "False"
+    if v[3] < WRIST_MIN:
+        return "False"
+    if v[3] > WRIST_MAX:
+        return "False"
+    if v[4] < WROT_MIN:
+        return "False"
+    if v[4] > WROT_MAX:
+        return "False"
+    if gripServo < GRIP_MIN:
+        return "False"
+    if gripServo > GRIP_MAX:
+        return "False"
+    if duration < DURATION_MIN:
+        return "False"
+    if duration > DURATION_MAX:
+        return "False"
+    
+    if np.any(np.isinf(v)) or np.any(np.isnan(v)):        # Probe auf Inf und NaN
+        return "False"
+    else:
+        resp = set_joint_angles(ser,v[0],v[1],v[2],v[3],v[4],gripServo,duration)
+        return resp
+
+def set_joint_angles(ser,baseServo,shoulderServo,elbowServo,wristServo,wristRotServo,gripServo,duration):
+    msg = (f"move {baseServo:.4f} {shoulderServo:.4f} {elbowServo:.4f} {wristServo:.4f} {wristRotServo:.4f} {int(gripServo)} {int(duration)}\n")
+    ser.write(msg.encode())
+    resp = ser.readline().decode()
+    return resp
+
+def get_joint_angles(ser):
+    ser.write('getJointAngles\n'.encode())
+    resp = list()
+    for i in range(0,6):
+        msg = ser.readline().decode()
+        resp.append(msg)
+    return resp
+
+def sleep(ser):
+    ser.write('sleep\n'.encode())
+
+def disconnect(ser):
+    ser.write('disconnect\n'.encode())
+    ser.readline().decode()
+    ser.close()