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"