Compare commits
2 Commits
| Author | SHA1 | Date |
|---|---|---|
 Arne van Iterson
|
0295758be6 | |
|
Arne van Iterson
|
fa0427b564 |
|
|
@ -135,12 +135,309 @@ cython_debug/
|
|||
# Ignore exercises
|
||||
exercises/
|
||||
|
||||
# LaTeX Garbage
|
||||
*.aux
|
||||
*.fdb_latexmk
|
||||
*.fls
|
||||
*.log
|
||||
*.synctex.gz
|
||||
# Logs
|
||||
logs/
|
||||
log/
|
||||
*.xlsx
|
||||
|
||||
# LaTeX output
|
||||
*.pdf
|
||||
# Drawio backups
|
||||
*.drawio.bkp
|
||||
*.drawio.dtmp
|
||||
|
||||
# LaTeX Garbage
|
||||
## Core latex/pdflatex auxiliary files:
|
||||
*.aux
|
||||
*.lof
|
||||
*.log
|
||||
*.lot
|
||||
*.fls
|
||||
*.out
|
||||
*.toc
|
||||
*.fmt
|
||||
*.fot
|
||||
*.cb
|
||||
*.cb2
|
||||
.*.lb
|
||||
|
||||
## Intermediate documents:
|
||||
*.dvi
|
||||
*.xdv
|
||||
*-converted-to.*
|
||||
# these rules might exclude image files for figures etc.
|
||||
# *.ps
|
||||
# *.eps
|
||||
# *.pdf
|
||||
|
||||
## Generated if empty string is given at "Please type another file name for output:"
|
||||
*.pdf
|
||||
|
||||
## Bibliography auxiliary files (bibtex/biblatex/biber):
|
||||
*.bbl
|
||||
*.bcf
|
||||
*.blg
|
||||
*-blx.aux
|
||||
*-blx.bib
|
||||
*.run.xml
|
||||
|
||||
## Build tool auxiliary files:
|
||||
*.fdb_latexmk
|
||||
*.synctex
|
||||
*.synctex(busy)
|
||||
*.synctex.gz
|
||||
*.synctex.gz(busy)
|
||||
*.pdfsync
|
||||
|
||||
## Build tool directories for auxiliary files
|
||||
# latexrun
|
||||
latex.out/
|
||||
|
||||
## Auxiliary and intermediate files from other packages:
|
||||
# algorithms
|
||||
*.alg
|
||||
*.loa
|
||||
|
||||
# achemso
|
||||
acs-*.bib
|
||||
|
||||
# amsthm
|
||||
*.thm
|
||||
|
||||
# beamer
|
||||
*.nav
|
||||
*.pre
|
||||
*.snm
|
||||
*.vrb
|
||||
|
||||
# changes
|
||||
*.soc
|
||||
|
||||
# comment
|
||||
*.cut
|
||||
|
||||
# cprotect
|
||||
*.cpt
|
||||
|
||||
# elsarticle (documentclass of Elsevier journals)
|
||||
*.spl
|
||||
|
||||
# endnotes
|
||||
*.ent
|
||||
|
||||
# fixme
|
||||
*.lox
|
||||
|
||||
# feynmf/feynmp
|
||||
*.mf
|
||||
*.mp
|
||||
*.t[1-9]
|
||||
*.t[1-9][0-9]
|
||||
*.tfm
|
||||
|
||||
#(r)(e)ledmac/(r)(e)ledpar
|
||||
*.end
|
||||
*.?end
|
||||
*.[1-9]
|
||||
*.[1-9][0-9]
|
||||
*.[1-9][0-9][0-9]
|
||||
*.[1-9]R
|
||||
*.[1-9][0-9]R
|
||||
*.[1-9][0-9][0-9]R
|
||||
*.eledsec[1-9]
|
||||
*.eledsec[1-9]R
|
||||
*.eledsec[1-9][0-9]
|
||||
*.eledsec[1-9][0-9]R
|
||||
*.eledsec[1-9][0-9][0-9]
|
||||
*.eledsec[1-9][0-9][0-9]R
|
||||
|
||||
# glossaries
|
||||
*.acn
|
||||
*.acr
|
||||
*.glg
|
||||
*.glo
|
||||
*.gls
|
||||
*.glsdefs
|
||||
*.lzo
|
||||
*.lzs
|
||||
*.slg
|
||||
*.slo
|
||||
*.sls
|
||||
|
||||
# uncomment this for glossaries-extra (will ignore makeindex's style files!)
|
||||
# *.ist
|
||||
|
||||
# gnuplot
|
||||
*.gnuplot
|
||||
*.table
|
||||
|
||||
# gnuplottex
|
||||
*-gnuplottex-*
|
||||
|
||||
# gregoriotex
|
||||
*.gaux
|
||||
*.glog
|
||||
*.gtex
|
||||
|
||||
# htlatex
|
||||
*.4ct
|
||||
*.4tc
|
||||
*.idv
|
||||
*.lg
|
||||
*.trc
|
||||
*.xref
|
||||
|
||||
# hyperref
|
||||
*.brf
|
||||
|
||||
# knitr
|
||||
*-concordance.tex
|
||||
# TODO Uncomment the next line if you use knitr and want to ignore its generated tikz files
|
||||
# *.tikz
|
||||
*-tikzDictionary
|
||||
|
||||
# listings
|
||||
*.lol
|
||||
|
||||
# luatexja-ruby
|
||||
*.ltjruby
|
||||
|
||||
# makeidx
|
||||
*.idx
|
||||
*.ilg
|
||||
*.ind
|
||||
|
||||
# minitoc
|
||||
*.maf
|
||||
*.mlf
|
||||
*.mlt
|
||||
*.mtc[0-9]*
|
||||
*.slf[0-9]*
|
||||
*.slt[0-9]*
|
||||
*.stc[0-9]*
|
||||
|
||||
# minted
|
||||
_minted*
|
||||
*.pyg
|
||||
|
||||
# morewrites
|
||||
*.mw
|
||||
|
||||
# newpax
|
||||
*.newpax
|
||||
|
||||
# nomencl
|
||||
*.nlg
|
||||
*.nlo
|
||||
*.nls
|
||||
|
||||
# pax
|
||||
*.pax
|
||||
|
||||
# pdfpcnotes
|
||||
*.pdfpc
|
||||
|
||||
# sagetex
|
||||
*.sagetex.sage
|
||||
*.sagetex.py
|
||||
*.sagetex.scmd
|
||||
|
||||
# scrwfile
|
||||
*.wrt
|
||||
|
||||
# svg
|
||||
svg-inkscape/
|
||||
|
||||
# sympy
|
||||
*.sout
|
||||
*.sympy
|
||||
sympy-plots-for-*.tex/
|
||||
|
||||
# pdfcomment
|
||||
*.upa
|
||||
*.upb
|
||||
|
||||
# pythontex
|
||||
*.pytxcode
|
||||
pythontex-files-*/
|
||||
|
||||
# tcolorbox
|
||||
*.listing
|
||||
|
||||
# thmtools
|
||||
*.loe
|
||||
|
||||
# TikZ & PGF
|
||||
*.dpth
|
||||
*.md5
|
||||
*.auxlock
|
||||
|
||||
# titletoc
|
||||
*.ptc
|
||||
|
||||
# todonotes
|
||||
*.tdo
|
||||
|
||||
# vhistory
|
||||
*.hst
|
||||
*.ver
|
||||
|
||||
# easy-todo
|
||||
*.lod
|
||||
|
||||
# xcolor
|
||||
*.xcp
|
||||
|
||||
# xmpincl
|
||||
*.xmpi
|
||||
|
||||
# xindy
|
||||
*.xdy
|
||||
|
||||
# xypic precompiled matrices and outlines
|
||||
*.xyc
|
||||
*.xyd
|
||||
|
||||
# endfloat
|
||||
*.ttt
|
||||
*.fff
|
||||
|
||||
# Latexian
|
||||
TSWLatexianTemp*
|
||||
|
||||
## Editors:
|
||||
# WinEdt
|
||||
*.bak
|
||||
*.sav
|
||||
|
||||
# Texpad
|
||||
.texpadtmp
|
||||
|
||||
# LyX
|
||||
*.lyx~
|
||||
|
||||
# Kile
|
||||
*.backup
|
||||
|
||||
# gummi
|
||||
.*.swp
|
||||
|
||||
# KBibTeX
|
||||
*~[0-9]*
|
||||
|
||||
# TeXnicCenter
|
||||
*.tps
|
||||
|
||||
# auto folder when using emacs and auctex
|
||||
./auto/*
|
||||
*.el
|
||||
|
||||
# expex forward references with \gathertags
|
||||
*-tags.tex
|
||||
|
||||
# standalone packages
|
||||
*.sta
|
||||
|
||||
# Makeindex log files
|
||||
*.lpz
|
||||
|
||||
# xwatermark package
|
||||
*.xwm
|
||||
|
|
@ -0,0 +1,250 @@
|
|||
<!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>
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 18 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 30 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 80 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 80 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 91 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 71 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 73 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 21 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 20 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 22 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 41 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 20 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 16 KiB |
|
|
@ -0,0 +1,32 @@
|
|||
@MISC{instruction,
|
||||
author = "Arne van Iterson",
|
||||
title = "Building instructions",
|
||||
url = {https://arnweb.nl/gitea/arne/EV5_Modcon/releases/download/pre-alpha/Instruction.pdf}
|
||||
},
|
||||
|
||||
@misc{htway,
|
||||
author = {David Lechner},
|
||||
howpublished = {Github},
|
||||
title = {Python port of the HiTechnic HTWay for ev3dev},
|
||||
year = {2010},
|
||||
url = {https://gist.github.com/dlech/11098915},
|
||||
}
|
||||
|
||||
@article{motorcomp,
|
||||
author = {Philippe Hurbain},
|
||||
title = {LEGO® 9V Technic Motors compared characteristics},
|
||||
year = {2012},
|
||||
url = {https://www.philohome.com/motors/motorcomp.htm}
|
||||
}
|
||||
|
||||
@ARTICLE{writers,
|
||||
author = "Los Angles Times",
|
||||
title = "Writers’ strike: What happened, how it ended and its impact on Hollywood",
|
||||
url = {https://www.latimes.com/entertainment-arts/business/story/2023-05-01/writers-strike-what-to-know-wga-guild-hollywood-productions}
|
||||
},
|
||||
|
||||
@ARTICLE{support,
|
||||
author = "Washington Post",
|
||||
title = "ChatGPT provided better customer service than his staff. He fired them.",
|
||||
url = {https://www.washingtonpost.com/technology/2023/10/03/ai-customer-service-jobs/}
|
||||
}
|
||||
986
doc/pendulum.tex
986
doc/pendulum.tex
File diff suppressed because it is too large
Load Diff
File diff suppressed because one or more lines are too long
Binary file not shown.
|
|
@ -0,0 +1,100 @@
|
|||
<mxfile host="Electron" modified="2024-01-09T14:53:40.737Z" agent="Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/22.0.3 Chrome/114.0.5735.289 Electron/25.8.4 Safari/537.36" etag="nXtkBaRbmuBOQNatfpNL" version="22.0.3" type="device">
|
||||
<diagram name="Pagina-1" id="PguFuK-8kF3X-0HopHM0">
|
||||
<mxGraphModel dx="810" dy="469" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="827" pageHeight="1169" math="1" shadow="0">
|
||||
<root>
|
||||
<mxCell id="0" />
|
||||
<mxCell id="1" parent="0" />
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-8" value="" style="endArrow=block;dashed=1;html=1;strokeWidth=1;rounded=0;exitX=0;exitY=0.5;exitDx=0;exitDy=0;startArrow=none;startFill=0;endFill=1;fillColor=#f5f5f5;strokeColor=#666666;" edge="1" parent="1">
|
||||
<mxGeometry width="50" height="50" relative="1" as="geometry">
|
||||
<mxPoint x="474" y="159.8" as="sourcePoint" />
|
||||
<mxPoint x="414" y="159.8" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-19" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;" vertex="1" parent="1">
|
||||
<mxGeometry x="460" y="150" width="20" height="20" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-17" value="" style="verticalLabelPosition=bottom;verticalAlign=top;html=1;shape=mxgraph.basic.arc;startAngle=0.7563628056739959;endAngle=0.24344062594951216;dashed=1;fillColor=#f5f5f5;fontColor=#333333;strokeColor=#666666;" vertex="1" parent="1">
|
||||
<mxGeometry x="135" y="20" width="210" height="210" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-1" value="" style="endArrow=none;html=1;rounded=0;" edge="1" parent="1">
|
||||
<mxGeometry width="50" height="50" relative="1" as="geometry">
|
||||
<mxPoint x="120" y="170" as="sourcePoint" />
|
||||
<mxPoint x="600" y="170" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-3" value="<font color="#ff7f2a">$$M$$</font>" style="rounded=0;whiteSpace=wrap;html=1;" vertex="1" parent="1">
|
||||
<mxGeometry x="200" y="120" width="80" height="30" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-5" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;" vertex="1" parent="1">
|
||||
<mxGeometry x="200" y="150" width="20" height="20" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-6" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;" vertex="1" parent="1">
|
||||
<mxGeometry x="260" y="150" width="20" height="20" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-7" value="" style="endArrow=block;dashed=1;html=1;strokeWidth=1;rounded=0;startArrow=none;startFill=0;endFill=1;fillColor=#f5f5f5;strokeColor=#666666;" edge="1" parent="1">
|
||||
<mxGeometry width="50" height="50" relative="1" as="geometry">
|
||||
<mxPoint x="270" y="134.8" as="sourcePoint" />
|
||||
<mxPoint x="330" y="134.8" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-13" value="" style="edgeStyle=isometricEdgeStyle;endArrow=none;html=1;rounded=1;fillColor=#ffe6cc;strokeColor=#d79b00;" edge="1" parent="1">
|
||||
<mxGeometry width="50" height="100" relative="1" as="geometry">
|
||||
<mxPoint x="210" y="50" as="sourcePoint" />
|
||||
<mxPoint x="240" y="20" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-14" value="<font color="#ff7f2a">$$m$$</font>" style="text;html=1;align=center;verticalAlign=middle;resizable=0;points=[];autosize=1;strokeColor=none;fillColor=none;rotation=0;" vertex="1" parent="1">
|
||||
<mxGeometry x="170" y="40" width="60" height="30" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-15" value="<font color="#ff7f2a">$$\alpha$$</font>" style="text;html=1;align=center;verticalAlign=middle;resizable=0;points=[];autosize=1;strokeColor=none;fillColor=none;" vertex="1" parent="1">
|
||||
<mxGeometry x="216" y="89" width="80" height="30" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-4" value="" style="endArrow=none;html=1;rounded=0;startArrow=oval;startFill=1;fillColor=#dae8fc;strokeColor=#6c8ebf;" edge="1" parent="1">
|
||||
<mxGeometry width="50" height="50" relative="1" as="geometry">
|
||||
<mxPoint x="240" y="20" as="sourcePoint" />
|
||||
<mxPoint x="239.8" y="120" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-18" value="" style="endArrow=none;html=1;rounded=0;startArrow=oval;startFill=1;fillColor=#dae8fc;strokeColor=#6c8ebf;" edge="1" parent="1" target="_G95BMDKVvPmWd-cPH-w-19">
|
||||
<mxGeometry width="50" height="50" relative="1" as="geometry">
|
||||
<mxPoint x="470" y="50" as="sourcePoint" />
|
||||
<mxPoint x="469.8" y="150" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-20" value="" style="endArrow=block;dashed=1;html=1;strokeWidth=1;rounded=0;exitX=1;exitY=0.5;exitDx=0;exitDy=0;startArrow=none;startFill=0;endFill=1;fillColor=#f5f5f5;strokeColor=#666666;" edge="1" parent="1">
|
||||
<mxGeometry width="50" height="50" relative="1" as="geometry">
|
||||
<mxPoint x="470" y="159.79999999999998" as="sourcePoint" />
|
||||
<mxPoint x="530" y="160.09" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-21" value="" style="endArrow=block;dashed=1;html=1;strokeWidth=1;rounded=0;startArrow=none;startFill=0;endFill=1;fillColor=#f5f5f5;strokeColor=#666666;" edge="1" parent="1">
|
||||
<mxGeometry width="50" height="50" relative="1" as="geometry">
|
||||
<mxPoint x="200" y="135" as="sourcePoint" />
|
||||
<mxPoint x="150" y="134.8" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-22" value="" style="verticalLabelPosition=bottom;verticalAlign=top;html=1;shape=mxgraph.basic.arc;startAngle=0.7287963039243575;endAngle=0.27281327830357194;dashed=1;fillColor=#f5f5f5;fontColor=#333333;strokeColor=#666666;" vertex="1" parent="1">
|
||||
<mxGeometry x="365" y="50" width="210" height="210" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-23" value="" style="verticalLabelPosition=bottom;verticalAlign=top;html=1;shape=mxgraph.basic.arc;startAngle=0;endAngle=0.24607104663499402;fillColor=#ffe6cc;strokeColor=#d79b00;dashed=1;dashPattern=1 1;" vertex="1" parent="1">
|
||||
<mxGeometry x="225" y="106" width="30" height="30" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-26" value="<font color="#ff7f2a">$$\alpha$$</font>" style="text;html=1;align=center;verticalAlign=middle;resizable=0;points=[];autosize=1;strokeColor=none;fillColor=none;" vertex="1" parent="1">
|
||||
<mxGeometry x="460" y="125" width="80" height="30" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-27" value="" style="verticalLabelPosition=bottom;verticalAlign=top;html=1;shape=mxgraph.basic.arc;startAngle=0;endAngle=0.269145619784681;fillColor=#ffe6cc;strokeColor=#d79b00;dashed=1;dashPattern=1 1;" vertex="1" parent="1">
|
||||
<mxGeometry x="440" y="136" width="60" height="60" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-28" value="" style="edgeStyle=isometricEdgeStyle;endArrow=none;html=1;rounded=1;fillColor=#ffe6cc;strokeColor=#d79b00;" edge="1" parent="1">
|
||||
<mxGeometry width="50" height="100" relative="1" as="geometry">
|
||||
<mxPoint x="440" y="119" as="sourcePoint" />
|
||||
<mxPoint x="470" y="89" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="_G95BMDKVvPmWd-cPH-w-29" value="<font color="#ff7f2a">$$m$$</font>" style="text;html=1;align=center;verticalAlign=middle;resizable=0;points=[];autosize=1;strokeColor=none;fillColor=none;rotation=0;" vertex="1" parent="1">
|
||||
<mxGeometry x="400" y="109" width="60" height="30" as="geometry" />
|
||||
</mxCell>
|
||||
</root>
|
||||
</mxGraphModel>
|
||||
</diagram>
|
||||
</mxfile>
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 12 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 32 KiB |
26
src/ev3.py
26
src/ev3.py
|
|
@ -1,26 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
|
||||
from ev3dev2.motor import LargeMotor, OUTPUT_B, OUTPUT_C, SpeedPercent, MoveTank
|
||||
from ev3dev2.sensor import INPUT_2
|
||||
from ev3dev2.sensor.lego import GyroSensor
|
||||
from ev3dev2.led import Leds
|
||||
|
||||
os.system('setfont Lat15-TerminusBold14')
|
||||
|
||||
# tank_drive = MoveTank(OUTPUT_B,OUTPUT_C)
|
||||
|
||||
gyro = GyroSensor(INPUT_2)
|
||||
|
||||
dt = 0
|
||||
t = time.time()
|
||||
|
||||
while (True):
|
||||
# Timekeepers
|
||||
dt = time.time() - t
|
||||
t = time.time()
|
||||
|
||||
# Gyro
|
||||
gyro_value = gyro.angle
|
||||
print("Gyro: " + str(gyro_value))
|
||||
|
|
@ -0,0 +1,90 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
import sys
|
||||
|
||||
from ev3dev2.motor import LargeMotor, OUTPUT_B, OUTPUT_C, SpeedPercent, MoveTank
|
||||
from ev3dev2.sensor import INPUT_2
|
||||
from ev3dev2.sensor.lego import GyroSensor
|
||||
from ev3dev2.button import Button
|
||||
|
||||
import time
|
||||
import logging
|
||||
from datetime import datetime
|
||||
|
||||
now = datetime.now() # current date and time
|
||||
LOGNAME = os.path.join(os.getcwd(), "logs/", (now.strftime("%m-%d_%H:%M:%S") + ".csv"))
|
||||
# print(LOGNAME)
|
||||
logging.basicConfig(filename=LOGNAME,
|
||||
filemode='a',
|
||||
format='%(message)s',
|
||||
level=logging.DEBUG)
|
||||
|
||||
os.system('setfont Lat15-TerminusBold14')
|
||||
|
||||
# Motors
|
||||
tank_drive = MoveTank(OUTPUT_B,OUTPUT_C)
|
||||
|
||||
def exit():
|
||||
print('Adios')
|
||||
tank_drive.stop()
|
||||
sys.exit(130)
|
||||
|
||||
# Button
|
||||
btn = Button()
|
||||
btn.on_backspace = exit
|
||||
|
||||
# Gyro
|
||||
gyro = GyroSensor(INPUT_2)
|
||||
gyro.mode = GyroSensor.MODE_GYRO_RATE
|
||||
|
||||
print("Put system in stable position!")
|
||||
for i in range(1, 0, -1):
|
||||
print(i)
|
||||
time.sleep(1)
|
||||
|
||||
gyro.calibrate()
|
||||
|
||||
# Keep time
|
||||
dt = 0
|
||||
t = time.time()
|
||||
error = 0
|
||||
|
||||
def main():
|
||||
global dt, t, error
|
||||
|
||||
Kp = 3
|
||||
Ki = 0
|
||||
Kd = 0.15
|
||||
|
||||
power = 0
|
||||
while (True):
|
||||
# Timekeepers
|
||||
dt = time.time() - t
|
||||
t = time.time()
|
||||
|
||||
# Gyro
|
||||
prev_err = error
|
||||
gyro_value = gyro.rate
|
||||
error += (gyro_value * dt)
|
||||
|
||||
power = (error * Kp) + (((error - prev_err) / (dt/1000)) * Kd)
|
||||
# logging.debug("%d;%d;%d", error, power)
|
||||
# print("Err: ", error, "Pwr: ", power)
|
||||
|
||||
if (power < -100):
|
||||
tank_drive.on(SpeedPercent(-100), SpeedPercent(-100))
|
||||
elif (power > 100):
|
||||
tank_drive.on(SpeedPercent(100), SpeedPercent(100))
|
||||
else:
|
||||
tank_drive.on(SpeedPercent(power), SpeedPercent(power))
|
||||
|
||||
# Has likely fallen over
|
||||
if (error > 40 or error < -40):
|
||||
exit()
|
||||
|
||||
if __name__ == '__main__':
|
||||
try:
|
||||
main()
|
||||
except KeyboardInterrupt:
|
||||
exit()
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
|
||||
from ev3dev2.motor import LargeMotor, OUTPUT_B, OUTPUT_C, SpeedPercent, MoveTank
|
||||
from ev3dev2.sensor import INPUT_2
|
||||
from ev3dev2.sensor.lego import GyroSensor
|
||||
from ev3dev2.led import Leds
|
||||
|
||||
os.system('setfont Lat15-TerminusBold14')
|
||||
|
||||
# tank_drive = MoveTank(OUTPUT_B,OUTPUT_C)
|
||||
|
||||
gyro = GyroSensor(INPUT_2)
|
||||
gyro.mode = GyroSensor.MODE_GYRO_RATE
|
||||
|
||||
dt = 0
|
||||
t = time.time()
|
||||
|
||||
error = 0.0
|
||||
total = 0
|
||||
for i in range(10):
|
||||
total += gyro.rate
|
||||
time.sleep(0.01)
|
||||
average = total / 10.0
|
||||
offset = average - 0.1
|
||||
|
||||
GYRO_OFFSET = 0.0005
|
||||
|
||||
while (True):
|
||||
# Timekeepers
|
||||
dt = time.time() - t
|
||||
t = time.time()
|
||||
|
||||
gyro_raw = gyro.rate
|
||||
offset = GYRO_OFFSET * gyro_raw + (1 - GYRO_OFFSET) * offset
|
||||
speed = (gyro_raw - offset)
|
||||
error += speed * (dt/1000)
|
||||
|
||||
# # Gyro
|
||||
# gyro_value = gyro.rate
|
||||
# error += (gyro_value * dt)
|
||||
|
||||
print("Gyro: " + str(error))
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
def exit():
|
||||
print('Adios')
|
||||
tank_drive.stop()
|
||||
sys.exit(130)
|
||||
|
|
@ -3,6 +3,8 @@
|
|||
# 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
|
||||
|
|
@ -22,14 +24,14 @@ WHEEL_RATIO_RCX = 1.4 # 81.6mm
|
|||
# the wheel size.
|
||||
KGYROANGLE = 7.5
|
||||
KGYROSPEED = 1.15
|
||||
KPOS = 0.07
|
||||
KSPEED = 0.1
|
||||
KPOS = 0
|
||||
KSPEED = 0
|
||||
|
||||
# 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
|
||||
KDRIVE = 0
|
||||
|
||||
# Power differential used for steering based on difference of target
|
||||
# steering and actual motor difference.
|
||||
|
|
@ -84,6 +86,7 @@ class Gyro:
|
|||
self.value0.seek(0)
|
||||
return int(self.value0.read())
|
||||
|
||||
|
||||
class EV3Gyro(Gyro):
|
||||
DRIVER_NAME = 'lego-ev3-gyro'
|
||||
|
||||
|
|
@ -111,23 +114,27 @@ class HTGyro(Gyro):
|
|||
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'))
|
||||
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.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())
|
||||
|
|
@ -139,7 +146,8 @@ class EV3Motor:
|
|||
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:
|
||||
with open(os.path.join(self.device.sys_path, 'command'),
|
||||
'w') as command:
|
||||
command.write(str(new_mode))
|
||||
|
||||
class EV3Motors:
|
||||
|
|
@ -185,23 +193,18 @@ def main():
|
|||
|
||||
gyro = Gyro.get_gyro()
|
||||
gyro.calibrate()
|
||||
|
||||
print("balancing in ...")
|
||||
print "balancing in ..."
|
||||
for i in range(5, 0, -1):
|
||||
print(i)
|
||||
print i
|
||||
os.system("beep -f 440 -l 100")
|
||||
time.sleep(1)
|
||||
print(0)
|
||||
|
||||
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)
|
||||
|
|
@ -225,6 +228,7 @@ def main():
|
|||
|
||||
time.sleep(WAIT_TIME)
|
||||
|
||||
|
||||
now_time = time.time()
|
||||
avg_interval = (now_time - start_time) / (loop_count+1)
|
||||
|
||||
|
|
@ -237,4 +241,4 @@ def main():
|
|||
motors.right.send_command('reset')
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
main()
|
||||
|
|
@ -0,0 +1,244 @@
|
|||
#!/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()
|
||||
|
|
@ -0,0 +1,125 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
# https://github.com/ev3dev/ev3dev-lang-python
|
||||
# https://github.com/ev3dev/ev3dev-lang-python-demo#balanc3r
|
||||
|
||||
from ev3dev2.motor import LargeMotor, OUTPUT_B, OUTPUT_C
|
||||
from ev3dev2.sensor import INPUT_2
|
||||
from ev3dev2.sensor.lego import GyroSensor
|
||||
from ev3dev2.sound import Sound
|
||||
|
||||
import time
|
||||
# import logging
|
||||
import os
|
||||
import csv
|
||||
|
||||
from datetime import datetime
|
||||
|
||||
now = datetime.now() # current date and time
|
||||
# LOGNAME = os.path.join(os.getcwd(), "logs/", (now.strftime("%m-%d_%H:%M:%S") + "final.csv"))
|
||||
# # print(LOGNAME)
|
||||
# logging.basicConfig(filename=LOGNAME,
|
||||
# filemode='a',
|
||||
# format='%(message)s',
|
||||
# level=logging.DEBUG)
|
||||
|
||||
log_t = []
|
||||
log_ang = []
|
||||
log_vang = []
|
||||
log_x = []
|
||||
log_v = []
|
||||
|
||||
# Motors
|
||||
l_motor = LargeMotor(OUTPUT_B)
|
||||
r_motor = LargeMotor(OUTPUT_C)
|
||||
|
||||
# Sensors
|
||||
gyro = GyroSensor(INPUT_2)
|
||||
|
||||
# Sound
|
||||
sound = Sound()
|
||||
|
||||
# tires are 56mm diameter
|
||||
def balance(target_angle=0):
|
||||
t = time.time()
|
||||
|
||||
# PID
|
||||
gyro_k_p = 7.5
|
||||
# float k_i = 0 # No integral in this system
|
||||
gyro_k_d = 1.15
|
||||
motor_k_p = 0.07
|
||||
motor_k_d = 0.1
|
||||
|
||||
# Calibrate gyro in current position
|
||||
gyro.calibrate()
|
||||
gyro.mode = GyroSensor.MODE_GYRO_G_A
|
||||
angle = gyro.angle
|
||||
|
||||
# Calibrate motor
|
||||
prev_sum_motor_pos = 0
|
||||
l_motor.reset()
|
||||
r_motor.reset()
|
||||
pos = 0
|
||||
|
||||
sound.speak('3, 2, 1')
|
||||
|
||||
# Stop if the robot has fallen over
|
||||
while abs(angle) < 40:
|
||||
# Keep time
|
||||
dt = time.time() - t
|
||||
t = time.time()
|
||||
|
||||
sum_motor_pos = l_motor.position + r_motor.position
|
||||
|
||||
delta_motor_pos = sum_motor_pos - prev_sum_motor_pos
|
||||
pos += delta_motor_pos
|
||||
speed = (delta_motor_pos / dt)
|
||||
|
||||
prev_sum_motor_pos = sum_motor_pos
|
||||
|
||||
angle, rate = gyro.angle_and_rate
|
||||
|
||||
log_t.append(t)
|
||||
log_ang.append(angle)
|
||||
log_vang.append(rate)
|
||||
log_x.append(pos)
|
||||
log_v.append(speed)
|
||||
|
||||
error = angle - target_angle
|
||||
pd = gyro_k_p * error + gyro_k_d * rate \
|
||||
+ motor_k_p * pos + motor_k_d * speed
|
||||
|
||||
# convert -100 ~ 100 to -1050 ~ 1050
|
||||
speed = (((pd - (-100)) * (1049 - (-1049))) / (100 - (-100))) + (-1049)
|
||||
l_motor.run_forever(speed_sp=speed)
|
||||
r_motor.run_forever(speed_sp=speed)
|
||||
|
||||
# logging.debug("%d;%d;%d", angle, rate, pd)
|
||||
|
||||
# if abs(pd) > 1050:
|
||||
# print("cap")
|
||||
|
||||
# speed = min(max(pd, -1050), 1050)
|
||||
|
||||
l_motor.stop(stop_action="hold")
|
||||
r_motor.stop(stop_action="hold")
|
||||
|
||||
|
||||
# Combine the arrays
|
||||
try:
|
||||
balance()
|
||||
except:
|
||||
l_motor.stop(stop_action="hold")
|
||||
r_motor.stop(stop_action="hold")
|
||||
finally:
|
||||
data = zip(log_t, log_ang, log_vang, log_x, log_v)
|
||||
|
||||
# Define the filename
|
||||
now = datetime.now() # current date and time
|
||||
LOGNAME = os.path.join(now.strftime("%m-%d_%H:%M:%S") + "new.csv")
|
||||
|
||||
# Write data to CSV file
|
||||
with open(LOGNAME, 'w', newline='') as csvfile:
|
||||
csvwriter = csv.writer(csvfile)
|
||||
csvwriter.writerow(['Time', 'Angle', 'Angular Velocity', 'X pos', 'Speed']) # Write header row
|
||||
csvwriter.writerows(data)
|
||||
|
|
@ -10,6 +10,9 @@ C_MTPRATIO = 100 # Pixels per meter
|
|||
C_P_ANG_START = 1 / 1000 * math.pi
|
||||
C_FALL_ANG = 52.5 / 100 * math.pi
|
||||
|
||||
C_ANG_RES_LIMIT = ((2 * math.pi) / 360) # rad
|
||||
C_V_CART_LIMIT = 0.403 # m/s
|
||||
C_A_CART_LIMIT = C_V_CART_LIMIT / 0.1 # m/s^2
|
||||
|
||||
class Pendulum:
|
||||
def __init__(self, theta, length, dx, mass, color):
|
||||
|
|
@ -35,6 +38,9 @@ class Pendulum:
|
|||
self.theta = [theta] # Angle in radians
|
||||
self.a_ang = [0] # Angular acceleration
|
||||
self.v_ang = [0] # Angular velocity
|
||||
|
||||
## Gyro offset
|
||||
self.theta_offset = 0
|
||||
|
||||
self.dx = dx # Horizontal displacement of "cart" from center
|
||||
self.a_cart = [0] # Acceleration of cart
|
||||
|
|
@ -49,9 +55,16 @@ class Pendulum:
|
|||
|
||||
## PID variables
|
||||
self.pid = True
|
||||
self.kp = 1.3
|
||||
self.pidDelay = 0
|
||||
# self.kp = 1.3
|
||||
# self.ki = 0.0
|
||||
# self.kd = 0.1
|
||||
self.kp = 7.5
|
||||
self.ki = 0.0
|
||||
self.kd = 0.1
|
||||
self.kd = 1.15
|
||||
|
||||
self.kp_m = 0.07
|
||||
self.kd_m = 0.1
|
||||
|
||||
def update(self, dt):
|
||||
self.doMath(dt)
|
||||
|
|
@ -60,7 +73,12 @@ class Pendulum:
|
|||
)
|
||||
|
||||
if self.pid:
|
||||
self.pidControl()
|
||||
self.pidDelay += dt
|
||||
|
||||
if self.pidDelay > 21:
|
||||
self.pidControl(self.pidDelay)
|
||||
self.theta_offset += self.pidDelay * (C_ANG_RES_LIMIT / 1000) ## One degree a second
|
||||
self.pidDelay = 0
|
||||
|
||||
if abs(self.theta[self.index]) == C_FALL_ANG:
|
||||
self.fallen = True
|
||||
|
|
@ -89,14 +107,14 @@ class Pendulum:
|
|||
|
||||
def doMath(self, dt):
|
||||
### ANGLE ###
|
||||
ang_term1 = self.a_cart[self.index] * math.cos(self.theta[self.index])
|
||||
ang_term2 = self.v_cart[self.index] * math.sin(self.theta[self.index])
|
||||
ang_term1 = self.a_cart[self.index] * math.cos(self.theta[self.index] + self.theta_offset)
|
||||
ang_term2 = self.v_cart[self.index] * math.sin(self.theta[self.index] + self.theta_offset)
|
||||
ang_term3 = (
|
||||
self.v_cart[self.index] # Previous cart velocity
|
||||
* self.v_ang[self.index] # previous angle velocity
|
||||
* math.sin(self.theta[self.index]) # Sin previous angle
|
||||
* math.sin(self.theta[self.index] + self.theta_offset) # Sin previous angle
|
||||
)
|
||||
ang_term4 = C_GRAVITY * math.sin(self.theta[self.index])
|
||||
ang_term4 = C_GRAVITY * math.sin(self.theta[self.index] + self.theta_offset)
|
||||
|
||||
# Angular acceleration
|
||||
self.a_ang.append(
|
||||
|
|
@ -110,10 +128,13 @@ class Pendulum:
|
|||
)
|
||||
|
||||
# Angular displacement
|
||||
self.theta.append(
|
||||
self.theta[self.index] # Previous angle
|
||||
+ (self.v_ang[self.index + 1] * (dt / 1000))
|
||||
)
|
||||
theta_res = self.theta[self.index] + (self.v_ang[self.index + 1] * (dt / 1000))
|
||||
theta_round = theta_res % C_ANG_RES_LIMIT
|
||||
if theta_round > 0.5 * C_ANG_RES_LIMIT:
|
||||
theta_res = theta_res + theta_round
|
||||
else:
|
||||
theta_res = theta_res - theta_round
|
||||
self.theta.append(theta_res)
|
||||
|
||||
# Limit fall of pendulum
|
||||
self.theta[self.index + 1] = self.clamp(
|
||||
|
|
@ -135,13 +156,20 @@ class Pendulum:
|
|||
)
|
||||
|
||||
# Cart acceleration
|
||||
self.a_cart.append((-cart_term1 + cart_term2) / (2 * self.mass))
|
||||
a_res = (-cart_term1 + cart_term2) / (2 * self.mass)
|
||||
self.a_cart.append(self.clamp(a_res, -C_A_CART_LIMIT, C_A_CART_LIMIT))
|
||||
# if abs(a_res) < C_A_CART_LIMIT:
|
||||
# self.a_cart.append(a_res)
|
||||
# else:
|
||||
# self.a_cart.append(C_A_CART_LIMIT * (a_res / abs(a_res)))
|
||||
|
||||
# Integrate acceleration to get velocity
|
||||
self.v_cart.append(
|
||||
self.v_cart[self.index] # Previous velocity
|
||||
+ (self.a_cart[self.index + 1] * (dt / 1000))
|
||||
)
|
||||
v_res = self.v_cart[self.index] + (self.a_cart[self.index + 1] * (dt / 1000)) # Previous velocity
|
||||
|
||||
if abs(v_res) < C_V_CART_LIMIT:
|
||||
self.v_cart.append(v_res)
|
||||
else:
|
||||
self.v_cart.append(C_V_CART_LIMIT * (v_res / abs(v_res)))
|
||||
|
||||
# Cart displacement
|
||||
self.s_cart.append(
|
||||
|
|
@ -167,6 +195,7 @@ class Pendulum:
|
|||
self.a_cart = [0]
|
||||
self.v_cart = [0]
|
||||
self.s_cart = [0]
|
||||
self.theta_offset = 0
|
||||
self.fallen = False
|
||||
self.update(0)
|
||||
|
||||
|
|
@ -193,7 +222,13 @@ class Pendulum:
|
|||
|
||||
plt.show()
|
||||
|
||||
def pidControl(self):
|
||||
error = self.theta[self.index]
|
||||
result = (self.kp * error) + (self.ki * sum(self.theta)) + (self.kd * self.v_ang[self.index])
|
||||
self.a_cart[self.index] = result * 10
|
||||
def pidControl(self, dt):
|
||||
error = self.theta[self.index]
|
||||
dt = (dt/1000)
|
||||
# result = (self.kp * error) + (self.ki * sum(self.theta)) + (self.kd * self.v_ang[self.index]) #PID
|
||||
result = (self.kp * error) \
|
||||
+ (((error - self.theta[self.index - 1]) / (dt + 1 / 1000)) \
|
||||
* self.kd) \
|
||||
+ (self.kp_m * self.s_cart[self.index]) \
|
||||
+ (self.kd_m * self.v_cart[self.index])
|
||||
self.a_cart[self.index] = self.clamp(result * 10, -C_A_CART_LIMIT, C_A_CART_LIMIT)
|
||||
|
|
@ -25,7 +25,7 @@ ui = SimUI(screen, pole)
|
|||
|
||||
# Pendulum setup
|
||||
# Start angle in radians, length, mass, color
|
||||
pendulum = Pendulum(0, 2, 0, 0.25, "red")
|
||||
pendulum = Pendulum(0, 0.2, 0, 0.7, "red")
|
||||
pendulum.reset()
|
||||
dx = 0 # x offset
|
||||
dt = 1 # delta time
|
||||
|
|
@ -100,9 +100,9 @@ while running:
|
|||
ui.grid(50, 0, 15)
|
||||
|
||||
# Update PID values
|
||||
pendulum.kp = slider_kp.getValue()
|
||||
pendulum.ki = slider_ki.getValue()
|
||||
pendulum.kd = slider_kd.getValue()
|
||||
# pendulum.kp = slider_kp.getValue()
|
||||
# pendulum.ki = slider_ki.getValue()
|
||||
# pendulum.kd = slider_kd.getValue()
|
||||
|
||||
# Update pendulum
|
||||
if not pendulum.fallen:
|
||||
|
|
|
|||
Loading…
Reference in New Issue