gyroboy.py

#!/usr/bin/env python3
#
# gyroboy.py - translation of the LEGO Education GyroBoy program to python for ev3dev
#
# MIT License
#
# Copyright (c) 2016 David Lechner <david@lechnology.com>
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.

import pyudev
import os
import time

# tuning constants

OFFSET_DAMPING_FACTOR = 0.0005

# shared global variables

color_sensor_value_fd = None
gyro_sensor_value_fd = None
touch_sensor_value_fd = None
ultrasonic_sensor_value_fd = None

left_motor_command_fd = None
left_motor_position_fd = None
left_motor_duty_cycle_sp_fd = None
left_motor_stop_command_fd = None
right_motor_command_fd = None
right_motor_position_fd = None
right_motor_duty_cycle_sp_fd = None
right_motor_stop_command_fd = None


def init():
    """This function does some setting up that is done automatically in the official LEGO software"""

    # Find devices in sysfs
    udev_ctx = pyudev.Context()

    # find the sensors first
    sensors = udev_ctx.list_devices(subsystem='lego-sensor')
    color_sensor_device = next(d for d in sensors if d.attributes.asstring('driver_name') == 'lego-ev3-color' and
                               d.attributes.asstring('address') == "in1")
    gyro_sensor_device = next(d for d in sensors if d.attributes.asstring('driver_name') == 'lego-ev3-gyro' and
                              d.attributes.asstring('address') == "in2")
    touch_sensor_device = next(d for d in sensors if d.attributes.asstring('driver_name') == 'lego-ev3-touch' and
                               d.attributes.asstring('address') == "in3")
    ultrasonic_sensor_device = next(d for d in sensors if d.attributes.asstring('driver_name') == 'lego-ev3-us' and
                                    d.attributes.asstring('address') == "in4")

    global color_sensor_value_fd
    global gyro_sensor_value_fd
    global touch_sensor_value_fd
    global ultrasonic_sensor_value_fd

    # get file handles for reading sensor values
    color_sensor_value_fd = os.open(os.path.join(color_sensor_device.sys_path, 'value0'), os.O_RDONLY)
    gyro_sensor_value_fd = os.open(os.path.join(gyro_sensor_device.sys_path, 'value0'), os.O_RDONLY)
    touch_sensor_value_fd = os.open(os.path.join(touch_sensor_device.sys_path, 'value0'), os.O_RDONLY)
    ultrasonic_sensor_value_fd = os.open(os.path.join(ultrasonic_sensor_device.sys_path, 'value0'), os.O_RDONLY)

    # set sensor modes here to save time in our loop
    with open(os.path.join(color_sensor_device.sys_path, 'mode'), 'w') as f:
        f.write('COL-COLOR')
    with open(os.path.join(gyro_sensor_device.sys_path, 'mode'), 'w') as f:
        f.write('GYRO-RATE')
    with open(os.path.join(touch_sensor_device.sys_path, 'mode'), 'w') as f:
        f.write('TOUCH')
    with open(os.path.join(ultrasonic_sensor_device.sys_path, 'mode'), 'w') as f:
        f.write('US-DIST-CM')

    # now for the motors
    motors = udev_ctx.list_devices(subsystem='tacho-motor')
    left_motor_device = next(d for d in motors if d.attributes.asstring('driver_name') == 'lego-ev3-l-motor' and
                             d.attributes.asstring('address') == "outA")
    right_motor_device = next(d for d in motors if d.attributes.asstring('driver_name') == 'lego-ev3-l-motor' and
                              d.attributes.asstring('address') == "outD")
    arms_motor_device = next(d for d in motors if d.attributes.asstring('driver_name') == 'lego-ev3-m-motor' and
                             d.attributes.asstring('address') == "outC")

    global left_motor_command_fd
    global left_motor_position_fd
    global left_motor_duty_cycle_sp_fd
    global left_motor_stop_command_fd
    global right_motor_command_fd
    global right_motor_position_fd
    global right_motor_duty_cycle_sp_fd
    global right_motor_stop_command_fd

    # motor file handles
    left_motor_command_fd = os.open(os.path.join(left_motor_device.sys_path, 'command'), os.O_WRONLY)
    left_motor_position_fd = os.open(os.path.join(left_motor_device.sys_path, 'position'), os.O_RDWR)
    left_motor_duty_cycle_sp_fd = os.open(os.path.join(left_motor_device.sys_path, 'duty_cycle_sp'), os.O_RDWR)
    left_motor_stop_command_fd = os.open(os.path.join(left_motor_device.sys_path, 'stop_command'), os.O_RDWR)
    right_motor_command_fd = os.open(os.path.join(right_motor_device.sys_path, 'command'), os.O_WRONLY)
    right_motor_position_fd = os.open(os.path.join(right_motor_device.sys_path, 'position'), os.O_RDWR)
    right_motor_duty_cycle_sp_fd = os.open(os.path.join(right_motor_device.sys_path, 'duty_cycle_sp'), os.O_RDWR)
    right_motor_stop_command_fd = os.open(os.path.join(left_motor_device.sys_path, 'stop_command'), os.O_RDWR)


