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)