def move_two_servos_timed(servo1, endPos1, servo2, endPos2, time):
if time == 0:
speed = 2047
else:
delta1 = endPos1 - get_servo_position(servo1)
delta2 = endPos2 - get_servo_position(servo2)
speed1 = (DELAY * delta1) / time
speed2 = (DELAY * delta2) / time
moves = delta1 / speed1
for x in range(1, abs(int(moves))):
set_servo_position(servo1, get_servo_position(servo1) + speed1 * x)
set_servo_position(servo2, get_servo_position(servo2) + speed2 * x)
msleep(DELAY)
set_servo_position(servo1, endPos1)
set_servo_position(servo2, endPos2)
def move_servo_timed(servo, endPos,
time): # Moves a servo over a specific time.
if time == 0:
speed = 2047
else:
speed = abs((DELAY * (get_servo_position(servo) - endPos)) / time)
move_servo(servo, endPos, speed)
def move_bin(armIn, speed=10): # 1263
if armIn < 5:
armEnd = 5
elif armIn > 2040:
armEnd = 2040
else:
armEnd = armIn
joint_start = get_servo_position(c.SERVO_JOINT) # 1750
arm_start = get_servo_position(c.SERVO_BIN_ARM) # 700
delta = armEnd - arm_start # 563
for shift in range(0, abs(delta), speed):
if delta < 0:
shift = -shift # handles negative values (moving bin downwards)
set_servo_position(c.SERVO_BIN_ARM, arm_start + shift)
set_joint = joint_start + shift
if set_joint > 1900:
set_joint = 1900
# return
elif set_joint < 5:
set_joint = 5
# return
set_servo_position(c.SERVO_JOINT, set_joint)
display("{}\t{}".format(get_servo_position(c.SERVO_JOINT),
get_servo_position(c.SERVO_BIN_ARM)))
msleep(DELAY)
set_joint = joint_start + delta
if set_joint > 1900:
set_joint = 1900
elif set_joint < 5:
set_joint = 5
set_servo_position(c.SERVO_BIN_ARM, armEnd)
set_servo_position(c.SERVO_JOINT, set_joint)
def moveServo(servo, endPos, speed=10):
# speed of 1 is slow
# speed of 2000 is fast
# speed of 10 is the default
now = get_servo_position(servo)
if now > 2048 :
PROGRAMMER_ERROR ("Servo setting too large")
if now < 0 :
PROGRAMMER_ERROR ("Servo setting too small")
if now > endPos:
speed = -speed
for i in range (now, endPos, speed):
set_servo_position(servo, i)
msleep(10)
set_servo_position(servo, endPos)
msleep(10)
def move_servo(servo, endPos, speed=10):
# speed of 1 is slow
# speed of 2000 is fast
# speed of 10 is the default
now = get_servo_position(servo)
if speed == 0:
speed = 2047
if endPos > 2040:
display("using 2040")
endPos = 2040
if endPos < 5:
display("using 5")
endPos = 5
if now > endPos:
speed = -speed
for i in range(int(now), int(endPos), int(speed)):
set_servo_position(servo, i)
msleep(DELAY)
set_servo_position(servo, endPos)
msleep(DELAY)
def move_servo(servo, endPos, speed=10): # Moves a servo with increment "speed".
# speed of 1 is slow
# speed of 2000 is fast
# speed of 10 is the default
now = get_servo_position(servo)
if speed == 0:
speed = 2047
if endPos >= 2048:
print("Programmer Error")
exit(0)
if endPos < 0:
print("Programmer Error")
exit(0)
if now > endPos:
speed = -speed
for i in range(int(now), int(endPos), int(speed)):
set_servo_position(servo, i)
msleep(DELAY)
set_servo_position(servo, endPos)
msleep(DELAY)
def move_servo_timed(servo, endPos, time):
if time == 0:
speed = 2047
else:
speed = abs((DELAY * (get_servo_position(servo) - endPos)) / time)
move_servo(servo, endPos, speed)
def centerX_servo(channel, greatest, tolerance, speed): # we can change tolerance if w.get_object_center_x(channel, greatest) < (w.get_camera_width() / 2.0) - tolerance: w.set_servo_position(0, w.get_servo_position(0)+speed) elif w.get_object_center_x(channel, greatest) > (w.get_camera_width() / 2.0) + tolerance: w.set_servo_position(0, w.get_servo_position(0)-speed)
