def base_bumpfollow_right(): if bumpPressedRight(): activate_motors(c.LFOLLOW_SMOOTH_LM_POWER, c.BASE_RM_POWER) else: activate_motors(c.BASE_LM_POWER, c.LFOLLOW_SMOOTH_RM_POWER) msleep(1) g.update_gyro()
def backwards_tics(tics, should_stop=True): cmpc(c.LEFT_MOTOR) cmpc(c.RIGHT_MOTOR) activate_motors(-1 * c.BASE_LM_POWER, -1 * c.BASE_RM_POWER) while gmpc(c.LEFT_MOTOR) > -1 * tics and gmpc(c.RIGHT_MOTOR) < tics: msleep(1) g.update_gyro() if should_stop: deactivate_motors()
def av(motor_port, desired_velocity): # Revs a motor up to a given velocity from 0. The motor and desired velocity must be specified. Cannot rev two motors simultaneously. rev = False if desired_velocity > 1450: desired_velocity = 1450 elif desired_velocity < -1450: desired_velocity = -1450 elif desired_velocity < 1 and desired_velocity >= 0: desired_velocity = 1 elif desired_velocity > -1 and desired_velocity < 0: desired_velocity = -1 if motor_port == c.LEFT_MOTOR: intermediate_velocity = c.CURRENT_LM_POWER if abs(desired_velocity - c.CURRENT_LM_POWER) > 600 or c.CURRENT_LM_POWER == 0: rev = True elif motor_port == c.RIGHT_MOTOR: intermediate_velocity = c.CURRENT_RM_POWER if abs(desired_velocity - c.CURRENT_RM_POWER) > 600 or c.CURRENT_RM_POWER == 0: rev = True else: print "An invalid motor port was selected." if rev == True: velocity_change = desired_velocity / 30.0 while abs(intermediate_velocity - desired_velocity) > 100: mav(motor_port, int(intermediate_velocity)) if motor_port == c.LEFT_MOTOR: c.CURRENT_LM_POWER = desired_velocity elif motor_port == c.RIGHT_MOTOR: c.CURRENT_RM_POWER = desired_velocity intermediate_velocity += velocity_change msleep(1) g.update_gyro() mav(motor_port, int(desired_velocity)) # Ensures actual desired value is reached if motor_port == c.LEFT_MOTOR: c.CURRENT_LM_POWER = desired_velocity elif motor_port == c.RIGHT_MOTOR: c.CURRENT_RM_POWER = desired_velocity
def activate_motors(left_motor_power=c.BASE_VALUE, right_motor_power=c.BASE_VALUE): if left_motor_power == c.BASE_VALUE: left_motor_power = c.BASE_LM_POWER if right_motor_power == c.BASE_VALUE: right_motor_power = c.BASE_RM_POWER if left_motor_power > 1450: left_motor_power = 1450 elif left_motor_power < -1450: left_motor_power = -1450 elif left_motor_power < 1 and left_motor_power >= 0: left_motor_power = 1 elif left_motor_power > -1 and left_motor_power < 0: left_motor_power = -1 if right_motor_power < -1450: right_motor_power = -1450 elif right_motor_power > 1450: right_motor_power = 1450 elif right_motor_power < 1 and right_motor_power >= 0: right_motor_power = 1 elif right_motor_power > -1 and right_motor_power < 0: right_motor_power = -1 if abs(left_motor_power - c.CURRENT_LM_POWER) > 600 or abs( right_motor_power - c.CURRENT_RM_POWER ) > 600 or c.CURRENT_LM_POWER == 0 or c.CURRENT_RM_POWER == 0: left_velocity_change = (left_motor_power - c.CURRENT_LM_POWER) / 30 right_velocity_change = (right_motor_power - c.CURRENT_RM_POWER) / 30 while abs(c.CURRENT_LM_POWER - left_motor_power) > 100 and abs( c.CURRENT_RM_POWER - right_motor_power) > 100: mav(c.LEFT_MOTOR, int(c.CURRENT_LM_POWER)) c.CURRENT_LM_POWER += left_velocity_change mav(c.RIGHT_MOTOR, int(c.CURRENT_RM_POWER)) c.CURRENT_RM_POWER += right_velocity_change msleep(1) g.update_gyro() mav(c.LEFT_MOTOR, int(left_motor_power)) # Ensures actual desired value is reached. mav(c.RIGHT_MOTOR, int(right_motor_power)) c.CURRENT_LM_POWER = left_motor_power c.CURRENT_RM_POWER = right_motor_power
def move_servo(servo_port, desired_servo_position, tics=3, ms=1): # Moves a servo to a given position from its current position. The servo and desired position must be specified. # Servo move speed = tics / ms # >18 tics is too high intermediate_position = get_servo_position(servo_port) print "Starting move_servo()" print "Servo current position = %d" % get_servo_position(servo_port) print "Servo desired position = %d" % desired_servo_position if desired_servo_position > c.MAX_SERVO_POS: print "Invalid desired servo position\n" exit(86) if desired_servo_position < c.MIN_SERVO_POS: print "Invalid desired servo position\n" exit(86) print "Speed = " + str(tics) + "/" + str(ms) + " tics per ms" if tics > 18: print "Tic value is too high\n" exit(86) while abs(get_servo_position(servo_port) - desired_servo_position) > 10: # Tolerance of +/- 10 included to account for servo value skipping if (get_servo_position(servo_port) - desired_servo_position) >= 1: set_servo_position(servo_port, intermediate_position) intermediate_position -= tics elif (get_servo_position(servo_port) - desired_servo_position) < 1: set_servo_position(servo_port, intermediate_position) intermediate_position += tics else: break msleep(ms) g.update_gyro() set_servo_position( servo_port, desired_servo_position ) # Ensures actual desired value is reached. Should be a minor point change msleep(30) print "Desired position reached. Curent position is %d" % get_servo_position( servo_port) print "Completed servo_slow\n"