driver = DobotDriver('/dev/tty.usbmodem1421')
driver.Open()
# driver.Open(timeout=0.3)
kinematics = DobotKinematics()
# Offsets must be found using this tool for your Dobot once
# (rear arm, frontarm)
offsets = (1024, 1024)
def toEndEffectorHeight(rear, front):
ret = kinematics.coordinatesFromAngles(0, rear, front)
return ret[2]
while True:
ret = driver.GetAccelerometers()
if ret[0]:
if driver.isFpga():
print("Rear arm: {0:10f} | Front arm: {1:10f} | End effector height: {2:10f} | Raw rear arm: {3:4d} | Raw front arm: {4:4d}".format(\
driver.accelToAngle(ret[1], offsets[0]), driver.accelToAngle(ret[4], offsets[1]),\
toEndEffectorHeight(driver.accelToRadians(ret[1], offsets[0]), driver.accelToRadians(ret[4], offsets[1])),\
ret[1], ret[4]))
else:
print("Rear arm: {0:6.2f} | Front arm: {1:6.2f} | End effector height: {2:7.2f} | Raw rear arm: {3:6d} {4:6d} {5:6d} | Raw front arm: {6:6d} {7:6d} {8:6d}".format(\
driver.accel3DXToAngle(ret[1], ret[2], ret[3]), -driver.accel3DXToAngle(ret[4], ret[5], ret[6]),\
toEndEffectorHeight(driver.accel3DXToRadians(ret[1], ret[2], ret[3]), -driver.accel3DXToRadians(ret[4], ret[5], ret[6])),\
ret[1], ret[2], ret[3], ret[4], ret[5], ret[6]))
else:
print('Error occurred reading data')
successes = 0
i = 0
while True:
ret = driver.isReady()
if ret[0] and ret[1]:
successes += 1
if successes > 10:
print("Dobot ready!")
break
if i > 100:
raise Exception('Comm problem')
gripper = 480
toolRotation = 0
print('Accelerometer data returned', driver.GetAccelerometers())
steps1 = driver.stepsToCmdVal(27)
steps2 = driver.stepsToCmdVal(3)
steps3 = driver.stepsToCmdVal(14)
driver.SetCounters(0, 0, 0)
print("Executing 20 commands with steps 27, -3, -14. Expecting GetCounters() to return:", 27*20, -3*20, -14*20)
errors = 0
for i in range(20):
ret = (5, 0)
while not ret[1]:
ret = driver.Steps(steps1, steps2, steps3, 1, 0, 1, gripper, toolRotation)
print("ret 0: ", ret[0], " | ret 1: ", ret[1])
time.sleep(3)