250 lines
42 KiB
HTML
250 lines
42 KiB
HTML
<!DOCTYPE html>
|
|
<html lang="en">
|
|
<head>
|
|
<meta charset="UTF-8">
|
|
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
|
<title>Document</title>
|
|
</head>
|
|
<body>
|
|
<pre class="python" style="font-family:monospace;"><span style="color: #808080; font-style: italic;">#!/usr/bin/env python</span>
|
|
|
|
<span style="color: #808080; font-style: italic;"># Python port of the HiTechnic HTWay for ev3dev</span>
|
|
<span style="color: #808080; font-style: italic;"># Copyright (c) 2014-2015 G33kDude, David Lechner</span>
|
|
<span style="color: #808080; font-style: italic;"># HiTechnic HTWay is Copyright (c) 2010 HiTechnic</span>
|
|
<span style="color: #ff7700;font-weight:bold;">import</span> <span style="color: #dc143c;">itertools</span>
|
|
<span style="color: #ff7700;font-weight:bold;">import</span> <span style="color: #dc143c;">os</span>
|
|
<span style="color: #ff7700;font-weight:bold;">import</span> <span style="color: #dc143c;">struct</span>
|
|
<span style="color: #ff7700;font-weight:bold;">import</span> <span style="color: #dc143c;">time</span>
|
|
<span style="color: #ff7700;font-weight:bold;">import</span> pyudev
|
|
|
|
<span style="color: #808080; font-style: italic;"># loop interval in seconds</span>
|
|
WAIT_TIME <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0.008</span>
|
|
|
|
<span style="color: #808080; font-style: italic;"># ratio of wheel size compared to NXT 1.0</span>
|
|
WHEEL_RATIO_NXT1 <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">1.0</span> <span style="color: #808080; font-style: italic;"># 56mm</span>
|
|
WHEEL_RATIO_NXT <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0.8</span> <span style="color: #808080; font-style: italic;"># 43.2mm (same as EV3)</span>
|
|
WHEEL_RATIO_RCX <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">1.4</span> <span style="color: #808080; font-style: italic;"># 81.6mm</span>
|
|
|
|
<span style="color: #808080; font-style: italic;"># These are the main four balance constants, only the gyro constants</span>
|
|
<span style="color: #808080; font-style: italic;"># are relative to the wheel size. KPOS and KSPEED are self-relative to</span>
|
|
<span style="color: #808080; font-style: italic;"># the wheel size.</span>
|
|
KGYROANGLE <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">7.5</span>
|
|
KGYROSPEED <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">1.15</span>
|
|
KPOS <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0.07</span>
|
|
KSPEED <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0.1</span>
|
|
|
|
<span style="color: #808080; font-style: italic;"># This constant aids in drive control. When the robot starts moving</span>
|
|
<span style="color: #808080; font-style: italic;"># because of user control, this constant helps get the robot leaning in</span>
|
|
<span style="color: #808080; font-style: italic;"># the right direction. Similarly, it helps bring robot to a stop when</span>
|
|
<span style="color: #808080; font-style: italic;"># stopping.</span>
|
|
KDRIVE <span style="color: #66cc66;">=</span> -<span style="color: #ff4500;">0.02</span>
|
|
|
|
<span style="color: #808080; font-style: italic;"># Power differential used for steering based on difference of target</span>
|
|
<span style="color: #808080; font-style: italic;"># steering and actual motor difference.</span>
|
|
KSTEER <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0.25</span>
|
|
|
|
<span style="color: #808080; font-style: italic;"># If robot power is saturated (over +/- 100) for over this time limit</span>
|
|
<span style="color: #808080; font-style: italic;"># then robot must have fallen. In seconds.</span>
|
|
TIME_FALL_LIMIT <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0.5</span>
|
|
|
|
<span style="color: #808080; font-style: italic;"># Gyro offset control</span>
|
|
<span style="color: #808080; font-style: italic;"># The HiTechnic gyro sensor will drift with time. This constant is</span>
|
|
<span style="color: #808080; font-style: italic;"># used in a simple long term averaging to adjust for this drift.</span>
|
|
<span style="color: #808080; font-style: italic;"># Every time through the loop, the current gyro sensor value is</span>
|
|
<span style="color: #808080; font-style: italic;"># averaged into the gyro offset weighted according to this constant.</span>
|
|
EMAOFFSET <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0.0005</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">class</span> Gyro:
|
|
<span style="color: #66cc66;">@</span><span style="color: #008000;">staticmethod</span>
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> get_gyro<span style="color: black;">(</span><span style="color: black;">)</span>:
|
|
devices <span style="color: #66cc66;">=</span> <span style="color: #008000;">list</span><span style="color: black;">(</span>pyudev.<span style="color: black;">Context</span><span style="color: black;">(</span><span style="color: black;">)</span>.<span style="color: black;">list_devices</span><span style="color: black;">(</span>subsystem<span style="color: #66cc66;">=</span><span style="color: #483d8b;">'lego-sensor'</span><span style="color: black;">)</span> \
|
|
.<span style="color: black;">match_property</span><span style="color: black;">(</span><span style="color: #483d8b;">"LEGO_DRIVER_NAME"</span><span style="color: #66cc66;">,</span> EV3Gyro.<span style="color: black;">DRIVER_NAME</span><span style="color: black;">)</span> \
|
|
.<span style="color: black;">match_property</span><span style="color: black;">(</span><span style="color: #483d8b;">"LEGO_DRIVER_NAME"</span><span style="color: #66cc66;">,</span> HTGyro.<span style="color: black;">DRIVER_NAME</span><span style="color: black;">)</span><span style="color: black;">)</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">if</span> <span style="color: #ff7700;font-weight:bold;">not</span> devices:
|
|
<span style="color: #ff7700;font-weight:bold;">raise</span> <span style="color: #008000;">Exception</span><span style="color: black;">(</span><span style="color: #483d8b;">'Gyro not found'</span><span style="color: black;">)</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">if</span> devices<span style="color: black;">[</span><span style="color: #ff4500;">0</span><span style="color: black;">]</span>.<span style="color: black;">attributes</span>.<span style="color: black;">get</span><span style="color: black;">(</span><span style="color: #483d8b;">'driver_name'</span><span style="color: black;">)</span> <span style="color: #66cc66;">==</span> EV3Gyro.<span style="color: black;">DRIVER_NAME</span>:
|
|
<span style="color: #ff7700;font-weight:bold;">return</span> EV3Gyro<span style="color: black;">(</span>devices<span style="color: black;">[</span><span style="color: #ff4500;">0</span><span style="color: black;">]</span><span style="color: black;">)</span>
|
|
<span style="color: #ff7700;font-weight:bold;">elif</span> devices<span style="color: black;">[</span><span style="color: #ff4500;">0</span><span style="color: black;">]</span>.<span style="color: black;">attributes</span>.<span style="color: black;">get</span><span style="color: black;">(</span><span style="color: #483d8b;">'driver_name'</span><span style="color: black;">)</span> <span style="color: #66cc66;">==</span> HTGyro.<span style="color: black;">DRIVER_NAME</span>:
|
|
<span style="color: #ff7700;font-weight:bold;">return</span> HTGyro<span style="color: black;">(</span>devices<span style="color: black;">[</span><span style="color: #ff4500;">0</span><span style="color: black;">]</span><span style="color: black;">)</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> <span style="color: #0000cd;">__init__</span><span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> device<span style="color: black;">)</span>:
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">device</span> <span style="color: #66cc66;">=</span> device
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">value0</span> <span style="color: #66cc66;">=</span> <span style="color: #008000;">open</span><span style="color: black;">(</span><span style="color: #dc143c;">os</span>.<span style="color: black;">path</span>.<span style="color: black;">join</span><span style="color: black;">(</span><span style="color: #008000;">self</span>.<span style="color: black;">device</span>.<span style="color: black;">sys_path</span><span style="color: #66cc66;">,</span> <span style="color: #483d8b;">'value0'</span><span style="color: black;">)</span><span style="color: #66cc66;">,</span> <span style="color: #483d8b;">'r'</span><span style="color: #66cc66;">,</span> <span style="color: #ff4500;">0</span><span style="color: black;">)</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">offset</span> <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0.0</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">angle</span> <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0.0</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> calibrate<span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: black;">)</span>:
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">angle</span> <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0.0</span>
|
|
total <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0</span>
|
|
<span style="color: #ff7700;font-weight:bold;">for</span> i <span style="color: #ff7700;font-weight:bold;">in</span> <span style="color: #008000;">range</span><span style="color: black;">(</span><span style="color: #ff4500;">10</span><span style="color: black;">)</span>:
|
|
total +<span style="color: #66cc66;">=</span> <span style="color: #008000;">self</span>.<span style="color: black;">get_rate</span><span style="color: black;">(</span><span style="color: black;">)</span>
|
|
<span style="color: #dc143c;">time</span>.<span style="color: black;">sleep</span><span style="color: black;">(</span><span style="color: #ff4500;">0.01</span><span style="color: black;">)</span>
|
|
average <span style="color: #66cc66;">=</span> total / <span style="color: #ff4500;">10.0</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">offset</span> <span style="color: #66cc66;">=</span> average - <span style="color: #ff4500;">0.1</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> set_mode<span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> value<span style="color: black;">)</span>:
|
|
<span style="color: #ff7700;font-weight:bold;">with</span> <span style="color: #008000;">open</span><span style="color: black;">(</span><span style="color: #dc143c;">os</span>.<span style="color: black;">path</span>.<span style="color: black;">join</span><span style="color: black;">(</span><span style="color: #008000;">self</span>.<span style="color: black;">device</span>.<span style="color: black;">sys_path</span><span style="color: #66cc66;">,</span> <span style="color: #483d8b;">'mode'</span><span style="color: black;">)</span><span style="color: #66cc66;">,</span> <span style="color: #483d8b;">'w'</span><span style="color: black;">)</span> <span style="color: #ff7700;font-weight:bold;">as</span> f:
|
|
f.<span style="color: black;">write</span><span style="color: black;">(</span><span style="color: #008000;">str</span><span style="color: black;">(</span>value<span style="color: black;">)</span><span style="color: black;">)</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> get_rate<span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: black;">)</span>:
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">value0</span>.<span style="color: black;">seek</span><span style="color: black;">(</span><span style="color: #ff4500;">0</span><span style="color: black;">)</span>
|
|
<span style="color: #ff7700;font-weight:bold;">return</span> <span style="color: #008000;">int</span><span style="color: black;">(</span><span style="color: #008000;">self</span>.<span style="color: black;">value0</span>.<span style="color: black;">read</span><span style="color: black;">(</span><span style="color: black;">)</span><span style="color: black;">)</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">class</span> EV3Gyro<span style="color: black;">(</span>Gyro<span style="color: black;">)</span>:
|
|
DRIVER_NAME <span style="color: #66cc66;">=</span> <span style="color: #483d8b;">'lego-ev3-gyro'</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> <span style="color: #0000cd;">__init__</span><span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> device<span style="color: black;">)</span>:
|
|
Gyro.<span style="color: #0000cd;">__init__</span><span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> device<span style="color: black;">)</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">set_mode</span><span style="color: black;">(</span><span style="color: #483d8b;">"GYRO-RATE"</span><span style="color: black;">)</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> get_data<span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> interval<span style="color: black;">)</span>:
|
|
gyro_raw <span style="color: #66cc66;">=</span> <span style="color: #008000;">self</span>.<span style="color: black;">get_rate</span><span style="color: black;">(</span><span style="color: black;">)</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">offset</span> <span style="color: #66cc66;">=</span> EMAOFFSET * gyro_raw + <span style="color: black;">(</span><span style="color: #ff4500;">1</span> - EMAOFFSET<span style="color: black;">)</span> * <span style="color: #008000;">self</span>.<span style="color: black;">offset</span>
|
|
speed <span style="color: #66cc66;">=</span> <span style="color: black;">(</span>gyro_raw - <span style="color: #008000;">self</span>.<span style="color: black;">offset</span><span style="color: black;">)</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">angle</span> +<span style="color: #66cc66;">=</span> speed * interval
|
|
<span style="color: #ff7700;font-weight:bold;">return</span> speed<span style="color: #66cc66;">,</span> <span style="color: #008000;">self</span>.<span style="color: black;">angle</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">class</span> HTGyro<span style="color: black;">(</span>Gyro<span style="color: black;">)</span>:
|
|
DRIVER_NAME <span style="color: #66cc66;">=</span> <span style="color: #483d8b;">'ht-nxt-gyro'</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> <span style="color: #0000cd;">__init__</span><span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> device<span style="color: black;">)</span>:
|
|
Gyro.<span style="color: #0000cd;">__init__</span><span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> device<span style="color: black;">)</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> get_data<span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> interval<span style="color: black;">)</span>:
|
|
gyro_raw <span style="color: #66cc66;">=</span> <span style="color: #008000;">self</span>.<span style="color: black;">get_rate</span><span style="color: black;">(</span><span style="color: black;">)</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">offset</span> <span style="color: #66cc66;">=</span> EMAOFFSET * gyro_raw + <span style="color: black;">(</span><span style="color: #ff4500;">1</span> - EMAOFFSET<span style="color: black;">)</span> * <span style="color: #008000;">self</span>.<span style="color: black;">offset</span>
|
|
speed <span style="color: #66cc66;">=</span> <span style="color: black;">(</span>gyro_raw - <span style="color: #008000;">self</span>.<span style="color: black;">offset</span><span style="color: black;">)</span> * <span style="color: #ff4500;">400.0</span> / <span style="color: #ff4500;">1953.0</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">angle</span> +<span style="color: #66cc66;">=</span> speed * interval
|
|
<span style="color: #ff7700;font-weight:bold;">return</span> speed<span style="color: #66cc66;">,</span> <span style="color: #008000;">self</span>.<span style="color: black;">angle</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">class</span> EV3Motor:
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> <span style="color: #0000cd;">__init__</span><span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> which<span style="color: #66cc66;">=</span><span style="color: #ff4500;">0</span><span style="color: black;">)</span>:
|
|
devices <span style="color: #66cc66;">=</span> <span style="color: #008000;">list</span><span style="color: black;">(</span>pyudev.<span style="color: black;">Context</span><span style="color: black;">(</span><span style="color: black;">)</span>.<span style="color: black;">list_devices</span><span style="color: black;">(</span>subsystem<span style="color: #66cc66;">=</span><span style="color: #483d8b;">'tacho-motor'</span><span style="color: black;">)</span>.<span style="color: black;">match_attribute</span><span style="color: black;">(</span><span style="color: #483d8b;">'driver_name'</span><span style="color: #66cc66;">,</span> <span style="color: #483d8b;">'lego-ev3-l-motor'</span><span style="color: black;">)</span><span style="color: black;">)</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">if</span> which <span style="color: #66cc66;">>=</span> <span style="color: #008000;">len</span><span style="color: black;">(</span>devices<span style="color: black;">)</span>:
|
|
<span style="color: #ff7700;font-weight:bold;">raise</span> <span style="color: #008000;">Exception</span><span style="color: black;">(</span><span style="color: #483d8b;">"Motor not found"</span><span style="color: black;">)</span>
|
|
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">device</span> <span style="color: #66cc66;">=</span> devices<span style="color: black;">[</span>which<span style="color: black;">]</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">pos</span> <span style="color: #66cc66;">=</span> <span style="color: #008000;">open</span><span style="color: black;">(</span><span style="color: #dc143c;">os</span>.<span style="color: black;">path</span>.<span style="color: black;">join</span><span style="color: black;">(</span><span style="color: #008000;">self</span>.<span style="color: black;">device</span>.<span style="color: black;">sys_path</span><span style="color: #66cc66;">,</span> <span style="color: #483d8b;">'position'</span><span style="color: black;">)</span><span style="color: #66cc66;">,</span> <span style="color: #483d8b;">'r+'</span><span style="color: #66cc66;">,</span><span style="color: #ff4500;">0</span><span style="color: black;">)</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">duty_cycle_sp</span> <span style="color: #66cc66;">=</span> <span style="color: #008000;">open</span><span style="color: black;">(</span><span style="color: #dc143c;">os</span>.<span style="color: black;">path</span>.<span style="color: black;">join</span><span style="color: black;">(</span><span style="color: #008000;">self</span>.<span style="color: black;">device</span>.<span style="color: black;">sys_path</span><span style="color: #66cc66;">,</span><span style="color: #483d8b;">'duty_cycle_sp'</span><span style="color: black;">)</span><span style="color: #66cc66;">,</span> <span style="color: #483d8b;">'w'</span><span style="color: #66cc66;">,</span> <span style="color: #ff4500;">0</span><span style="color: black;">)</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">reset</span><span style="color: black;">(</span><span style="color: black;">)</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> reset<span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: black;">)</span>:
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">set_pos</span><span style="color: black;">(</span><span style="color: #ff4500;">0</span><span style="color: black;">)</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">set_duty_cycle_sp</span><span style="color: black;">(</span><span style="color: #ff4500;">0</span><span style="color: black;">)</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">send_command</span><span style="color: black;">(</span><span style="color: #483d8b;">"run-direct"</span><span style="color: black;">)</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> get_pos<span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: black;">)</span>:
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">pos</span>.<span style="color: black;">seek</span><span style="color: black;">(</span><span style="color: #ff4500;">0</span><span style="color: black;">)</span>
|
|
<span style="color: #ff7700;font-weight:bold;">return</span> <span style="color: #008000;">int</span><span style="color: black;">(</span><span style="color: #008000;">self</span>.<span style="color: black;">pos</span>.<span style="color: black;">read</span><span style="color: black;">(</span><span style="color: black;">)</span><span style="color: black;">)</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> set_pos<span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> new_pos<span style="color: black;">)</span>:
|
|
<span style="color: #ff7700;font-weight:bold;">return</span> <span style="color: #008000;">self</span>.<span style="color: black;">pos</span>.<span style="color: black;">write</span><span style="color: black;">(</span><span style="color: #008000;">str</span><span style="color: black;">(</span><span style="color: #008000;">int</span><span style="color: black;">(</span>new_pos<span style="color: black;">)</span><span style="color: black;">)</span><span style="color: black;">)</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> set_duty_cycle_sp<span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> new_duty_cycle_sp<span style="color: black;">)</span>:
|
|
<span style="color: #ff7700;font-weight:bold;">return</span> <span style="color: #008000;">self</span>.<span style="color: black;">duty_cycle_sp</span>.<span style="color: black;">write</span><span style="color: black;">(</span><span style="color: #008000;">str</span><span style="color: black;">(</span><span style="color: #008000;">int</span><span style="color: black;">(</span>new_duty_cycle_sp<span style="color: black;">)</span><span style="color: black;">)</span><span style="color: black;">)</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> send_command<span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> new_mode<span style="color: black;">)</span>:
|
|
<span style="color: #ff7700;font-weight:bold;">with</span> <span style="color: #008000;">open</span><span style="color: black;">(</span><span style="color: #dc143c;">os</span>.<span style="color: black;">path</span>.<span style="color: black;">join</span><span style="color: black;">(</span><span style="color: #008000;">self</span>.<span style="color: black;">device</span>.<span style="color: black;">sys_path</span><span style="color: #66cc66;">,</span> <span style="color: #483d8b;">'command'</span><span style="color: black;">)</span><span style="color: #66cc66;">,</span><span style="color: #483d8b;">'w'</span><span style="color: black;">)</span> <span style="color: #ff7700;font-weight:bold;">as</span> command:
|
|
command.<span style="color: black;">write</span><span style="color: black;">(</span><span style="color: #008000;">str</span><span style="color: black;">(</span>new_mode<span style="color: black;">)</span><span style="color: black;">)</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">class</span> EV3Motors:
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> <span style="color: #0000cd;">__init__</span><span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> left<span style="color: #66cc66;">=</span><span style="color: #ff4500;">0</span><span style="color: #66cc66;">,</span> right<span style="color: #66cc66;">=</span><span style="color: #ff4500;">1</span><span style="color: black;">)</span>:
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">left</span> <span style="color: #66cc66;">=</span> EV3Motor<span style="color: black;">(</span>left<span style="color: black;">)</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">right</span> <span style="color: #66cc66;">=</span> EV3Motor<span style="color: black;">(</span>right<span style="color: black;">)</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">pos</span> <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0.0</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">speed</span> <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0.0</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">diff</span> <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">target_diff</span> <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">drive</span> <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">steer</span> <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">prev_sum</span> <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">prev_deltas</span> <span style="color: #66cc66;">=</span> <span style="color: black;">[</span><span style="color: #ff4500;">0</span><span style="color: #66cc66;">,</span><span style="color: #ff4500;">0</span><span style="color: #66cc66;">,</span><span style="color: #ff4500;">0</span><span style="color: black;">]</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> get_data<span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> interval<span style="color: black;">)</span>:
|
|
left_pos <span style="color: #66cc66;">=</span> <span style="color: #008000;">self</span>.<span style="color: black;">left</span>.<span style="color: black;">get_pos</span><span style="color: black;">(</span><span style="color: black;">)</span>
|
|
right_pos <span style="color: #66cc66;">=</span> <span style="color: #008000;">self</span>.<span style="color: black;">right</span>.<span style="color: black;">get_pos</span><span style="color: black;">(</span><span style="color: black;">)</span>
|
|
|
|
pos_sum <span style="color: #66cc66;">=</span> right_pos + left_pos
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">diff</span> <span style="color: #66cc66;">=</span> left_pos - right_pos
|
|
|
|
delta <span style="color: #66cc66;">=</span> pos_sum - <span style="color: #008000;">self</span>.<span style="color: black;">prev_sum</span>
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">pos</span> +<span style="color: #66cc66;">=</span> delta
|
|
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">speed</span> <span style="color: #66cc66;">=</span> <span style="color: black;">(</span>delta + <span style="color: #008000;">sum</span><span style="color: black;">(</span><span style="color: #008000;">self</span>.<span style="color: black;">prev_deltas</span><span style="color: black;">)</span><span style="color: black;">)</span> / <span style="color: black;">(</span><span style="color: #ff4500;">4</span> * interval<span style="color: black;">)</span>
|
|
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">prev_sum</span> <span style="color: #66cc66;">=</span> pos_sum
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">prev_deltas</span> <span style="color: #66cc66;">=</span> <span style="color: black;">[</span>delta<span style="color: black;">]</span> + <span style="color: #008000;">self</span>.<span style="color: black;">prev_deltas</span><span style="color: black;">[</span><span style="color: #ff4500;">0</span>:<span style="color: #ff4500;">2</span><span style="color: black;">]</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">return</span> <span style="color: #008000;">self</span>.<span style="color: black;">speed</span><span style="color: #66cc66;">,</span> <span style="color: #008000;">self</span>.<span style="color: black;">pos</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> steer_control<span style="color: black;">(</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> power<span style="color: #66cc66;">,</span> steering<span style="color: #66cc66;">,</span> interval<span style="color: black;">)</span>:
|
|
<span style="color: #008000;">self</span>.<span style="color: black;">target_diff</span> +<span style="color: #66cc66;">=</span> steering * interval
|
|
power_steer <span style="color: #66cc66;">=</span> KSTEER * <span style="color: black;">(</span><span style="color: #008000;">self</span>.<span style="color: black;">target_diff</span> - <span style="color: #008000;">self</span>.<span style="color: black;">diff</span><span style="color: black;">)</span>
|
|
power_left <span style="color: #66cc66;">=</span> <span style="color: #008000;">max</span><span style="color: black;">(</span>-<span style="color: #ff4500;">100</span><span style="color: #66cc66;">,</span> <span style="color: #008000;">min</span><span style="color: black;">(</span>power + power_steer<span style="color: #66cc66;">,</span> <span style="color: #ff4500;">100</span><span style="color: black;">)</span><span style="color: black;">)</span>
|
|
power_right <span style="color: #66cc66;">=</span> <span style="color: #008000;">max</span><span style="color: black;">(</span>-<span style="color: #ff4500;">100</span><span style="color: #66cc66;">,</span> <span style="color: #008000;">min</span><span style="color: black;">(</span>power - power_steer<span style="color: #66cc66;">,</span> <span style="color: #ff4500;">100</span><span style="color: black;">)</span><span style="color: black;">)</span>
|
|
<span style="color: #ff7700;font-weight:bold;">return</span> power_left<span style="color: #66cc66;">,</span> power_right
|
|
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">def</span> main<span style="color: black;">(</span><span style="color: black;">)</span>:
|
|
wheel_ratio <span style="color: #66cc66;">=</span> WHEEL_RATIO_NXT
|
|
|
|
gyro <span style="color: #66cc66;">=</span> Gyro.<span style="color: black;">get_gyro</span><span style="color: black;">(</span><span style="color: black;">)</span>
|
|
gyro.<span style="color: black;">calibrate</span><span style="color: black;">(</span><span style="color: black;">)</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">print</span><span style="color: black;">(</span><span style="color: #483d8b;">"balancing in ..."</span><span style="color: black;">)</span>
|
|
<span style="color: #ff7700;font-weight:bold;">for</span> i <span style="color: #ff7700;font-weight:bold;">in</span> <span style="color: #008000;">range</span><span style="color: black;">(</span><span style="color: #ff4500;">5</span><span style="color: #66cc66;">,</span> <span style="color: #ff4500;">0</span><span style="color: #66cc66;">,</span> -<span style="color: #ff4500;">1</span><span style="color: black;">)</span>:
|
|
<span style="color: #ff7700;font-weight:bold;">print</span><span style="color: black;">(</span>i<span style="color: black;">)</span>
|
|
<span style="color: #dc143c;">os</span>.<span style="color: black;">system</span><span style="color: black;">(</span><span style="color: #483d8b;">"beep -f 440 -l 100"</span><span style="color: black;">)</span>
|
|
<span style="color: #dc143c;">time</span>.<span style="color: black;">sleep</span><span style="color: black;">(</span><span style="color: #ff4500;">1</span><span style="color: black;">)</span>
|
|
<span style="color: #ff7700;font-weight:bold;">print</span><span style="color: black;">(</span><span style="color: #ff4500;">0</span><span style="color: black;">)</span>
|
|
|
|
<span style="color: #dc143c;">os</span>.<span style="color: black;">system</span><span style="color: black;">(</span><span style="color: #483d8b;">"beep -f 440 -l 1000"</span><span style="color: black;">)</span>
|
|
<span style="color: #dc143c;">time</span>.<span style="color: black;">sleep</span><span style="color: black;">(</span><span style="color: #ff4500;">1</span><span style="color: black;">)</span>
|
|
|
|
motors <span style="color: #66cc66;">=</span> EV3Motors<span style="color: black;">(</span><span style="color: black;">)</span>
|
|
start_time <span style="color: #66cc66;">=</span> <span style="color: #dc143c;">time</span>.<span style="color: #dc143c;">time</span><span style="color: black;">(</span><span style="color: black;">)</span>
|
|
last_okay_time <span style="color: #66cc66;">=</span> start_time
|
|
|
|
avg_interval <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0.0055</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">for</span> loop_count <span style="color: #ff7700;font-weight:bold;">in</span> <span style="color: #dc143c;">itertools</span>.<span style="color: black;">count</span><span style="color: black;">(</span><span style="color: black;">)</span>:
|
|
gyro_speed<span style="color: #66cc66;">,</span> gyro_angle <span style="color: #66cc66;">=</span> gyro.<span style="color: black;">get_data</span><span style="color: black;">(</span>avg_interval<span style="color: black;">)</span>
|
|
motors_speed<span style="color: #66cc66;">,</span> motors_pos <span style="color: #66cc66;">=</span> motors.<span style="color: black;">get_data</span><span style="color: black;">(</span>avg_interval<span style="color: black;">)</span>
|
|
<span style="color: #ff7700;font-weight:bold;">print</span> gyro_speed<span style="color: #66cc66;">,</span> gyro_angle<span style="color: #66cc66;">,</span> motors_speed<span style="color: #66cc66;">,</span> motors_pos
|
|
motors_pos -<span style="color: #66cc66;">=</span> motors.<span style="color: black;">drive</span> * avg_interval
|
|
motors.<span style="color: black;">pos</span> <span style="color: #66cc66;">=</span> motors_pos
|
|
|
|
power <span style="color: #66cc66;">=</span> <span style="color: black;">(</span>KGYROSPEED * gyro_speed
|
|
+ KGYROANGLE * gyro_angle<span style="color: black;">)</span> \
|
|
/ wheel_ratio \
|
|
+ KPOS * motors_pos \
|
|
+ KDRIVE * motors.<span style="color: black;">drive</span> \
|
|
+ KSPEED * motors_speed
|
|
|
|
left_power<span style="color: #66cc66;">,</span> right_power <span style="color: #66cc66;">=</span> motors.<span style="color: black;">steer_control</span><span style="color: black;">(</span>power<span style="color: #66cc66;">,</span> <span style="color: #ff4500;">0</span><span style="color: #66cc66;">,</span> avg_interval<span style="color: black;">)</span>
|
|
|
|
<span style="color: #808080; font-style: italic;">#print left_power, right_power</span>
|
|
|
|
motors.<span style="color: black;">left</span>.<span style="color: black;">set_duty_cycle_sp</span><span style="color: black;">(</span>left_power<span style="color: black;">)</span>
|
|
motors.<span style="color: black;">right</span>.<span style="color: black;">set_duty_cycle_sp</span><span style="color: black;">(</span>right_power<span style="color: black;">)</span>
|
|
|
|
<span style="color: #dc143c;">time</span>.<span style="color: black;">sleep</span><span style="color: black;">(</span>WAIT_TIME<span style="color: black;">)</span>
|
|
|
|
now_time <span style="color: #66cc66;">=</span> <span style="color: #dc143c;">time</span>.<span style="color: #dc143c;">time</span><span style="color: black;">(</span><span style="color: black;">)</span>
|
|
avg_interval <span style="color: #66cc66;">=</span> <span style="color: black;">(</span>now_time - start_time<span style="color: black;">)</span> / <span style="color: black;">(</span>loop_count+<span style="color: #ff4500;">1</span><span style="color: black;">)</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">if</span> <span style="color: #008000;">abs</span><span style="color: black;">(</span>power<span style="color: black;">)</span> <span style="color: #66cc66;"><</span> <span style="color: #ff4500;">100</span>:
|
|
last_okay_time <span style="color: #66cc66;">=</span> now_time
|
|
<span style="color: #ff7700;font-weight:bold;">elif</span> now_time - last_okay_time <span style="color: #66cc66;">></span> TIME_FALL_LIMIT:
|
|
<span style="color: #ff7700;font-weight:bold;">break</span>
|
|
|
|
motors.<span style="color: black;">left</span>.<span style="color: black;">send_command</span><span style="color: black;">(</span><span style="color: #483d8b;">'reset'</span><span style="color: black;">)</span>
|
|
motors.<span style="color: black;">right</span>.<span style="color: black;">send_command</span><span style="color: black;">(</span><span style="color: #483d8b;">'reset'</span><span style="color: black;">)</span>
|
|
|
|
<span style="color: #ff7700;font-weight:bold;">if</span> __name__ <span style="color: #66cc66;">==</span> <span style="color: #483d8b;">"__main__"</span>:
|
|
main<span style="color: black;">(</span><span style="color: black;">)</span></pre>
|
|
</body>
|
|
</html> |