else: print("step cannot be made any larger than 400mm") #shutdown program if ControlId == 12: break if step_needed == True: try: cj = kin.inverseKinematics(pose, cj) if cj == -1: # if no solution is found dont adjust cj or pose cj = list(input_cj) pose = list(last_valid_pose) print("new angles ", kin.native2kin_angles(cj)) except: print("could not print for some reason", cj) arm.standUp() while((not arm.armCalibrated()) or (arm.checkMovement())): time.sleep(1) arm.stop() else: print("Initial arrangment is invalid")
import json #Clear Log File open('logs/log.txt', 'w').close() with open('config/config.json') as json_file: config = json.load(json_file) arm = Arm(config) angle = 70 arm.calibrateArm() while not arm.armCalibrated(): time.sleep(0.5) while arm.checkMovement(): time.sleep(0.5) arm.setSpeed(0,90) arm.setSpeed(1,90) arm.setSpeed(2,90) arm.setSpeed(3,200) arm.setSpeed(4,200) arm.setSpeed(5,200) while arm.connected: arm.moveJoint(0,angle) arm.moveJoint(1,angle) arm.moveJoint(2,angle) arm.moveJoint(3,angle)