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