NCVCの作者のページ

GyroBoyでライントレース

GyroBoyでライントレース

 ここに載せるべきかどうか疑問が残りますがww 一応 Python のデモということで,LEGO MINDSTORM EV3 の MicroPython を使って動作させています. オリジナルは本家Webページから. 処理速度が遅いと言われる Python ですが,がんばってバランスとっています.

プログラム

 ほぼ本家と同じですが, 超音波センサは使っていませんし,光センサの役目も変わっているので,不要な部分は除いています. 光センサの状態によって表情を変えたかったのですが,それをすると処理が追い付かずに倒れてしまったので,コメントアウトしました.


#!/usr/bin/env pybricks-micropython

from ucollections import namedtuple
from pybricks.hubs import EV3Brick
from pybricks.ev3devices import Motor, TouchSensor, ColorSensor, GyroSensor
from pybricks.parameters import Port, Color, ImageFile, SoundFile
from pybricks.tools import wait, StopWatch

# Initialize the EV3 brick.
ev3 = EV3Brick()

# Initialize the motors connected to the drive wheels.
left_motor = Motor(Port.D)
right_motor = Motor(Port.A)

# Initialize the motor connected to the arms.
arm_motor = Motor(Port.C)

# Initialize the Color Sensor. It is used to detect the colors that command
# which way the robot should move.
Rcolor_sensor = ColorSensor(Port.S1)
Lcolor_sensor = ColorSensor(Port.S3)

# Initialize the gyro sensor. It is used to provide feedback for balancing the
# robot.
gyro_sensor = GyroSensor(Port.S2)

# Initialize the touch sensor.
touch_sensor = TouchSensor(Port.S4)

# Initialize the timers.
fall_timer = StopWatch()
single_loop_timer = StopWatch()
control_loop_timer = StopWatch()
action_timer = StopWatch()

# The following (UPPERCASE names) are constants that control how the program
# behaves.
GYRO_CALIBRATION_LOOP_COUNT = 200
GYRO_OFFSET_FACTOR = 0.0005
TARGET_LOOP_PERIOD = 15  # ms
ARM_MOTOR_SPEED = 600  # deg/s

SPEED = 300
STEER = 150

# If we fall over in the middle of an action, the arm motors could be moving or
# the speaker could be beeping, so we need to stop both of those.
def stop_action():
    ev3.speaker.beep(0, -1)
    arm_motor.run_target(ARM_MOTOR_SPEED, 0)

while True:
    # Sleeping eyes and light off let us know that the robot is waiting for
    # any movement to stop before the program can continue.
    ev3.screen.load_image(ImageFile.SLEEPING)
    ev3.light.off()

    # Reset the sensors and variables.
    left_motor.reset_angle(0)
    right_motor.reset_angle(0)
    fall_timer.reset()

    motor_position_sum = 0
    wheel_angle = 0
    motor_position_change = [0, 0, 0, 0]
    drive_speed, steering = SPEED, 0  # FORWARD_FAST
    control_loop_count = 0
    robot_body_angle = -0.25

    # Calibrate the gyro offset. This makes sure that the robot is perfectly
    # still by making sure that the measured rate does not fluctuate more than
    # 2 deg/s. Gyro drift can cause the rate to be non-zero even when the robot
    # is not moving, so we save that value for use later.
    while True:
        gyro_minimum_rate, gyro_maximum_rate = 440, -440
        gyro_sum = 0
        for _ in range(GYRO_CALIBRATION_LOOP_COUNT):
            gyro_sensor_value = gyro_sensor.speed()
            gyro_sum += gyro_sensor_value
            if gyro_sensor_value > gyro_maximum_rate:
                gyro_maximum_rate = gyro_sensor_value
            if gyro_sensor_value < gyro_minimum_rate:
                gyro_minimum_rate = gyro_sensor_value
            wait(5)
        if gyro_maximum_rate - gyro_minimum_rate < 2:
            break
    gyro_offset = gyro_sum / GYRO_CALIBRATION_LOOP_COUNT
#    gyro_offset = 0

    # Awake eyes and green light let us know that the robot is ready to go!
    ev3.screen.load_image(ImageFile.ANGRY)
    ev3.light.on(Color.GREEN)
    ev3.speaker.play_file(SoundFile.SPEED_UP)

