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