Python Examples

from mindstorms import MSHub
from mindstorms.control import wait_for_seconds

# Initialize the hub
hub = MSHub()

# Display text (scrolls if longer than 1 character)
hub.light_matrix.write("HI")
wait_for_seconds(2)

# Display a smiley face image
hub.light_matrix.show_image('HAPPY')
wait_for_seconds(2)

# Create custom image (5x5 grid, values 0-9 for brightness)
custom_image = '09090:00000:09990:90009:09990'
hub.light_matrix.show(custom_image)
wait_for_seconds(2)

# Clear the display
hub.light_matrix.off()

from mindstorms import Motor
from mindstorms.control import wait_for_seconds

# Initialize motor on port A
motor = Motor('A')

# Run motor for specific degrees
motor.run_for_degrees(360)  # One full rotation
wait_for_seconds(1)

# Run motor for specific time
motor.run_for_seconds(2)  # Run for 2 seconds
wait_for_seconds(1)

# Run motor at specific speed (-100 to 100)
motor.start(50)  # 50% speed
wait_for_seconds(2)
motor.stop()

# Run to absolute position
motor.run_to_position(0)  # Return to starting position

# Get motor position
position = motor.get_position()
print("Motor position:", position)

from mindstorms import MotorPair
from mindstorms.control import wait_for_seconds

# Initialize motor pair (left on port A, right on port B)
motors = MotorPair('A', 'B')

# Set default speed
motors.set_default_speed(50)

# Move straight for distance (in cm, requires wheel size)
motors.move(20, 'cm')  # Move 20 cm forward
wait_for_seconds(1)

# Move for specific degrees
motors.move(360, 'degrees')
wait_for_seconds(1)

# Turn on spot (positive = right, negative = left)
motors.move(90, 'degrees', steering=100)  # Turn right
wait_for_seconds(1)

# Curved driving (steering: -100 to 100)
motors.start(steering=50)  # Curve right
wait_for_seconds(2)
motors.stop()

# Tank driving (control each motor separately)
motors.start_tank(50, -50)  # Spin in place
wait_for_seconds(1)
motors.stop()

from mindstorms import ColorSensor
from mindstorms.control import wait_for_seconds

# Initialize color sensor on port C
color_sensor = ColorSensor('C')

# Get detected color name
color = color_sensor.get_color()
print("Detected color:", color)
# Returns: 'black', 'violet', 'blue', 'cyan', 'green',
#          'yellow', 'red', 'white', or None

# Get reflected light intensity (0-100)
# Higher = lighter surface
reflection = color_sensor.get_reflected_light()
print("Reflected light:", reflection)

# Get ambient light intensity (0-100)
ambient = color_sensor.get_ambient_light()
print("Ambient light:", ambient)

# Get RGB values (each 0-1024)
red = color_sensor.get_red()
green = color_sensor.get_green()
blue = color_sensor.get_blue()
print(f"RGB: {red}, {green}, {blue}")

# Wait for specific color
color_sensor.wait_until_color('red')
print("Red detected!")

# Continuous color monitoring
while True:
    color = color_sensor.get_color()
    if color == 'red':
        print("STOP!")
        break
    elif color == 'green':
        print("GO!")
    wait_for_seconds(0.1)

from mindstorms import DistanceSensor, MotorPair
from mindstorms.control import wait_for_seconds

# Initialize sensors and motors
distance = DistanceSensor('D')
motors = MotorPair('A', 'B')

# Get distance in cm (returns None if nothing detected)
dist_cm = distance.get_distance_cm()
print(f"Distance: {dist_cm} cm")

# Get distance in percentage (0-100%)
dist_pct = distance.get_distance_percentage()
print(f"Distance: {dist_pct}%")

# Wait until close to object
distance.wait_for_distance_closer_than(10, 'cm')
print("Object detected within 10cm!")

# Control the sensor's LED eyes
distance.light_up_all(100)  # Full brightness
wait_for_seconds(1)
distance.light_up(100, 0, 0, 100)  # Individual eyes
wait_for_seconds(1)

# Simple obstacle avoidance robot
SAFE_DISTANCE = 20  # cm

while True:
    dist = distance.get_distance_cm()

    if dist is None or dist > SAFE_DISTANCE:
        # Safe to drive forward
        motors.start(steering=0)
    else:
        # Obstacle detected - turn right
        motors.stop()
        motors.move(180, 'degrees', steering=100)

    wait_for_seconds(0.1)

from mindstorms import ColorSensor, MotorPair
from mindstorms.control import wait_for_seconds

# Initialize
color_sensor = ColorSensor('C')
motors = MotorPair('A', 'B')

# Calibration values - adjust for your surface
BLACK = 10   # Reflected light on black line
WHITE = 90   # Reflected light on white surface
TARGET = (BLACK + WHITE) / 2  # Edge of line

# PID constants - tune these!
KP = 1.5   # Proportional gain
KI = 0.0   # Integral gain (optional)
KD = 0.0   # Derivative gain (optional)

# Speed
BASE_SPEED = 40

# Variables for PID
integral = 0
last_error = 0

while True:
    # Read sensor
    reflection = color_sensor.get_reflected_light()

    # Calculate error (how far from line edge)
    error = reflection - TARGET

    # PID calculation
    integral = integral + error
    derivative = error - last_error

    correction = (KP * error) + (KI * integral) + (KD * derivative)

    # Apply correction to steering (-100 to 100)
    steering = max(-100, min(100, correction))

    # Drive with correction
    motors.start(steering=int(steering))

    # Save for next iteration
    last_error = error

    wait_for_seconds(0.01)  # Small delay for stability

from mindstorms import MSHub, MotorPair
from mindstorms.control import wait_for_seconds

hub = MSHub()
motors = MotorPair('A', 'B')

# Reset yaw angle to 0
hub.motion_sensor.reset_yaw_angle()

# Get current angles
yaw = hub.motion_sensor.get_yaw_angle()    # Rotation around vertical axis
pitch = hub.motion_sensor.get_pitch_angle() # Tilt forward/backward
roll = hub.motion_sensor.get_roll_angle()   # Tilt left/right

print(f"Yaw: {yaw}, Pitch: {pitch}, Roll: {roll}")

# Function to turn to specific angle
def turn_to_angle(target_angle, speed=30):
    KP = 2.0

    while True:
        current = hub.motion_sensor.get_yaw_angle()
        error = target_angle - current

        # Normalize error to -180 to 180
        while error > 180:
            error -= 360
        while error < -180:
            error += 360

        if abs(error) < 2:  # Close enough
            motors.stop()
            break

        correction = KP * error
        correction = max(-100, min(100, correction))
        motors.start_tank(int(correction), int(-correction))

        wait_for_seconds(0.01)

# Function to drive straight using gyro
def drive_straight(distance_cm, speed=50):
    hub.motion_sensor.reset_yaw_angle()
    motors.move(distance_cm, 'cm', steering=0)
    # More advanced: use gyro to maintain heading while driving...

# Example usage
turn_to_angle(90)   # Turn right 90 degrees
wait_for_seconds(0.5)
turn_to_angle(0)    # Turn back to start
wait_for_seconds(0.5)
turn_to_angle(-90)  # Turn left 90 degrees