def test_Device():
print "Test Device"
d = Device("COM6", "COM7")
servos = [0, 1]
for servo_id in servos:
print "-- servo:", servo_id
print "Go Home"
d.go_home()
print "Get Errors"
print d.get_errors()
print "Get Position"
pos = d.get_position(servo_id)
print pos
newsetpos = pos - 50
print "Set Target:", newsetpos
d.set_target(servo_id, newsetpos)
d.wait_until_at_target()
newpos = d.get_position(servo_id)
print "%s==%s" % (newsetpos, newpos),
print newsetpos == newpos
newsetpos = pos + 50
print "Set Target:", newsetpos
d.set_target(servo_id, newsetpos)
d.wait_until_at_target()
newpos = d.get_position(servo_id)
print "%s==%s" % (newsetpos, newpos),
print newsetpos == newpos
d.go_home()
print d.get_errors()
del d
print "Device tested"
