Formatierung und Gerade bewegung

phantomController.py

@@ -9,7 +9,7 @@ def connect():
     ser.readline().decode()
     return ser
 
-def move_pose(ser,w,gripServo,duration):
+def move_pose(ser,w,gripServo,duration, straight):
     BASE_MIN = -2.6112
     BASE_MAX = 2.6061
     SHOULDER_MIN = -1.5708
@@ -44,37 +44,24 @@ def move_pose(ser,w,gripServo,duration):
     v[2] = v[2] - 1.314998850007427
     v[3] = v[3] + 1.570781108285199
 
-    ######################
+    if straight:
+        v[4] = v[0]
     
     # Ü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 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"
@@ -82,6 +69,7 @@ def move_pose(ser,w,gripServo,duration):
         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())