def deliver_ambulance_and_blocks():
w.check_zones_hospital()
if c.SAFE_HOSPITAL == c.NEAR_ZONE:
g.drive_gyro_through_line_right()
s.align_far()
m.open_claw(1, 1, c.CLAW_WAY_OPEN_POS)
msleep(500)
m.move_arm(c.ARM_HIGH_POS)
#m.move_claw(c.CLAW_WAY_OPEN_POS)
#w.close_graphics_window()
g.turn_left_gyro(190)
else:
u.halve_speeds()
g.drive_gyro_through_line_right()
s.align_far()
s.left_forwards_until_white(0)
s.left_forwards_until_black()
m.open_claw(1, 1, c.CLAW_WAY_OPEN_POS)
msleep(500)
m.move_arm(c.ARM_HIGH_POS)
#m.move_claw(c.CLAW_WAY_OPEN_POS)
u.normalize_speeds()
#w.close_graphics_window()
u.sd()
g.turn_left_gyro(190)
g.backwards_gyro_through_line_left(1100)
m.lower_ambulance_arm()
g.backwards_gyro(500)
def align_far_cliffs():
u.halve_speeds()
left_forward_untile(lcliff.senses_white)
right_forward_until(rcliff.senses_white)
left_backwards_until(lcliff.senses_black)
right_backwards_until(rcliff.senses_black)
u.normalize_speeds()
def align_close_fcliffs():
u.halve_speeds()
right_backwards_until(rfcliff.senses_white)
left_backwards_until(lfcliff.senses_white)
right_forward_until(rfcliff.senses_black)
left_forward_until(lfcliff.senses_black)
right_backwards_until(rfcliff.senses_white)
left_backwards_until(lfcliff.senses_white)
u.normalize_speeds()
def wfollow_left_smooth_until(boolean_function, *, time=c.SAFETY_TIME):
c.BASE_LM_POWER = c.FULL_LM_POWER * 1.3
sec = seconds() + time / 1000
while seconds() < sec and not(boolean_function()):
if isRoombaBumped():
m.base_veer_right(0.9)
c.FIRST_BUMP = False
else:
m.base_veer_left(0.9)
c.FIRST_BUMP = True
u.normalize_speeds()
msleep(c.LFOLLOW_REFRESH_RATE)
u.normalize_speeds()
if should_stop:
m.deactivate_motors()
def wfollow_right_smooth(time=c.SAFETY_TIME, should_stop=True):
c.BASE_LM_POWER = c.FULL_LM_POWER * 1.3
sec = seconds() + time / 1000
while seconds() < sec:
if isRoombaBumped():
m.base_veer_left(0.9)
c.FIRST_BUMP = False
else:
m.base_veer_right(0.9)
c.FIRST_BUMP = True
u.normalize_speeds()
u.change_speeds_by()
msleep(c.LFOLLOW_REFRESH_RATE)
u.normalize_speeds()
if should_stop:
m.deactivate_motors()
def forwards_gyro_wall_assisted_on_left(time=c.SAFETY_TIME, kp=1):
angle = 0
error = 0
target_angle = 0
first_bump = False
if time == 0:
time = c.SAFETY_TIME_NO_STOP
sec = seconds() + time / 1000.0
while seconds() < sec:
if s.isRoombaBumped():
if first_bump == True:
time_at_last_check = seconds()
time_on_wall = 0
time_on_wall += (seconds() - time_at_last_check)
time_at_last_check = seconds()
u.halve_speeds()
if s.isLeftBumped() and not (
s.isRightBumped()) and time_on_wall < 0.1:
print "Left is bumped, early on the wall."
target_angle -= 10
print "(Left) Target angle: " + str(target_angle)
elif s.isLeftBumped() and not (
s.isRightBumped()) and time_on_wall >= 0.1:
print "Left is bumped, late on wall."
target_angle -= 2
u.normalize_speeds()
print "(Left) Target angle: " + str(target_angle)
else:
print "Both bumped. Turning away faster."
target_angle -= 70
print "(Left) Target angle: " + str(target_angle)
first_bump = False
else:
first_bump = True
u.normalize_speeds()
left_speed = c.BASE_LM_POWER - error
right_speed = c.BASE_RM_POWER + error
m.activate_motors(left_speed, right_speed)
msleep(10)
angle += (gyro_z() - bias) * 10
error = target_angle - 0.003830106222 * kp * angle # Negative error means veering left. Positive means veering right.
if time != c.SAFETY_TIME_NO_STOP:
m.deactivate_motors()
def wfollow_right_choppy_until(boolean_function, *, time=c.SAFETY_TIME):
sec = seconds() + time / 1000
while seconds() < sec and not(boolean_function()):
if isRoombaBumped():
if isLeftBumped():
m.backwards(100)
m.turn_left()
else:
if c.FIRST_BUMP == True:
m.deactivate_motors()
u.halve_speeds()
m.base_turn_left()
c.FIRST_BUMP = False
else:
m.base_veer_right(0.6)
c.FIRST_BUMP = True
u.normalize_speeds()
msleep(c.LFOLLOW_REFRESH_RATE)
u.normalize_speeds()
if should_stop:
m.deactivate_motors()
def wfollow_right_choppy(time):
sec = seconds() + time / 1000
while seconds() < sec:
if isRoombaBumped():
if isLeftBumped():
m.backwards(100)
m.turn_left()
else:
if c.FIRST_BUMP == True:
m.deactivate_motors()
u.halve_speeds()
m.base_turn_left()
c.FIRST_BUMP = False
else:
m.base_veer_right(0.6)
c.FIRST_BUMP = True
u.normalize_speeds()
msleep(c.LFOLLOW_REFRESH_RATE)
u.normalize_speeds()
if should_stop:
m.deactivate_motors()
def forwards_gyro_wall_assisted_on_right(time=c.SAFETY_TIME, kp=1):
angle = 0
error = 0
target_angle = 0
if time == 0:
time = c.SAFETY_TIME_NO_STOP
sec = seconds() + time / 1000.0
while seconds() < sec:
if s.isRoombaBumped():
u.halve_speeds()
kp = 5
target_angle -= 0.1
print "(Right) Target angle: " + str(target_angle)
else:
u.normalize_speeds()
kp = 1
left_speed = c.BASE_LM_POWER - error
right_speed = c.BASE_RM_POWER + error
m.activate_motors(left_speed, right_speed)
msleep(100)
angle += (gyro_z() - bias) * 100
error = target_angle - 0.003830106222 * kp * angle # Negative error means veering left. Positive means veering right.
if time != c.SAFETY_TIME_NO_STOP:
m.deactivate_motors()