ev3dev HTWay

htway.py

#!/usr/bin/env python

# Python port of the HiTechnic HTWay for ev3dev
# Copyright (c) 2014-2015 G33kDude, David Lechner
# HiTechnic HTWay is Copyright (c) 2010 HiTechnic


import itertools
import os
import struct
import time
import pyudev

# loop interval in seconds
WAIT_TIME = 0.008

# ratio of wheel size compared to NXT 1.0
WHEEL_RATIO_NXT1 = 1.0 # 56mm
WHEEL_RATIO_NXT = 0.8 # 43.2mm (same as EV3)
WHEEL_RATIO_RCX = 1.4 # 81.6mm

# These are the main four balance constants, only the gyro constants
# are relative to the wheel size. KPOS and KSPEED are self-relative to
# the wheel size.
KGYROANGLE = 7.5
KGYROSPEED = 1.15
KPOS = 0.07
KSPEED = 0.1

# This constant aids in drive control. When the robot starts moving
# because of user control, this constant helps get the robot leaning in
# the right direction. Similarly, it helps bring robot to a stop when
# stopping.
KDRIVE = -0.02

# Power differential used for steering based on difference of target
# steering and actual motor difference.
KSTEER = 0.25

# If robot power is saturated (over +/- 100) for over this time limit
# then robot must have fallen.  In seconds.
TIME_FALL_LIMIT = 0.5

# Gyro offset control
# The HiTechnic gyro sensor will drift with time.  This constant is
# used in a simple long term averaging to adjust for this drift.
# Every time through the loop, the current gyro sensor value is
# averaged into the gyro offset weighted according to this constant.
EMAOFFSET = 0.0005

class Gyro:
    @staticmethod
    def get_gyro():
        devices = list(pyudev.Context().list_devices(subsystem='lego-sensor') \
                .match_property("LEGO_DRIVER_NAME", EV3Gyro.DRIVER_NAME) \
                .match_property("LEGO_DRIVER_NAME", HTGyro.DRIVER_NAME))

        if not devices:
            raise Exception('Gyro not found')

        if devices[0].attributes['driver_name'] == EV3Gyro.DRIVER_NAME:
            return EV3Gyro(devices[0])
        elif devices[0].attributes['driver_name'] == HTGyro.DRIVER_NAME:
            return HTGyro(devices[0])

    def __init__(self, device):
        self.device = device
        self.value0 = open(os.path.join(self.device.sys_path, 'value0'), 'r', 0)
        self.offset = 0.0
        self.angle = 0.0

    def calibrate(self):
        self.angle = 0.0
        total = 0
        for i in range(10):
            total += self.get_rate()
            time.sleep(0.01)
        average = total / 10.0
        self.offset = average - 0.1

    def set_mode(self, value):
        with open(os.path.join(self.device.sys_path, 'mode'), 'w') as f:
            f.write(str(value))

    def get_rate(self):
        self.value0.seek(0)
        return int(self.value0.read())


class EV3Gyro(Gyro):
    DRIVER_NAME = 'lego-ev3-gyro'

    def __init__(self, device):
        Gyro.__init__(self, device)
        self.set_mode("GYRO-RATE")

    def get_data(self, interval):
        gyro_raw = self.get_rate()
        self.offset = EMAOFFSET * gyro_raw + (1 - EMAOFFSET) * self.offset
        speed = (gyro_raw - self.offset)
        self.angle += speed * interval
        return speed, self.angle

class HTGyro(Gyro):
    DRIVER_NAME = 'ht-nxt-gyro'

    def __init__(self, device):
        Gyro.__init__(self, device)

    def get_data(self, interval):
        gyro_raw = self.get_rate()
        self.offset = EMAOFFSET * gyro_raw + (1 - EMAOFFSET) * self.offset
        speed = (gyro_raw - self.offset) * 400.0 / 1953.0
        self.angle += speed * interval
        return speed, self.angle


