240 lines
7.1 KiB
Python
240 lines
7.1 KiB
Python
#!/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.get('driver_name') == EV3Gyro.DRIVER_NAME:
|
|
return EV3Gyro(devices[0])
|
|
elif devices[0].attributes.get('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_NXT
|
|
|
|
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() |