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 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_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 align_on_wall_right(): m.base_veer_right(0.5) wait_until(isRoombaBumped) m.deactivate_motors() u.halve_speeds() m.base_turn_left() wait_until(isRoombaNotBumped) m.deactivate_motors() msleep(100) g.turn_right_gyro(4) msleep(100)
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()