class EV3Motor:
    def __init__(self, which=0):
        devices = list(pyudev.Context().list_devices(subsystem='tacho-motor') \
                .match_attribute('driver_name', 'lego-ev3-l-motor'))

        if which >= len(devices):
            raise Exception("Motor not found")

        self.device = devices[which]
        self.pos = open(os.path.join(self.device.sys_path, 'position'), 'r+',
                 0)
        self.duty_cycle_sp = open(os.path.join(self.device.sys_path,
                 'duty_cycle_sp'), 'w', 0)

        self.reset()

    def reset(self):
        self.set_pos(0)
        self.set_duty_cycle_sp(0)
        self.send_command("run-direct")
    def get_pos(self):
        self.pos.seek(0)
        return int(self.pos.read())

    def set_pos(self, new_pos):
        return self.pos.write(str(int(new_pos)))

    def set_duty_cycle_sp(self, new_duty_cycle_sp):
        return self.duty_cycle_sp.write(str(int(new_duty_cycle_sp)))

    def send_command(self, new_mode):
        with open(os.path.join(self.device.sys_path, 'command'),
                 'w') as command:
            command.write(str(new_mode))

class EV3Motors:
    def __init__(self, left=0, right=1):
        self.left = EV3Motor(left)
        self.right = EV3Motor(right)
        self.pos = 0.0
        self.speed = 0.0
        self.diff = 0
        self.target_diff = 0
        self.drive = 0
        self.steer = 0
        self.prev_sum = 0
        self.prev_deltas = [0,0,0]

    def get_data(self, interval):
        left_pos = self.left.get_pos()
        right_pos = self.right.get_pos()

        pos_sum = right_pos + left_pos
        self.diff = left_pos - right_pos

        delta = pos_sum - self.prev_sum
        self.pos += delta

        self.speed = (delta + sum(self.prev_deltas)) / (4 * interval)

        self.prev_sum = pos_sum
        self.prev_deltas = [delta] + self.prev_deltas[0:2]

        return self.speed, self.pos

    def steer_control(self, power, steering, interval):
        self.target_diff += steering * interval
        power_steer = KSTEER * (self.target_diff - self.diff)
        power_left = max(-100, min(power + power_steer, 100))
        power_right = max(-100, min(power - power_steer, 100))
        return power_left, power_right


def main():
    wheel_ratio = WHEEL_RATIO_RCX

    gyro = Gyro.get_gyro()
    gyro.calibrate()
    print "balancing in ..."
    for i in range(5, 0, -1):
        print i
        os.system("beep -f 440 -l 100")
        time.sleep(1)
    print 0
    os.system("beep -f 440 -l 1000")
    time.sleep(1)
    motors = EV3Motors()
    start_time = time.time()
    last_okay_time = start_time
    avg_interval = 0.0055
    for loop_count in itertools.count():
        gyro_speed, gyro_angle = gyro.get_data(avg_interval)
        motors_speed, motors_pos = motors.get_data(avg_interval)
        #print gyro_speed, gyro_angle, motors_speed, motors_pos
        motors_pos -= motors.drive * avg_interval
        motors.pos = motors_pos

        power = (KGYROSPEED * gyro_speed
                + KGYROANGLE * gyro_angle) \
                / wheel_ratio \
                + KPOS * motors_pos \
                + KDRIVE * motors.drive \
                + KSPEED * motors_speed

        left_power, right_power = motors.steer_control(power, 0, avg_interval)

        #print left_power, right_power

        motors.left.set_duty_cycle_sp(left_power)
        motors.right.set_duty_cycle_sp(right_power)

        time.sleep(WAIT_TIME)


        now_time = time.time()
        avg_interval = (now_time - start_time) / (loop_count+1)

        if abs(power) < 100:
            last_okay_time = now_time
        elif now_time - last_okay_time > TIME_FALL_LIMIT:
            break

    motors.left.send_command('reset')
    motors.right.send_command('reset')

if __name__ == "__main__":
    main()

Thanks you are a living robotics legend