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