def main():
    """This is a block-by-block translation of the official GyroBoy program. For the most part, 1 line = one block."""

    # Main (M) loop
    while True:
        # Begin RST My Block
        print("reset")
        os.pwrite(left_motor_command_fd, b'reset', 0)
        os.pwrite(right_motor_command_fd, b'reset', 0)
        # TODO: this can be removed when motor driver is fixed
        os.pwrite(left_motor_command_fd, b'stop', 0)
        os.pwrite(right_motor_command_fd, b'stop', 0)
        # calling run_direct here so we don't waste time repeating it in the loop
        os.pwrite(left_motor_command_fd, b'run-direct', 0)
        os.pwrite(right_motor_command_fd, b'run-direct', 0)
        # TODO: reset gyro sensor - probably doesn't actually make a difference though
        timer2 = time.perf_counter()
        motor_sum = 0  # mSum
        motor_position = 0  # mPos
        motor_delta = 0  # mD
        motor_delta_pos_1 = 0  # mDP1
        motor_delta_pos_2 = 0  # mDP2
        motor_delta_pos_3 = 0  # mDP3
        drive_control = 0  # Cdrv
        loop_count = 0  # Clo
        gyro_angle = 0  # gAng
        fell_over = False  # ok
        power = 0  # pwr
        state = 0  # st
        steering_control = 0  # Cstr
        # official program sets cDrv (drive_control) twice - it is omitted here
        # End RST My Block

        # TODO: display sleeping eyes image

        # Begin gOS My Block
        print("calc_gyro_offset")
        # OSL loop
        while True:
            gyro_min = 1000  # gMn
            gyro_max = -1000  # gMx
            gyro_sum = 0  # gSum
            # gChk loop
            count = 200
            for i in range(0, count):
                gyro_rate = int(os.pread(gyro_sensor_value_fd, 4096, 0))  # gyro
                gyro_sum += gyro_rate
                if gyro_rate > gyro_max:
                    gyro_max = gyro_rate
                if gyro_rate < gyro_min:
                    gyro_min = gyro_rate
                time.sleep(0.004)
            if gyro_max - gyro_min < 2:
                break
        gyro_offset = gyro_sum / count  # gOS
        print("gyro_offset:", gyro_offset)
        # End gOS My Block

        gyro_angle = -0.25
        # TODO: play speed up sound
        # TODO: play awake sound
        state = 1
        # Balance (BAL) loop
        print("loop")
        while True:
            # For performance reasons, we are not using functions for MyBlocks in the balancing loop
            #p1 = time.perf_counter()
            # Begin GT My Block
            if loop_count == 0:
                integral_time = 0.014  # tInt
                timer1 = time.perf_counter()
            else:
                integral_time = (time.perf_counter() - timer1) / loop_count
            loop_count += 1
            # End GT My Block

            #p2 = time.perf_counter()
            start_time = time.perf_counter() - timer1

            #p3 = time.perf_counter()
            # Begin GG My Block
            gyro_rate = int(os.pread(gyro_sensor_value_fd, 4096, 0))  # gyro
            gyro_offset = OFFSET_DAMPING_FACTOR * gyro_rate + (1 - OFFSET_DAMPING_FACTOR) * gyro_offset
            gyro_speed = gyro_rate - gyro_offset  # gSpd
            gyro_angle += gyro_speed * integral_time
            # End GG My Block

            #p4 = time.perf_counter()
            # Begin GM My Block
            prev_motor_sum = motor_sum
            left_motor_pos = int(os.pread(left_motor_position_fd, 4096, 0))
            right_motor_pos = int(os.pread(right_motor_position_fd, 4096, 0))
            motor_sum = right_motor_pos + left_motor_pos
            motor_difference = right_motor_pos - left_motor_pos  # mDiff
            motor_delta = motor_sum - prev_motor_sum
            motor_position += motor_delta
            avg_delta = (motor_delta_pos_3 + motor_delta_pos_2 + motor_delta_pos_1 + motor_delta) / 4
            motor_speed = avg_delta / integral_time  # mSpd
            motor_delta_pos_3 = motor_delta_pos_2
            motor_delta_pos_2 = motor_delta_pos_1
            motor_delta_pos_1 = motor_delta
            # End TM My Block

            #p5 = time.perf_counter()
            # Begin EQ My Block
            motor_position -= drive_control * integral_time
            power = -0.01 * drive_control
            power += 0.08 * motor_speed
            power += 0.12 * motor_position
            power += 0.8 * gyro_speed
            power += 15 * gyro_angle
            if power > 100:
                power = 100
            if power < -100:
                power = -100
            # End EQ My Block

            #p6 = time.perf_counter()
            # Begin cntrl My Block
            motor_position -= drive_control * integral_time
            steering = steering_control * 0.1
            left_power = int(power - steering)
            right_power = int(power + steering)
            # End cntrl My Block

            os.pwrite(left_motor_duty_cycle_sp_fd, str(left_power).encode(), 0)
            os.pwrite(right_motor_duty_cycle_sp_fd, str(right_power).encode(), 0)

            #p7 = time.perf_counter()
            # Begin CHK My Block
            if abs(power) < 100:
                timer2 = time.perf_counter()
            if time.perf_counter() - timer2 > 1:
                fell_over = True
            # End CHK My Block

            #p8 = time.perf_counter()
            while time.perf_counter() - timer1 < 0.005:
                pass
            #p9 = time.perf_counter()
            if fell_over:
                break
        print("oops")
        print("integral_time:", round(integral_time * 1000))
        #print("GT:", round((p2-p1)*1000))
        #print("time:", round((p3-p2)*1000))
        #print("GG:", round((p4-p3)*1000))
        #print("GM:", round((p5-p4)*1000))
        #print("EQ:", round((p6-p5)*1000))
        #print("cntrl:", round((p7-p6)*1000))
        #print("CHK:", round((p8-p7)*1000))
        #print("sleep:", round((p9-p8)*1000))
        os.pwrite(left_motor_stop_command_fd, b'hold', 0)
        os.pwrite(right_motor_stop_command_fd, b'hold', 0)
        os.pwrite(left_motor_command_fd, b'stop', 0)
        os.pwrite(right_motor_command_fd, b'stop', 0)
        state = 0
        # TODO: make LEDs red, flashing
        # TODO: show knocked out eyes
        # TODO: play power down sound
        while not int(os.pread(touch_sensor_value_fd, 4096, 0)):
            time.sleep(0.01)
        while int(os.pread(touch_sensor_value_fd, 4096, 0)):
            time.sleep(0.01)
        # TODO: reset LEDs


def stop():
    """This takes care of making sure motors have stopped when the program ends"""
    os.pwrite(left_motor_command_fd, b'reset', 0)
    os.pwrite(right_motor_command_fd, b'reset', 0)
    # os.pwrite(arms_motor_command_fd, b'reset', 0)
    # TODO: this can be removed when motor driver is fixed
    os.pwrite(left_motor_command_fd, b'stop', 0)
    os.pwrite(right_motor_command_fd, b'stop', 0)


if __name__ == '__main__':
    init()
    try:
        main()
    finally:
        stop()

Cool stuff, it works for a while, but sometimes, the robot goes forward like crazy (to catch the fall). I can you want to add a link to the build instructions: http://robotsquare.com/wp-content/uploads/2013/10/45544_gyroboy.pdf
Also would be nice to add a way to terminate the program (e.g. using the back or center button), so that I don't need to be connected via ssh.

Tried a few things on the process:

sudo renice -n +19 -p 633
sudo ionice -c 1 -n 7 -p 633

but still after a minute or so it falls. How long does it stay up for you?