def move_to_color(color_sensor, stop_on_color, speed_mm_s):
robot.drive(speed_mm_s, 0)
# Check if color reached.
while color_sensor.color() != stop_on_color:
wait(10)
robot.stop(stop_type=Stop.BRAKE)
def turn_to_color_left(color_sensor, stop_on_color, angular_speed_deg_s):
robot.drive(0, -1 * angular_speed_deg_s)
# Check if color reached.
while color_sensor.color() != stop_on_color:
wait(10)
robot.stop(stop_type=Stop.BRAKE)
def move_straight(distance_mm, speed_mm_s):
left_motor.reset_angle(0)
motor_target_angle = int(DEGREES_PER_MM * distance_mm)
robot.drive(speed_mm_s, 0)
print('done1')
wait(2700)
robot.stop(stop_type=Stop.BRAKE)
def follow_line_border(
color_sensor,
distance_mm,
speed_mm_s):
left_motor.reset_angle(0)
motor_target_angle = int(DEGREES_PER_MM * distance_mm)
while (abs(left_motor.angle()) < abs(motor_target_angle)):
wait(50)
darkness = 100 - color_sensor.reflection()
if color_sensor.color() == Color.WHITE:
robot.drive(speed_mm_s, 0)
elif color_sensor.color() == Color.BLACK:
robot.drive(speed_mm_s, 0.5 * darkness)
else :
robot.drive(speed_mm_s, -0.5 * darkness)
robot.stop(stop_type=Stop.BRAKE)
def move_to_obstacle(obstacle_sensor, stop_on_obstacle_at, speed_mm_s):
robot.drive(speed_mm_s, 0)
# Check if obstacle too close.
while obstacle_sensor.distance() > stop_on_obstacle_at:
wait(10)
robot.stop(stop_type=Stop.BRAKE)
def move_straight(max_distance, speed_mm_s=DEFAULT_SPEED):
left_motor.reset_angle(0)
right_motor.reset_angle(0)
log_string('Move stratight at speed ' + str(speed_mm_s) + ' dist ' +
str(max_distance))
duration = abs(int(1000 * max_distance / speed_mm_s))
robot.drive_time(speed_mm_s, 0, duration)
robot.stop(stop_type=Stop.BRAKE)
def turn(angle, speed_mm_s=DEFAULT_SPEED):
if angle > 0: # right turns are a bit under-steered
angle = int(1.1 * angle)
else:
angle = int(angle / 1)
robot.drive_time(0, angle, 1000)
robot.stop(stop_type=Stop.BRAKE)
def drive_raising_crane(duration_ms, robot_distance_mm, robot_turn_angle,
crane_motor, crane_angle):
crane_angular_speed = int(1000 * crane_angle / duration_ms)
turn_angular_speed_deg_s = abs(int(1000 * robot_turn_angle / duration_ms))
forward_speed = int(1000 * robot_distance_mm / duration_ms)
robot.drive(forward_speed, turn_angular_speed_deg_s)
crane_motor.run(crane_angular_speed)
wait(duration_ms)
crane_motor.stop(Stop.BRAKE)
robot.stop(stop_type=Stop.BRAKE)
def turn_to_angle( gyro, target_angle):
error = target_angle - gyro.angle()
while ( abs(error) >= 4):
adj_angular_speed = error * 1.5
robot.drive(0, adj_angular_speed)
wait(100)
error=target_angle - gyro.angle()
robot.stop(stop_type=Stop.BRAKE)
def color_test(color_sensor, distance_mm, speed_mm_s):
left_motor.reset_angle(0)
motor_target_angle = int(DEGREES_PER_MM * distance_mm)
while (abs(left_motor.angle()) < abs(motor_target_angle)):
log_string('Color : ' + str(color_sensor.color()) + ' Intense:' + str(color_sensor.reflection()))
robot.drive(speed_mm_s, 0)
wait(300)
robot.stop(stop_type=Stop.BRAKE)
def move_straight(distance_mm, speed_mm_s):
motor_target_angle = int(DEGREES_PER_MM * distance_mm)
left_motor.reset_angle(0)
robot.drive(speed_mm_s, 0)
while (abs(left_motor.angle()) < abs(motor_target_angle)):
wait(50)
robot.stop(stop_type=Stop.BRAKE)
def move_straight(distance_mm, speed_mm_s):
left_motor.reset_angle(0)
motor_target_angle = int(DEGREES_PER_MM * distance_mm)
# Keep moving till the angle of the left motor reaches target
while (abs(left_motor.angle()) < abs(motor_target_angle)):
robot.drive(speed_mm_s, 0)
wait(100)
robot.stop(stop_type=Stop.BRAKE)
def move_straight_target_direction(gyro, distance_mm, speed_mm_s, target_angle):
turn_to_angle( gyro, target_angle)
duration = abs(int(1000 * distance_mm / speed_mm_s))
time_spent=0
while (time_spent<duration):
error = target_angle - gyro.angle()
adj_angular_speed = error * 1.5
robot.drive(speed_mm_s, adj_angular_speed)
wait(200)
time_spent = time_spent + 200
robot.stop(stop_type=Stop.BRAKE)
def move_to_obstacle(distance_from_obstacle_mm, speed_mm_s):
robot.drive(speed_mm_s, 0)
message = 'Distance ' + str(ultrasound.distance())
print(message)
brick.display.text(message)
# Check if color reached.
while ultrasound.distance() > abs(distance_from_obstacle_mm):
message = 'Distance ' + str(ultrasound.distance())
print(message)
brick.display.text(message)
wait(100)
robot.stop(stop_type=Stop.BRAKE)
def moverobotcrane (
distance_mm,
angle,
crane_motor,
angle_crane
ms):
speed=(distance_mm/ms)*1000
anglespeed=(angle/ms)*1000
robot.drive(speed, anglespeed)
anglespeedcrane=(angle_crane/ms)*1000
crane_motor.run(anglespeedcrane)
wait(ms)
robot.stop(Stop.BRAKE)
crane_motor.stop(Stop.BRAKE)
def stop_with_object_at(distance_from_object_mm, max_distance_mm, speed_mm_s):
left_motor.reset_angle(0)
motor_target_angle = int(DEGREES_PER_MM * max_distance_mm)
robot.drive(speed_mm_s, 0)
message = 'Distance ' + str(ultrasound.distance())
print(message)
brick.display.text(message)
while (abs(left_motor.angle()) < abs(motor_target_angle)
and ultrasound.distance() > abs(distance_from_object_mm)):
wait(50)
robot.stop(stop_type=Stop.BRAKE)
def move_straight_target_direction_motor_angle(gyro, distance_mm, speed_mm_s, target_angle):
turn_to_angle( gyro, target_angle)
# Use a different variable name for gyro so you dont get confused
gyro_target_angle = target_angle
left_motor.reset_angle(0)
motor_target_angle = int(DEGREES_PER_MM * distance_mm)
while (abs(left_motor.angle()) < abs(motor_target_angle)):
error = target_angle - gyro.angle()
log_string('Gyro :' + str(gyro.angle()) + ' err: '+ str(error))
adj_angular_speed = error * 1.5
robot.drive(speed_mm_s, adj_angular_speed)
wait(50)
robot.stop(stop_type=Stop.BRAKE)
def follow_line_border(color_sensor, distance_mm, speed_mm_s, border_color,
color_on_left):
left_motor.reset_angle(0)
motor_target_angle = int(DEGREES_PER_MM * distance_mm)
# Keep moving till the angle of the left motor reaches target
while (abs(left_motor.angle()) < abs(motor_target_angle)):
error = 100 - color_sensor.reflection()
if color_sensor.color() == border_color:
robot.drive(speed_mm_s, 0)
elif color_sensor.color() == color_on_left:
robot.drive(speed_mm_s, 0.5 * error)
else:
robot.drive(speed_mm_s, -0.5 * error)
wait(50)
robot.stop(stop_type=Stop.BRAKE)
def follow_line_border(color_sensor, distance_mm, speed_mm_s):
left_motor.reset_angle(0)
motor_target_angle = int(DEGREES_PER_MM * distance_mm)
target_intensity = color_sensor.reflection()
# Keep moving till the angle of the left motor reaches target
while (abs(left_motor.angle()) < abs(motor_target_angle)):
darkness = target_intensity - color_sensor.reflection()
if color_sensor.color() == Color.WHITE:
robot.drive(speed_mm_s, 0)
elif color_sensor.color() == Color.BLACK:
robot.drive(speed_mm_s, abs(darkness))
else:
robot.drive(speed_mm_s, -1 * abs(darkness))
wait(250)
robot.stop(stop_type=Stop.BRAKE)
def move_to_wall(touch_sensor, speed_mm_s):
robot.drive(speed_mm_s, 0)
# Check if touch sensore pressed.
while touch_sensor.pressed() == False :
wait(10)
robot.stop(stop_type=Stop.BRAKE)
def run_to_home():
turn_to_direction(gyro, target_angle=190)
robot.drive(400, 0)
while left_motor.stalled() != True:
wait(100)
robot.stop()
