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)
