def color_test(robot, color_sensor): common_methods.log_string('color ' + str(color_sensor.color()) + ' intens:' + str(color_sensor.reflection())) robot.drive(50, 0) for x in range(20): common_methods.log_string('color ' + str(color_sensor.color()) + ' intens:' + str(color_sensor.reflection())) wait(100) robot.stop(stop_type=Stop.BRAKE)
def follow_line_dark(robot, color_sensor, max_distance = 0, stop_on_color=None, line_color = Color.BLACK, border_color = Color.WHITE, speed_mm_s = DEFAULT_SPEED): follow_attempts = 0 while follow_attempts < 2: follow_attempts += 1 if ( True != search_for_color(robot, color_sensor, line_color)): common_methods.log_string('Could not find line to follow') return False if ( True != align_with_line(robot, color_sensor, line_color)): common_methods.log_string('Could not align with line') return False if follow_line_after_alignment(robot, color_sensor, max_distance = 0, stop_on_color=None, line_color = Color.BLACK, border_color = Color.WHITE, speed_mm_s = speed_mm_s): return True return False
def turn_to_direction(robot, gyro, target_angle, speed_mm_s = DEFAULT_SPEED): start_angle = gyro.angle() angle_change = target_angle - start_angle if (angle_change >180 ): angle_change = angle_change - 360 if (angle_change < -180 ): angle_change = angle_change + 360 target_angle = angle_change + start_angle robot.drive_time(0, 0.9 * angle_change, 1000) robot.stop(stop_type=Stop.BRAKE) max_attempts=10 # limit oscialltions to 10, not forever while ( abs(target_angle - gyro.angle()) > 1 and max_attempts >0): error=target_angle - gyro.angle() adj_angular_speed = error * 1.5 robot.drive(0, adj_angular_speed) wait(100) max_attempts -= 1 robot.stop(stop_type=Stop.BRAKE) adjusted_angle = gyro.angle() common_methods.log_string('turn_to_direction -- Adjusted target: ' + str(target_angle) + ' now: ' + str(adjusted_angle) + ' remain attempts : ' + str(max_attempts) )
def move_to_color(robot, color_sensor, stop_on_color, speed_mm_s = DEFAULT_COLOR_FIND_SPEED): robot.drive(speed_mm_s, 0) # Check if color reached. while color_sensor.color() != stop_on_color: common_methods.log_string('color: ' + str(color_sensor.color()) + ' intens: ' + str(color_sensor.reflection())) wait(10) robot.stop(stop_type=Stop.BRAKE)
def follow_line_border(robot, color_sensor, max_distance = 0, stop_on_color=None, line_color = Color.BLACK, border_color = Color.WHITE, speed_mm_s = DEFAULT_LINEFOLLOW_SPEED): if ( True != search_for_color(robot, color_sensor, border_color)): common_methods.log_string('follow_line_border :Could not find line to follow') return False target_intensity = color_sensor.reflection() follow_speed_mm_s = min(speed_mm_s, DEFAULT_LINEFOLLOW_SPEED) # line follow speed is slower sample_distance_mm = 10 # sample every 1cm to check on track interval = sample_distance_mm / (speed_mm_s/1000) # millisecpnds to sample max_duration = 1000 * int(max_distance / speed_mm_s) cum_duration = 0 intensity = color_sensor.reflection() off_track_cnt=0 while True: intensity = color_sensor.reflection() current_color = color_sensor.color() error = target_intensity - intensity if current_color != line_color and current_color != border_color: turn = 1 #right elif current_color == border_color: turn = 0 elif current_color == line_color : turn = -1 #left robot.drive(follow_speed_mm_s, turn * abs(error)) wait(interval) cum_duration += interval print(' intensity ' + str(intensity) + ' error ' + str(error) + ' color ' + str(current_color) + ' turned ' + str(turn) + ' cum_dist ' + str(int((cum_duration * speed_mm_s)/1000)) ) # Check any endng conditions being met if ((max_distance > 0 and cum_duration >= max_duration) or (stop_on_color and color_sensor.color() == stop_on_color)): robot.stop(stop_type=Stop.BRAKE) print('Stopping as end met') common_methods.sound_happy() return True prev_intensity = intensity prev_turn = turn
def align_with_line(robot, color_sensor, line_color = Color.BLACK): # If not already on the line then search for it - need to start on line if color_sensor.color() != line_color: if ( True != search_for_color(robot, color_sensor, line_color)): common_methods.log_string('Could not find line to follow') return False steering = 0 while steering <=30: #jump forward 5cm and check if on line color common_methods.log_string('align right ' + ' steer ' + str(steering)) robot.drive_time(100, steering, 500) if color_sensor.color() == line_color: robot.stop(stop_type=Stop.BRAKE) return True else: common_methods.log_string('align right back up ' + ' steer ' + str(steering)) robot.drive_time(-100, steering, 500) # Check the other side if steering > 0 : common_methods.log_string('align left ' + ' steer ' + str(steering)) robot.drive_time(100, -1 * steering, 500) if color_sensor.color() == line_color: robot.stop(stop_type=Stop.BRAKE) return True else: common_methods.log_string('align left back up ' + ' steer ' + str(steering)) robot.drive_time(-100, -1 * steering, 500) steering += 7 # Check 7 degrees on either side next return False
def search_for_color(robot, color_sensor, stop_on_color): if color_sensor.color() == stop_on_color: # if already there return True forward_steps =0 while forward_steps < 3: sweep_width = 1 sweep_attempts = 0 sweep_speed = 45 while sweep_attempts < 5: common_methods.log_string('Sweep sweep_width ' + str(sweep_width)) robot.drive_time(0, sweep_speed, sweep_width * 100) #sweep right if color_sensor.color() == stop_on_color: robot.stop(stop_type=Stop.BRAKE) return True robot.drive_time(0, -1 * sweep_speed, sweep_width * 100) #sweep left if color_sensor.color() == stop_on_color: robot.stop(stop_type=Stop.BRAKE) return True sweep_width += 1 sweep_attempts += 1 # reset to point at mid point robot.drive_time(0, sweep_speed, int(sweep_width * 100 / 2)) # step forward by 1 cm to sweep again robot.drive_time(100, 0, 100) forward_steps += 1 common_methods.sound_alarm() return False
def gyro_color_test_angled(robot, gyro, color_sensor, crane_motor): common_methods.log_string('Pre-run gyro speed ' + str(gyro.speed()) + ' angle:' + str(gyro.angle())) worker.move_crane_to_floor(robot, crane_motor) worker.move_crane_up(robot, crane_motor, degrees=135) move_robot.move_straight(robot=robot, max_distance=800) common_methods.log_string('post Run speed ' + str(gyro.speed()) + ' angle:' + str(gyro.angle())) move_robot.turn_to_direction(robot, gyro, target_angle=45) common_methods.log_string('post +45 turn gyro speed ' + str(gyro.speed()) + ' angle:' + str(gyro.angle())) move_robot.move_to_color(robot=robot,color_sensor=color_sensor, stop_on_color=Color.BLACK, speed_mm_s = 70) move_robot.turn_to_direction(robot, gyro, target_angle=90) common_methods.log_string('post +90 turn gyro speed ' + str(gyro.speed()) + ' angle:' + str(gyro.angle())) move_robot.move_straight(robot=robot, max_distance=100) worker.move_crane_to_floor(robot, crane_motor) worker.move_crane_up(robot, crane_motor, degrees=45) worker.move_crane_to_floor(robot, crane_motor) move_robot.move_reverse(robot=robot, max_distance=100) worker.move_crane_up(robot, crane_motor, degrees=135) move_robot.turn_to_direction(robot, gyro, target_angle=210) common_methods.log_string('post +210 turn gyro speed ' + str(gyro.speed()) + ' angle:' + str(gyro.angle())) move_robot.move_straight(robot=robot, max_distance=700) common_methods.log_string('post Run speed ' + str(gyro.speed()) + ' angle:' + str(gyro.angle()))
def gyro_test(robot, gyro): common_methods.log_string('Pre-run gyro speed ' + str(gyro.speed()) + ' angle:' + str(gyro.angle())) move_robot.move_straight(robot=robot, max_distance=300) common_methods.log_string('post Run speed ' + str(gyro.speed()) + ' angle:' + str(gyro.angle())) move_robot.turn_to_direction(robot, gyro, target_angle=90) common_methods.log_string('post +90 turn gyro speed ' + str(gyro.speed()) + ' angle:' + str(gyro.angle())) move_robot.turn_to_direction(robot=robot, gyro=gyro, target_angle=180) common_methods.log_string('post U turn gyro speed ' + str(gyro.speed()) + ' angle:' + str(gyro.angle())) move_robot.move_straight(robot=robot, max_distance=300) common_methods.log_string('post st. run gyro speed ' + str(gyro.speed()) + ' angle:' + str(gyro.angle())) move_robot.turn_to_direction(robot=robot, gyro=gyro, target_angle=-90) common_methods.log_string('post -90 turn gyro speed ' + str(gyro.speed()) + ' angle:' + str(gyro.angle()))
def follow_line_after_alignment(robot, color_sensor, max_distance = 0, stop_on_color=None, line_color = Color.BLACK, border_color = Color.WHITE, speed_mm_s = DEFAULT_SPEED): speed_mm_s = 100 # line follow speed is slower sample_distance_mm =10 interval = sample_distance_mm / (speed_mm_s/1000) # millisecpnds to sample max_duration = 1000 * int(max_distance / speed_mm_s) cum_duration = 0 intensity = color_sensor.reflection() prev_intensity=intensity prev_turn=0 last_turn_toward_goal=0 while True: intensity = color_sensor.reflection() delta_intensity= intensity - prev_intensity current_color = color_sensor.color() if current_color != line_color and current_color != border_color: if ( True != search_for_color(robot, color_sensor, line_color)): common_methods.log_string('follow_line_after_alignment :Could not find line to follow') return False else: continue elif delta_intensity == 0: turn=0 elif delta_intensity < 0: last_turn_toward_goal = prev_turn #save the last turn which took it closer turn=0 else: #Do the opposite of the prev turn or the opposite of the last turn with gain if prev_turn == 0: if last_turn_toward_goal == 0: if ( True != search_for_color(robot, color_sensor, line_color)): common_methods.log_string('follow_line_after_alignment :no prev turn, search') return False else: continue else: turn = -1 * last_turn_toward_goal else: turn = -1 * prev_turn robot.drive(speed_mm_s, turn) wait(interval) cum_duration += interval print(' intensity ' + str(intensity) + ' prev_int ' + str(prev_intensity) + ' delta_intensity ' + str(delta_intensity) + ' color ' + str(color_sensor.color()) + ' turned ' + str(turn) + ' prev_trn ' + str(prev_turn) + ' cum_dist ' + str(int((cum_duration * speed_mm_s)/1000)) ) # Check any endng conditions being met if ((max_distance > 0 and cum_duration >= max_duration) or (stop_on_color and color_sensor.color() == stop_on_color)): robot.stop(stop_type=Stop.BRAKE) print('Stopping as end met') return True prev_intensity = intensity prev_turn = turn