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_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 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 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)
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