From 7b873fdd44c980832aada1ea2ee6ae456497935a Mon Sep 17 00:00:00 2001
From: Lukas Hoffleit <lukas.hoffleit03@gmail.com>
Date: Thu, 8 May 2025 17:16:46 +0200
Subject: [PATCH] Formatierung und Gerade bewegung
---
phantomController.py | 48 +++++++++++++++++---------------------------
1 file changed, 18 insertions(+), 30 deletions(-)
diff --git a/phantomController.py b/phantomController.py
index 3577427..2d187d0 100644
--- a/phantomController.py
+++ b/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())
--
GitLab