diff --git a/phantomController.py b/phantomController.py index 3577427903124d0679cfae6b05365963542f99b7..2d187d00056c331c66c4ea831b513b7d7d6badfc 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())