#    gyro_sensor.reset_angle(0)

    # Main control loop for balancing the robot.
    while True:

        # Color Sensor
        cr = Rcolor_sensor.reflection()
        cl = Lcolor_sensor.reflection()
        if cr > 25:
            if cl > 25:
                steering = 0
                ds = 0
#                ev3.screen.load_image(ImageFile.STOP_1)
#                break
            else:
#                ev3.screen.load_image(ImageFile.LEFT)
                steering = -STEER   # TRUN_LEFT
#                ds = SPEED
        elif cl > 25:
#            ev3.screen.load_image(ImageFile.RIGHT)
            steering = STEER      # TRUN_RIGHT
#            ds = SPEED
#        elif steering != 0:
#            ev3.screen.load_image(ImageFile.ANGRY)
        else:
            steering = 0
            ds = SPEED

        if drive_speed != ds:
            drive_speed = ds
            if drive_speed > 0:
                ev3.screen.load_image(ImageFile.ANGRY)
            else:
                ev3.screen.load_image(ImageFile.STOP_1)

        # This timer measures how long a single loop takes. This will be used
        # to help keep the loop time consistent, even when different actions
        # are happening.
        single_loop_timer.reset()

        # This calculates the average control loop period. This is used in the
        # control feedback calculation instead of the single loop time to
        # filter out random fluctuations.
        if control_loop_count == 0:
            # The first time through the loop, we need to assign a value to
            # avoid dividing by zero later.
            average_control_loop_period = TARGET_LOOP_PERIOD / 1000
            control_loop_timer.reset()
        else:
            average_control_loop_period = (control_loop_timer.time() / 1000 /
                                           control_loop_count)
        control_loop_count += 1

        # calculate robot body angle and speed
        gyro_sensor_value = gyro_sensor.speed()
        gyro_offset *= (1 - GYRO_OFFSET_FACTOR)
        gyro_offset += GYRO_OFFSET_FACTOR * gyro_sensor_value
        robot_body_rate = gyro_sensor_value - gyro_offset
        robot_body_angle += robot_body_rate * average_control_loop_period

        # calculate wheel angle and speed
        left_motor_angle = left_motor.angle()
        right_motor_angle = right_motor.angle()
        previous_motor_sum = motor_position_sum
        motor_position_sum = left_motor_angle + right_motor_angle
        change = motor_position_sum - previous_motor_sum
        motor_position_change.insert(0, change)
        del motor_position_change[-1]
        wheel_angle += change - drive_speed * average_control_loop_period
        wheel_rate = sum(motor_position_change) / 4 / average_control_loop_period

        # This is the main control feedback calculation.
        output_power = (-0.01 * drive_speed) + (0.8 * robot_body_rate +
                                                15 * robot_body_angle +
                                                0.08 * wheel_rate +
                                                0.12 * wheel_angle)
        if output_power > 100:
            output_power = 100
        if output_power < -100:
            output_power = -100

        # Drive the motors.
        left_motor.dc(output_power - 0.1 * steering)
        right_motor.dc(output_power + 0.1 * steering)

        # Check if robot fell down. If the output speed is +/-100% for more
        # than one second, we know that we are no longer balancing properly.
        if abs(output_power) < 100:
            fall_timer.reset()
        elif fall_timer.time() > 1000:
            break

        # Make sure loop time is at least TARGET_LOOP_PERIOD. The output power
        # calculation above depends on having a certain amount of time in each
        # loop.
        wait(TARGET_LOOP_PERIOD - single_loop_timer.time())

    # Handle falling over. If we get to this point in the program, it means
    # that the robot fell over.

    # Stop all of the motors.
    stop_action()
    left_motor.stop()
    right_motor.stop()

    # Knocked out eyes and red light let us know that the robot lost its
    # balance.
    ev3.light.on(Color.RED)
#    ev3.screen.load_image(ImageFile.KNOCKED_OUT)
    ev3.screen.load_image(ImageFile.DIZZY)
    ev3.speaker.play_file(SoundFile.SPEED_DOWN)

    # Wait for a few seconds before trying to balance again.
    wait(3000)

<< 前のページに戻る