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 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 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 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 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 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(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 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_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 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 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):
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 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_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('done')
from robot_setup import SOUND_VOLUME
from robot_setup import WHEEL_DIAMETER_MM
from robot_setup import AXLE_TRACK_MM
from robot_setup import SENSOR_TO_AXLE
from robot_setup import WHEEL_CIRCUM_MM
from robot_setup import DEGREES_PER_MM
##### Do not change above this line ##########################################
def move_crane_up(crane_motor, degrees):
crane_motor.run_angle(90, degrees, Stop.BRAKE)
def move_crane_down(crane_motor, degrees):
crane_motor.run_angle(90, -1 * degrees, Stop.BRAKE)
def move_crane_to_floor(crane_motor):
crane_motor.run_until_stalled(-180, Stop.COAST, 35)
move_crane_up(crane_motor, degrees=5)
def move_crane_to_top(crane_motor):
crane_motor.run_until_stalled(180, Stop.COAST, 35)
move_crane_to_floor(crane_motor)
robot.drive(100, 0)
from robot_setup import right_motor
from robot_setup import robot
from robot_setup import rack_motor
from robot_setup import crane_motor
from robot_setup import gyro
from robot_setup import touch_sensor
from robot_setup import color_sensor_left
from robot_setup import color_sensor_right
from robot_setup import color_sensor_center
from robot_setup import touch_sensor
from robot_setup import SOUND_VOLUME
from robot_setup import WHEEL_DIAMETER_MM
from robot_setup import AXLE_TRACK_MM
from robot_setup import SENSOR_TO_AXLE
from robot_setup import WHEEL_CIRCUM_MM
from robot_setup import DEGREES_PER_MM
# Get wheel circumference
WHEEL_CIRCUM_MM = 3.149 * 89
# 360 degrees -> WHEEL_CIRCUM_MM so 1 degree -> ?
DEGREES_PER_MM = 360 / WHEEL_CIRCUM_MM
motor_target_angle = int(DEGREES_PER_MM * distance_mm)
left_motor.reset_angle(0)
while (abs(left_motor.angle()) < abs(motor_target_angle)):
error = target_angle - gyro.angle()
robot.drive(speed_mm_s, error)
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()
