Assignment due
This commit is contained in:
parent
a0942f99a0
commit
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/}
|
||||
}
|
||||
897
doc/pendulum.tex
897
doc/pendulum.tex
|
|
@ -2,16 +2,20 @@
|
|||
|
||||
\documentclass{article} % \documentclass{} is the first command in any LaTeX code. It is used to define what kind of document you are creating such as an article or a book, and begins the document preamble
|
||||
|
||||
\usepackage[english]{babel}
|
||||
\usepackage[a4paper,top=2cm,bottom=2cm,left=2cm,right=2cm,marginparwidth=1.75cm]{geometry}
|
||||
\usepackage{amsmath} % \usepackage is a command that allows you to add functionality to your LaTeX code
|
||||
\usepackage{amsmath}
|
||||
\usepackage{multicol}
|
||||
\usepackage{textcomp}
|
||||
\usepackage{graphicx}
|
||||
\usepackage{float}
|
||||
\usepackage{lipsum}
|
||||
\usepackage{hyperref}
|
||||
\usepackage{listings}
|
||||
|
||||
\title{
|
||||
Single inverted pendulum\\~\
|
||||
\large{Modeling and Control course}
|
||||
Single inverted pendulum\linebreak
|
||||
\large{EV5 - Modelling and Control}
|
||||
}
|
||||
|
||||
\author{Arne van Iterson} % Sets authors name
|
||||
|
|
@ -23,47 +27,886 @@
|
|||
|
||||
|
||||
\begin{abstract}
|
||||
Very interesting Introduction
|
||||
|
||||
Electrical engineering students at the University of applied sciences Utrecht are assigned to simulate, build and control an inherently unstable system during the 'Modelling and Control' course of the third year. This paper describes the process of doing just that for a single inverted pendulum. The pendulum itself is built using LEGO\textsuperscript{\tiny\textregistered} Mindstorms EV3. The control system is a PID-loop is implemented using Python. A digital twin, also built in Python, is used to simulate the system.
|
||||
\end{abstract}
|
||||
|
||||
\noindent\makebox[\linewidth]{\rule{\linewidth}{0.4pt}}
|
||||
|
||||
\begin{multicols}{2} % creates two columns
|
||||
% Theory
|
||||
\section{Theory}
|
||||
The system that will be discussed in this paper is a single inverted pendulum. Common implementations of this system include a cart, this is not the case in this paper.
|
||||
\label{section:theory}
|
||||
The system that will be discussed in this paper is a single inverted pendulum. Common implementations of this system include a cart of some sort that carries the pendulum on a pivot point over a defined path.
|
||||
|
||||
While the system could theoretically stand up straight and never fall over, in practice the system is inherently unstable and will fall over if not controlled. The system can be controlled using a motor that moves the cart up and down the path to compensate for the pendulums falling movement. This method of control only requires a sensor to measure the angle of the pendulum.
|
||||
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{../res/segway.jpg}
|
||||
\caption{Segway\textsuperscript{\tiny\textregistered} in use}
|
||||
\label{fig:segway}
|
||||
\end{figure}
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{../res/drawio/system_diagram.png}
|
||||
\caption{System with (left) and without cart (right)}
|
||||
\label{fig:sysdiag}
|
||||
\end{figure}
|
||||
|
||||
The system used in this case of this assignment is slightly different; Instead of a cart that carries the pendulum, the pendulum itself is equipped with a motor and the angle is measured using a gyroscope.
|
||||
This difference has been visualised in figure \ref{fig:sysdiag}. The physics of the system are practically identical, however, it allows for some minor simplifications which will be discussed in section \ref{section:model}.
|
||||
|
||||
A practical example of this system is a Segway\textsuperscript{\tiny\textregistered} People Transporter (figure \ref{fig:segway}).
|
||||
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{../res/segway.jpg}
|
||||
\caption{Two Segway\textsuperscript{\tiny\textregistered} PT units in use}
|
||||
\label{fig:segway}
|
||||
\end{figure}
|
||||
|
||||
A practical example of this system is a Segway\textsuperscript{\tiny\textregistered} (figure \ref{fig:segway}). The system is inherently unstable, meaning that if the pendulum is not controlled, it will fall over.
|
||||
|
||||
\section{Model}
|
||||
\label{section:model}
|
||||
The physics behind the system in question have been described using the Lagrangian method. The Lagrangian method is a way of describing the dynamics of a system using the kinetic and potential energy of the system.
|
||||
|
||||
\subsection{Lagrangian}
|
||||
Usually the model for this system is divided into two parts: The cart and the pendulum. The cart is usually considered to be a mass $M$ that moves horizontally along a track. The pendulum is usually considered to be a mass $m$ that is attached to the cart using a pivot point. In this case however, since the motor is attached to the pendulum and the pivot point is the driving shaft of the motor itself, there is no separate mass for the cart. This means that $M$ is equal to $m$ and the formula can be simplified slightly.
|
||||
|
||||
\newpage
|
||||
|
||||
The kinetic energy of the system is made up from by the $x$ movement of the system and the falling of the pendulum. Since the system can only move in the $x$ direction using the motor, the kinetic energy is simply:
|
||||
\begin{equation}
|
||||
T_{1} = \frac{1}{2} m v^2 = \frac{1}{2} m \dot x^2
|
||||
\end{equation}
|
||||
Where $x$ is the displacement of the system in the $x$ direction and $m$ is the mass of the system. \\
|
||||
|
||||
The kinetic energy of the pendulum is influenced by both the $x$ and $y$ movements of the pendulum and therefore:
|
||||
\begin{equation*}
|
||||
T_{2} = \frac{1}{2} m (\dot x_p^2 + \dot y_p^2)
|
||||
\end{equation*}
|
||||
The $x$ and $y$ position of the top of the pendulum can be calculated using the length and angle, this assumes that the pendulum is upright at $\frac{1}{2}\pi$:
|
||||
\begin{equation*}
|
||||
\begin{split}
|
||||
x_{p} & = x + l\cdot sin(\theta) \\
|
||||
y_{p} & = -l\cdot cos(\theta)
|
||||
\end{split}
|
||||
\end{equation*}
|
||||
The $x$ and $y$ velocity can be derived from the position:
|
||||
\begin{equation*}
|
||||
\begin{split}
|
||||
\dot{x}_{p} & = \dot{x} + l\cdot cos(\theta)\cdot \dot{\theta} \\
|
||||
\dot{y}_{p} & = l\cdot sin(\theta)\cdot \dot{\theta}
|
||||
\end{split}
|
||||
\end{equation*}
|
||||
Putting this all together gives the kinetic energy of the pendulum:
|
||||
\begin{align*}
|
||||
T_{2} &= \frac{1}{2} m (\dot x_s^2 + \dot y_s^2) \\
|
||||
T_{2} &= \frac{1}{2} m((\dot x + \ell \dot \theta cos(\theta))^2 + (\ell \dot \theta sin(\theta))^2) \\
|
||||
T_{2} &= \frac{1}{2} m(\dot x^2 + 2 \dot x \ell \dot \theta cos(\theta) + \ell^2 \dot \theta^2 cos(\theta)^2 + \ell^2 \dot \theta^2 sin(\theta)^2) \\
|
||||
T_{2} &= \frac{1}{2} m(\dot x^2 + 2 \dot x \ell \dot \theta cos(\theta) + \ell^2 \dot \theta^2(cos(\theta)^2 + sin(\theta)^2)) \\
|
||||
\intertext{Given that $cos(x)^2 + sin(x)^2 = 1$:}
|
||||
T_{2} &= \frac{1}{2} m(\dot x^2 + 2 \dot x \ell \dot \theta cos(\theta) + \ell^2 \dot \theta^2)
|
||||
\intertext{Combining $T_1$ and $T_2$ gives the total kinetic energy of the system:}
|
||||
T & = T_1 + T_2 \\
|
||||
T & = \frac{1}{2} m \dot x^2 + \frac{1}{2} m(\dot x^2 + 2 \dot x \ell \dot \theta cos(\theta) + \ell^2 \dot \theta^2) \\
|
||||
T & = m \dot x^2 + \frac{1}{2} m(2 \dot x \ell \dot \theta cos(\theta) + \ell^2 \dot \theta^2) \\
|
||||
T & = m \dot x^2 + m \dot x \ell \dot \theta cos(\theta) + \frac{1}{2} m\ell^2 \dot \theta^2
|
||||
\end{align*}
|
||||
The potential energy of the system is:
|
||||
\begin{equation}
|
||||
V = -mg\ell cos(\theta)
|
||||
\end{equation}
|
||||
This makes the full Lagrangian of the system:
|
||||
\begin{equation}
|
||||
\begin{split}
|
||||
\mathcal{L} & = T - V \\
|
||||
\mathcal{L} & = m\dot x^2 + m \dot x \ell \dot \theta cos(\theta) + \frac{1}{2}m \ell^2 \dot \theta^2 + mg\ell cos(\theta)
|
||||
\end{split}
|
||||
\end{equation}
|
||||
|
||||
Since the falling pendulum influences the moving pivot point ("cart") and vice versa, the Lagrangian will have to be solved for both the angular acceleration $\ddot\theta$ of the pendulum and the horizontal acceleration of the pivot $\ddot{x}$
|
||||
|
||||
\subsection{Horizontal acceleration}
|
||||
The horizontal acceleration of the pivot point can be derived from the Lagrangian using the following formula:
|
||||
\begin{equation}
|
||||
\label{eq:horac}
|
||||
\frac{d}{dt} \left(\frac{\partial \mathcal{L}}{\partial \dot x} \right) = \frac{\partial \mathcal{L}}{\partial x}
|
||||
\end{equation}
|
||||
Filling in for $\dot x$
|
||||
% Dependant on xdot
|
||||
\begin{equation}
|
||||
\label{eq:horacxdot}
|
||||
\begin{split}
|
||||
&\ m\dot x^2 + m \dot x \ell \dot \theta cos(\theta) \\
|
||||
\frac{\partial \mathcal{L}}{\partial \dot x} =&\ 2m\dot x + m \ell \dot \theta cos(\theta) \\
|
||||
\frac{d}{dt} \left(\frac{\partial \mathcal{L}}{\partial \dot x} \right) =&\ 2m\ddot x + m \ell \ddot \theta cos(\theta) - m \ell \dot \theta sin(\theta)
|
||||
\end{split}
|
||||
\end{equation}
|
||||
Filling in for $x$
|
||||
% Dependant on x
|
||||
\begin{equation}
|
||||
\label{eq:horacx}
|
||||
\frac{\partial \mathcal{L}}{\partial x} = 0
|
||||
\end{equation}
|
||||
Combining formulas \ref{eq:horacxdot} and \ref{eq:horacx} using formula \ref{eq:horac} gives:
|
||||
\begin{equation}
|
||||
2m\ddot x + m \ell \ddot \theta cos(\theta) - m \ell \dot \theta sin(\theta) = 0
|
||||
\end{equation}
|
||||
Which gives:
|
||||
\begin{equation}
|
||||
\begin{split}
|
||||
2m\ddot x &= - m \ell \ddot \theta cos(\theta) + m \ell \dot \theta sin(\theta) \\
|
||||
\ddot x &= -\frac{m \ell \ddot \theta cos(\theta) + m \ell \dot \theta sin(\theta)}{2m}
|
||||
\end{split}
|
||||
\end{equation}
|
||||
|
||||
\subsection{Angular acceleration}
|
||||
\begin{equation}
|
||||
\label{eq:angac}
|
||||
\frac{d}{dt} \left(\frac{\partial \mathcal{L}}{\partial \dot \theta} \right) = \frac{\partial \mathcal{L}}{\partial \theta}
|
||||
\end{equation}
|
||||
Filling in for $\dot \theta$
|
||||
% Dependant on thetadot
|
||||
\begin{equation}
|
||||
\label{eq:angacthetadot}
|
||||
\begin{split}
|
||||
&\ m \dot x \ell \dot \theta cos(\theta) + \frac{1}{2}m \ell^2 \dot \theta^2 \\
|
||||
\frac{\partial \mathcal{L}}{\partial \dot \theta} =&\ m \dot x \ell cos(\theta) + m \ell^2 \dot\theta\\
|
||||
\frac{d}{dt} \left(\frac{\partial \mathcal{L}}{\partial \dot \theta} \right) =&\ m \ddot x \ell cos(\theta) - m \dot x \ell sin(\theta) + m \ell^2 \ddot\theta\\
|
||||
\end{split}
|
||||
\end{equation}
|
||||
Filling in for $\theta$
|
||||
% Dependant on theta
|
||||
\begin{equation}
|
||||
\label{eq:angactheta}
|
||||
\begin{split}
|
||||
&\ m \dot x \ell \dot \theta cos(\theta) + mg\ell cos(\theta) \\
|
||||
\frac{\partial \mathcal{L}}{\partial \theta} =&\ -m \dot x \ell \dot \theta sin(\theta) - mg\ell sin(\theta) \\
|
||||
\end{split}
|
||||
\end{equation}
|
||||
Combining formulas \ref{eq:angacthetadot} and \ref{eq:angactheta} using formula \ref{eq:angac} gives:
|
||||
\begin{equation}
|
||||
\begin{split}
|
||||
m \ell^2 \ddot\theta =& - m \ddot x \ell cos(\theta) + m \dot x \ell sin(\theta) -m \dot x \ell \dot \theta sin(\theta) - mg\ell sin(\theta) \\
|
||||
\ell \ddot\theta =& - \ddot x cos(\theta) + \dot x sin(\theta) -\dot x \dot \theta sin(\theta) - g sin(\theta) \\
|
||||
\ddot\theta =& - \frac{\ddot x cos(\theta) - \dot x sin(\theta) + \dot x \dot \theta sin(\theta) + g sin}{l}
|
||||
\end{split}
|
||||
\end{equation}
|
||||
|
||||
% Make clear that the simulation is built from the ground up and pygame is only used for the UI
|
||||
\subsection{Simulation}
|
||||
The formula's in the previous sections have then been put into a purpose built Python simulation suite called Pendulum Simulator 4000. The source code for this can be found in the appendix and on git.
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{../res/screenshot2.png}
|
||||
\caption{Main window of Pendulum Simulator 4000}
|
||||
\label{fig:simmain}
|
||||
\end{figure}
|
||||
|
||||
% Hardware setup
|
||||
\section{Setup}
|
||||
To simplify hardware design, the system will be built and run on a LEGO\textsuperscript{\tiny\textregistered} Mindstorms EV3 development kit. The programmable brick itself, which will be referred to as "EV3", runs a Linux distribution known as ev3dev. Ev3dev allows for various programming languages to be used including Python and C. Both of which will be further explained later.
|
||||
To simplify hardware design, the system will be built and run on a LEGO\textsuperscript{\tiny\textregistered} Mindstorms EV3 development kit. The programmable brick itself, hereinafter referred to as "EV3", runs a Linux distribution known as ev3dev. Ev3dev allows for various programming languages to be used including Python and C. Both of which will be further explained in section \ref{section:control}.
|
||||
|
||||
To reproduce the robot in Figure \ref{fig:balancer}, please refer to the building instructions in the appendix.
|
||||
To reproduce the robot in Figure \ref{fig:balancer}, please refer to the building instructions. \cite{instruction}.
|
||||
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{../res/bricklink/modcon_balancer2.png}
|
||||
\caption{Balancing robot}
|
||||
\label{fig:balancer}
|
||||
\end{figure}
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{../res/bricklink/modcon_balancer2.png}
|
||||
\caption{Balancing robot}
|
||||
\label{fig:balancer}
|
||||
\end{figure}
|
||||
|
||||
\section{Model}
|
||||
\subsection{Linearisation}
|
||||
\section{Control}
|
||||
\label{section:control}
|
||||
In the simulation it was found that the system was best controlled using a PD-loop, this was to be expected since the system includes integration by it's very nature. The control loop was implemented using Python and the ev3dev library. The source code for this can be found in the appendix.
|
||||
|
||||
\section{Control}
|
||||
dsaf
|
||||
The choice of hardware has significantly influenced the control system. Unfortunately, it has brought along a lot of issues with the physical model. The gyroscope used has a very limited resolution of 1 degree (1 deg) for angle and 1 degree per second (1 deg/sec) for angular velocity. Add the fact that the sensor drifts about 0,5 deg/sec and the system becomes very unstable very quickly.
|
||||
|
||||
\section{Validation}
|
||||
fsafdfa
|
||||
This issue would probably have been overcome if the system was easier to program, in its current state the EV3 runs a limited version of Python that takes about two minutes to load every time the program is run. Adding any reasonable logging, through the file system or through dumping python arrays, makes the timing too slow to keep the control loop running fast enough causing the robot to lose balance fairly quickly.
|
||||
|
||||
There has been some success in using the C programming language and a compiling toolchain has been set-up, however, the control of simple features like the motor is very low level. The plan was to first get it working in Python and then port it to C but time has unfortunately run out.
|
||||
|
||||
\section{Conclusion}
|
||||
asfjdklasdfas
|
||||
In order to get the system to balance at all, the control loop had to be altered to include the position and speed of the motors as has been done in the HTWay project by D. Lechner \cite{htway}. The theory behind this is that if the system is still falling in a certain direction while the motors are already moving to compensate that, the movement speed should be increased. In a way the system now uses a cascaded control loop to balance.
|
||||
|
||||
A small set of data points has been collected by pre-assigning memory to a numpy array and writing to it directly, then dumping this to non-volatile memory. This gives some insight into the issues with the system, however, this also limits the run time of the system.
|
||||
|
||||
\section{Validation}
|
||||
The behaviour of the system is as follows:
|
||||
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{./img/ang_run_1.png}
|
||||
\caption{Run 1}
|
||||
\end{figure}
|
||||
|
||||
The fact that the system oscillates is a typical sign of an ill-tuned control loop. The PID values have been tuned to the best of my ability but the system is still very unstable. Tuning the system has proven difficult due to the inclusion of the motor speed and position in the control loop.
|
||||
|
||||
The system is able to say upright using the HTWay project by D. Lechner \cite{htway}. Some of the control logic has been used in this project, however the HTWay project includes some filtering logic for the gyro sensor that I did not yet understood well enough to implement.
|
||||
\\\\
|
||||
The fact that I was unable to get the system to balance properly should not change the fact that the simulation should behave the same given the same characteristics and control constants as the physical model, which is not the case.
|
||||
|
||||
Running the simulation tuned exactly the same as the physical model reveals an interesting issue; The simulated system is impossible to topple over, which is clearly not the case with the physical model.
|
||||
\\\\
|
||||
The simulation and the model differ in a few ways:
|
||||
\begin{itemize}
|
||||
\item The simulation does not include sensor drift or resolution
|
||||
\item The simulation does not include the physical limitations of the motor
|
||||
\item The simulation loop runs significantly faster than the physical model does
|
||||
\end{itemize}
|
||||
|
||||
Drawing any comparisons between the two will be largely useless at this point, so instead we will limit the simulation to check if the simulation will then mimic the actual behaviour.
|
||||
|
||||
\subsection{Limiting the simulated system}
|
||||
First, the resolution of the gyroscope will be limited to 1 degree. Converting this to radians gives a resolution of 0.0175 rad. The result can be seen in \ref{fig:simreslim1}. Note that the graph has been cropped, showing only a stable part of the simulation.
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{./img/sim_run_resolutionlimit1.png}
|
||||
\caption{Simulation run with angle resolution limit}
|
||||
\label{fig:simreslim1}
|
||||
\end{figure}
|
||||
|
||||
The limited resolution does cause the system to oscillate slightly, but not escalating as expected. Additionally, the system is still impossible to topple over.
|
||||
\\\\
|
||||
Next, the simulation will be limited in update speed. This means that the physics simulation will continue while the control loop can only update every certain interval. The log file from the physical model reveals that the average update time for the control loop is about 21 ms or 47 frames per second (fps).
|
||||
|
||||
Currently, the simulation runs at 60 fps, the update call for the pendulum control is called every frame. The pendulum control has been changed to only actually control the system after the dt value is more or equal to 21 ms. Because the program runs at 60 fps or roughly 16 ms per frame, the actual PID loop will be called every 32 ms; This is longer than required, but should demonstrate the effect of the delay. The result can be seen in \ref{fig:simpidlim1}.
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{./img/sim_run_pidcontroldelay.png}
|
||||
\caption{Simulation run with PID control delay}
|
||||
\label{fig:simpidlim1}
|
||||
\end{figure}
|
||||
|
||||
Interestingly, the limited update frequency of the PID controller seems to act as a low-pass filter, reducing the oscillations in the system. The user is now able to topple the system over.
|
||||
\\\\
|
||||
Next, the simulation will be limited in the speed at which it can change the horizontal position of the pendulum. The motor speed on the EV3 is measured in an arbitrary value between -1050 and 1050. The max speed of the motor is about 175 rpm as stated in the research by P. Hurbain. \cite{motorcomp}. Combined with the 44 mm diameter of the used wheels gives the maximum speed of the robot as:
|
||||
\begin{equation}
|
||||
\begin{split}
|
||||
r_{wheel} &= 22 \text{[mm]}\\
|
||||
s_{circumference} &= 2\pi r_{wheel} = 138.23 \text{mm} = 0.1382 \text{m}\\
|
||||
v_{max} &= (175 / 60) \cdot 0.1382 \\
|
||||
&= 2.92 \cdot 0.1382 = 0.403 \text{m/s}
|
||||
\end{split}
|
||||
\end{equation}
|
||||
|
||||
The simulation has been changed to never exceed the speed limit of 0.403 m/s even if the controller asks for it, just like the EV3 motors would do. Limiting the simulation to 0.403 m/s did little to change the behaviour of the simulation. As can be seen in \ref{fig:simslms}, the system never reaches this speed in the first place.
|
||||
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{./img/sim_run_speedlimit.png}
|
||||
\caption{Simulation run with speed limit}
|
||||
\label{fig:simslms}
|
||||
\end{figure}
|
||||
|
||||
Interesting however is the acceleration as can be seen in \ref{fig:simslms2}.
|
||||
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{./img/sim_run_speedlimit_ms2.png}
|
||||
\caption{Simulation run with speed limit}
|
||||
\label{fig:simslms2}
|
||||
\end{figure}
|
||||
|
||||
The initial kick in acceleration is the attempt to topple the system over by the user. The response is quite harsh, reaching up to 7 m/s\textsuperscript{2} which is only slightly impossible for the physical system to achieve. Unfortunately, this is also quite difficult to limit, since the acceleration of the motor would require a measuring setup to determine.
|
||||
|
||||
If we make a generous assumption that the motor can reach it's top speed within 0.15 seconds, the acceleration would be:
|
||||
\begin{equation}
|
||||
\begin{split}
|
||||
v_{max} &= 0.403 \text{m/s} \\
|
||||
t_{max} &= 0.1 \text{s} \\
|
||||
a_{max} &= \frac{v_{max}}{t_{max}} = \frac{0.403}{0.15} = 2.69\text{m/s}^2
|
||||
\end{split}
|
||||
\end{equation}
|
||||
|
||||
This does cause the system to fall over in the simulation as expected, though not as quickly as the physical model and with less oscillations.
|
||||
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{./img/sim_run_alllimits.png}
|
||||
\caption{Simulation run with all limits in place}
|
||||
\label{fig:simalllim1}
|
||||
\end{figure}
|
||||
|
||||
Lastly, the sensor will be offset with about 0,5 degrees per second which will be updated every time the control loop is run. The results can be seen in \ref{fig:simdrift1}.
|
||||
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{./img/sim_run_alllimits2.png}
|
||||
\caption{Simulation run with sensor drift}
|
||||
\label{fig:simdrift1}
|
||||
\end{figure}
|
||||
|
||||
With that, we have successfully created a digital twin for a poorly tuned system. The system is now no longer able to balance properly, it oscillates and falls over just like the physical model.
|
||||
|
||||
\section{Conclusion}
|
||||
Due to issues with the physical model, this entire project has run slightly backwards. Having to alter the control loop significantly nearing the end of the project has caused the physical model to behave differently than the simulation to a point where comparing them was not really possible.
|
||||
The simulation has been altered to include the limitations of the physical model as closely as possible, which has been successful and the resulting behaviour is comparable to the physical model. So while the system does not balance, at least it makes sense.
|
||||
|
||||
\section{Recommendations}
|
||||
\begin{enumerate}
|
||||
\item Do not make a robot using LEGO.
|
||||
\end{enumerate}
|
||||
|
||||
\end {multicols}
|
||||
|
||||
\newpage
|
||||
|
||||
\section{Ethics of Artificial Intelligence}
|
||||
\label{appendix:ethics}
|
||||
In Utilitarian ethics the greatest good for the greatest number is the most important. Whatever happens along the way does not matter as long as the outcome is good for the most people or society as a whole. The last couple of years have seen great development in the field of Artificial Intelligence, which brings along the question of how and if this technology should be used from a utilitarian perspective.
|
||||
\\\\
|
||||
The answer to this question is not as simple as it may seem. On the one hand, AI can be used to automate tasks that are dangerous or tedious for humans. This would allow them to focus on more important tasks and would reduce the amount of work related injuries; Which would be favourable according to utilitarian ethics. This also means however, that it would replace work that is currently done by humans, therefore leading to a loss of jobs and, potentially, a group of people that are unable to find work.
|
||||
\\\\
|
||||
Then again, we are currently experiencing the development of AI, which means that what is happening now might just be part of the AI development that will eventually lead to the greatest thing humans have ever done that will eventually benefit everyone. In that case, we might just have to accept the loss of jobs and the other negative effects of AI development as a necessary evil until the great-for-everyone endpoint is reached.
|
||||
\\\\
|
||||
This great-for-everyone endpoint however, is not a certainty and if it is ever reached, it might take a very long time. Therefore, I am of the opinion that not all types of AI should be treated with the same ethical standards.
|
||||
\\\\
|
||||
In the context of modelling and control, I think most of the work described in this paper, maybe with the exception of building the physical model itself, will no longer be done by a person in the near future. Writing this paper and parts of the software in VSCode with Github Copilot Enabled has taught me that with a more limited context compared to generic AI, like ChatGPT, the AI 'understood' more complex code structures and physics than I expected. At the moment, I don't think it would be a great idea to run Copilot code without any checking by a human, but I do think that it will be possible in the near future.
|
||||
\\\\
|
||||
There is a lot more to engineering than just coding, therefore I don't think engineers as a whole will be replaced at all. AI development towards the aid of developers will likely not directly cost any jobs. There have been cases where it has or likely will, especially creative jobs, journalism and administrative jobs. \cite{writers} This type of development, I believe, should not stop but should be kept in check. We have seen the writers strikes and the companies firing their entire support staff after OpenAI opened the GPT-3 API. \cite{support} While it might be part of the upcoming great-for-everyone endpoint, right now I do not believe we should allow AI to completely replace jobs with half-baked solutions for the sake of profit.
|
||||
\\\\
|
||||
|
||||
\newpage
|
||||
|
||||
\bibliographystyle{IEEEtran}
|
||||
\bibliography{pendulum}
|
||||
|
||||
\newpage
|
||||
|
||||
\appendix
|
||||
\section{Simulation source code}
|
||||
\subsection{sim.py}
|
||||
\begin{lstlisting}[language=Python]
|
||||
# Pendulum simulator 4000
|
||||
# Arne van Iterson, 2023
|
||||
|
||||
# Imports
|
||||
import pygame_widgets
|
||||
import pygame
|
||||
from pygame_widgets.slider import Slider
|
||||
from pygame.math import Vector2
|
||||
import math
|
||||
|
||||
# pygame setup
|
||||
pygame.init()
|
||||
screen = pygame.display.set_mode((1280, 720))
|
||||
clock = pygame.time.Clock()
|
||||
running = True
|
||||
update = True
|
||||
pole = Vector2(screen.get_rect().center) # center of screen
|
||||
|
||||
# Own objects must be imported after pygame init
|
||||
from pendulum import Pendulum
|
||||
from uiHelpers import *
|
||||
|
||||
# UI helpers
|
||||
ui = SimUI(screen, pole)
|
||||
|
||||
# Pendulum setup
|
||||
# Start angle in radians, length, mass, color
|
||||
pendulum = Pendulum(0, 2, 0, 0.25, "red")
|
||||
pendulum.reset()
|
||||
dx = 0 # x offset
|
||||
dt = 1 # delta time
|
||||
|
||||
# Sliders
|
||||
slider_kp = Slider(screen, 910, 590, 320, 16, initial=pendulum.kp, min=0, max=2, step=0.001)
|
||||
slider_ki = Slider(screen, 910, 620, 320, 16, initial=pendulum.ki, min=0, max=0.5, step=0.001)
|
||||
slider_kd = Slider(screen, 910, 650, 320, 16, initial=pendulum.kd, min=0, max=0.5, step=0.001)
|
||||
|
||||
# Gametime
|
||||
rt = 10 # run time
|
||||
highscore = 0
|
||||
|
||||
|
||||
# Metadata values
|
||||
def meta():
|
||||
ui.meta(pendulum.theta[pendulum.index], "Theta")
|
||||
ui.meta(pendulum.a_ang[pendulum.index], "Angular acceleration")
|
||||
ui.meta(pendulum.dx, "dx")
|
||||
ui.meta(pendulum.a_cart[pendulum.index], "Cart acceleration")
|
||||
|
||||
ui.meta(pendulum.pid, "Control")
|
||||
|
||||
ui.meta(pendulum.kd, "Kd")
|
||||
ui.meta(pendulum.ki, "Ki")
|
||||
ui.meta(pendulum.kp, "Kp")
|
||||
|
||||
ui.meta(dt, "dt")
|
||||
|
||||
ui.meta(not update, "Paused")
|
||||
ui.meta(rt / 1000, "Run time [s]")
|
||||
ui.meta(highscore / 1000, "Highscore [s]")
|
||||
|
||||
|
||||
while running:
|
||||
events = pygame.event.get()
|
||||
### User controls ###
|
||||
for event in events:
|
||||
# Quit
|
||||
if event.type == pygame.QUIT:
|
||||
running = False
|
||||
elif event.type == pygame.KEYDOWN:
|
||||
# Quit
|
||||
if event.key == pygame.K_ESCAPE:
|
||||
running = False
|
||||
# Reset simulation
|
||||
elif event.key == pygame.K_SPACE:
|
||||
pendulum.reset()
|
||||
rt = 0
|
||||
# Pause simulation
|
||||
elif event.key == pygame.K_p:
|
||||
update = not update
|
||||
# Display plot if simulation is not running
|
||||
elif event.key == pygame.K_g:
|
||||
if pendulum.fallen:
|
||||
pendulum.plot()
|
||||
else:
|
||||
update = False
|
||||
pendulum.plot()
|
||||
# Toggle PID controller
|
||||
elif event.key == pygame.K_c:
|
||||
pendulum.pid = not pendulum.pid
|
||||
|
||||
# Move pendulum
|
||||
keys = pygame.key.get_pressed()
|
||||
if keys[pygame.K_LEFT] or keys[pygame.K_a]:
|
||||
pendulum.a_cart[pendulum.index] -= 4
|
||||
if keys[pygame.K_RIGHT] or keys[pygame.K_d]:
|
||||
pendulum.a_cart[pendulum.index] += 4
|
||||
|
||||
# Draw grid
|
||||
ui.grid(50, 0, 15)
|
||||
|
||||
# Update PID values
|
||||
pendulum.kp = slider_kp.getValue()
|
||||
pendulum.ki = slider_ki.getValue()
|
||||
pendulum.kd = slider_kd.getValue()
|
||||
|
||||
# Update pendulum
|
||||
if not pendulum.fallen:
|
||||
if update:
|
||||
rt += dt
|
||||
pendulum.update(dt)
|
||||
else:
|
||||
ui.gameover(rt)
|
||||
|
||||
# Update highscore
|
||||
if rt > highscore:
|
||||
highscore = rt
|
||||
|
||||
# Draw metadata
|
||||
ui.update(dt)
|
||||
meta()
|
||||
|
||||
# Draw pendulum
|
||||
dx = (pendulum.dx, 0)
|
||||
pygame.draw.line(screen, pendulum.color, pole + dx, pole + pendulum.vector + dx, 3)
|
||||
pygame.draw.circle(screen, "black", pole + dx, 15, 3)
|
||||
|
||||
# Draw frame
|
||||
pygame_widgets.update(events)
|
||||
pygame.display.flip()
|
||||
dt = clock.tick(60) # limits FPS to 120
|
||||
|
||||
pygame.quit()
|
||||
|
||||
\end{lstlisting}
|
||||
\subsection{pendulum.py}
|
||||
\begin{lstlisting}[language=Python]
|
||||
from pygame.math import Vector2
|
||||
import math
|
||||
import numpy as np
|
||||
import random
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
# Constants
|
||||
C_GRAVITY = 9.81 # m/s^2
|
||||
C_MTPRATIO = 100 # Pixels per meter
|
||||
C_P_ANG_START = 1 / 1000 * math.pi
|
||||
C_FALL_ANG = 52.5 / 100 * math.pi
|
||||
|
||||
|
||||
class Pendulum:
|
||||
def __init__(self, theta, length, dx, mass, color):
|
||||
"""
|
||||
Initialize a Pendulum object.
|
||||
|
||||
Parameters:
|
||||
theta (float): Angle [rad].
|
||||
length (float): Length of the pendulum [m].
|
||||
dx (float): Horizontal displacement of the "cart" from the center [m].
|
||||
mass (float): Mass of the pendulum for physics calculations [kg].
|
||||
color (str): Display color.
|
||||
|
||||
Returns:
|
||||
None
|
||||
"""
|
||||
## Game variables
|
||||
self.vector = None # Vector2 object
|
||||
self.fallen = False # Stop when pendulum falls over
|
||||
|
||||
## Physics variables
|
||||
self.index = 0 # Index helper for plotting graphs
|
||||
self.theta = [theta] # Angle in radians
|
||||
self.a_ang = [0] # Angular acceleration
|
||||
self.v_ang = [0] # Angular velocity
|
||||
|
||||
self.dx = dx # Horizontal displacement of "cart" from center
|
||||
self.a_cart = [0] # Acceleration of cart
|
||||
self.v_cart = [0] # Velocity of cart
|
||||
self.s_cart = [0] # Displacement of cart [m]
|
||||
|
||||
# self.r_factor = 0.50 # Damping factor
|
||||
|
||||
self.length = length # Length of pendulum
|
||||
self.mass = mass # Mass of pendulum for physics
|
||||
self.color = color # Display color
|
||||
|
||||
## PID variables
|
||||
self.pid = True
|
||||
self.kp = 1.3
|
||||
self.ki = 0.0
|
||||
self.kd = 0.1
|
||||
|
||||
def update(self, dt):
|
||||
self.doMath(dt)
|
||||
self.vector = Vector2.from_polar(
|
||||
((self.length * C_MTPRATIO), math.degrees(self.theta[self.index] - (0.5 * math.pi)))
|
||||
)
|
||||
|
||||
if self.pid:
|
||||
self.pidControl(dt)
|
||||
|
||||
if abs(self.theta[self.index]) == C_FALL_ANG:
|
||||
self.fallen = True
|
||||
|
||||
# def update(self, dt):
|
||||
# """
|
||||
# Update the pendulum's state based on the elapsed time.
|
||||
|
||||
# Parameters:
|
||||
# - dt (float): The elapsed time in milliseconds.
|
||||
|
||||
# Returns:
|
||||
# None
|
||||
# """
|
||||
# a_ang = (-(C_GRAVITY * math.sin(self.theta)) / (self.length)) - (self.r_factor * self.v_ang) # Angular acceleration
|
||||
|
||||
# v_ang = a_ang * (dt/1000) + self.v_ang # Integrate acceleration to get velocity
|
||||
# s_ang = v_ang * (dt/1000) # Angular displacement
|
||||
|
||||
# self.theta += s_ang # Update value
|
||||
|
||||
# self.vector = Vector2.from_polar(((self.length * 150), math.degrees(self.theta + math.pi/2)))
|
||||
|
||||
# self.a_ang = a_ang # Update value
|
||||
# self.v_ang = v_ang # Update value
|
||||
|
||||
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_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
|
||||
)
|
||||
ang_term4 = C_GRAVITY * math.sin(self.theta[self.index])
|
||||
|
||||
# Angular acceleration
|
||||
self.a_ang.append(
|
||||
(ang_term1 - ang_term2 + ang_term3 - ang_term4) / -(self.length)
|
||||
)
|
||||
|
||||
# Integrate acceleration to get velocity
|
||||
self.v_ang.append(
|
||||
self.v_ang[self.index] # Previous velocity
|
||||
+ (self.a_ang[self.index + 1] * (dt / 1000))
|
||||
)
|
||||
|
||||
# Angular displacement
|
||||
self.theta.append(
|
||||
self.theta[self.index] # Previous angle
|
||||
+ (self.v_ang[self.index + 1] * (dt / 1000))
|
||||
)
|
||||
|
||||
# Limit fall of pendulum
|
||||
self.theta[self.index + 1] = self.clamp(
|
||||
self.theta[self.index + 1], -C_FALL_ANG, C_FALL_ANG
|
||||
)
|
||||
|
||||
### CART ###
|
||||
cart_term1 = (
|
||||
self.mass # Mass
|
||||
* self.length # Length
|
||||
* self.a_ang[self.index + 1] # Current angle acceleration
|
||||
* math.cos(self.theta[self.index + 1]) # Current angle
|
||||
)
|
||||
cart_term2 = (
|
||||
self.mass # Mass
|
||||
* self.length # Length
|
||||
* self.v_ang[self.index + 1] # Current angle velocity
|
||||
* math.sin(self.theta[self.index + 1]) # Current angle
|
||||
)
|
||||
|
||||
# Cart acceleration
|
||||
self.a_cart.append((-cart_term1 + cart_term2) / (2 * self.mass))
|
||||
|
||||
# Integrate acceleration to get velocity
|
||||
self.v_cart.append(
|
||||
self.v_cart[self.index] # Previous velocity
|
||||
+ (self.a_cart[self.index + 1] * (dt / 1000))
|
||||
)
|
||||
|
||||
# Cart displacement
|
||||
self.s_cart.append(
|
||||
self.s_cart[self.index] # Previous displacement
|
||||
+ (self.v_cart[self.index + 1] * (dt / 1000))
|
||||
)
|
||||
self.dx = self.s_cart[self.index + 1] * C_MTPRATIO # Convert to pixels
|
||||
|
||||
# Update index
|
||||
self.index += 1
|
||||
|
||||
def clamp(self, n, minn, maxn):
|
||||
return max(min(maxn, n), minn)
|
||||
|
||||
def reset(self):
|
||||
self.index = 0
|
||||
|
||||
self.a_ang = [0]
|
||||
self.v_ang = [0]
|
||||
self.dx = [0]
|
||||
self.theta = [random.choice([1, -1]) * C_P_ANG_START]
|
||||
|
||||
self.a_cart = [0]
|
||||
self.v_cart = [0]
|
||||
self.s_cart = [0]
|
||||
self.fallen = False
|
||||
self.update(0)
|
||||
|
||||
def plot(self):
|
||||
fig, axs = plt.subplots(2, 2)
|
||||
fig.suptitle("Pendulum")
|
||||
|
||||
axs[0,0].plot(self.theta)
|
||||
axs[0,0].set_title('Angle [rad]')
|
||||
axs[0,1].plot(self.v_ang)
|
||||
axs[0,1].set_title('Angular velocity [rad/s]')
|
||||
axs[1,0].plot(self.a_ang)
|
||||
axs[1,0].set_title('Angular acceleration [rad/s^2]')
|
||||
|
||||
fig, axs = plt.subplots(2, 2)
|
||||
fig.suptitle("Cart")
|
||||
|
||||
axs[0,0].plot(self.s_cart)
|
||||
axs[0,0].set_title('Position [m]')
|
||||
axs[0,1].plot(self.v_cart)
|
||||
axs[0,1].set_title('Speed [m/s]')
|
||||
axs[1,0].plot(self.a_cart)
|
||||
axs[1,0].set_title('Acceleration [m/s^2]')
|
||||
|
||||
plt.show()
|
||||
|
||||
def pidControl(self, dt):
|
||||
error = self.theta[self.index]
|
||||
# (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) #PD
|
||||
self.a_cart[self.index] = result * 10
|
||||
\end{lstlisting}
|
||||
\subsection{uiHelpers.py}
|
||||
\begin{lstlisting}[language=Python]
|
||||
import pygame
|
||||
|
||||
# Constants
|
||||
C_GRID_L_VALUE = 200
|
||||
C_GRID_D_VALUE = 100
|
||||
C_MPLOT_START = 700
|
||||
C_BLINK_TIME = 500
|
||||
|
||||
gridLight = pygame.Color(C_GRID_L_VALUE, C_GRID_L_VALUE, C_GRID_L_VALUE)
|
||||
gridDark = pygame.Color(C_GRID_D_VALUE, C_GRID_D_VALUE, C_GRID_D_VALUE)
|
||||
font_h = pygame.font.SysFont(None, 28)
|
||||
font_m = pygame.font.SysFont(None, 16)
|
||||
|
||||
# UI Class
|
||||
class SimUI:
|
||||
def __init__(self, screen, pole):
|
||||
self.screen = screen
|
||||
self.pole = pole
|
||||
|
||||
self.metaPlotY = C_MPLOT_START
|
||||
|
||||
self.blink = False
|
||||
self.blinkTimer = 0
|
||||
|
||||
self.controlled = True
|
||||
self.paused = False
|
||||
|
||||
def meta(self, val, desc):
|
||||
# Capture values to display status
|
||||
if desc == "Control":
|
||||
self.controlled = val
|
||||
if desc == "Paused":
|
||||
self.paused = val
|
||||
|
||||
# Print em
|
||||
self.screen.blit(
|
||||
font_m.render(f"{desc} = {val}", True, "black"), (10, self.metaPlotY)
|
||||
)
|
||||
self.metaPlotY -= 15
|
||||
|
||||
def grid(self, dist, Xoff=0, Yoff=0):
|
||||
# Clear the screen
|
||||
self.screen.fill("white")
|
||||
|
||||
# Drawing offsets so the grid aligns with the pole
|
||||
cXoff = self.pole.x % dist
|
||||
cYoff = self.pole.y % dist
|
||||
|
||||
# Draw the grid
|
||||
for i in range(0, 1280, dist):
|
||||
pygame.draw.line(
|
||||
self.screen,
|
||||
gridLight,
|
||||
(i + Xoff + cXoff, 0),
|
||||
(i + Xoff + cXoff, 720),
|
||||
1,
|
||||
)
|
||||
pygame.draw.line(
|
||||
self.screen,
|
||||
gridLight,
|
||||
(0, i + Yoff + cYoff),
|
||||
(1280, i + Yoff + cYoff),
|
||||
1,
|
||||
)
|
||||
|
||||
# Draw the center lines darker
|
||||
pygame.draw.line(
|
||||
self.screen, gridDark, (self.pole.x + Xoff, 0), (self.pole.x + Xoff, 720), 1
|
||||
)
|
||||
pygame.draw.line(
|
||||
self.screen,
|
||||
gridDark,
|
||||
(0, self.pole.y + Yoff),
|
||||
(1280, self.pole.y + Yoff),
|
||||
1,
|
||||
)
|
||||
|
||||
def centeredText(self, font, text="", colour="black", y=0):
|
||||
textObj = font.render(text, True, colour)
|
||||
text_rect = textObj.get_rect(center=(1280 / 2, 720 / 2 - y))
|
||||
self.screen.blit(textObj, text_rect)
|
||||
|
||||
def gameover(self, time):
|
||||
font_g = pygame.font.Font("res\\pricedown.otf", 128)
|
||||
self.centeredText(font_g, "wasted", "red", 150)
|
||||
self.centeredText(font_m, f"You controlled the pendulum for {time / 1000} seconds", "black", 80)
|
||||
|
||||
self.centeredText(font_m, "Press space to restart", "black", 60)
|
||||
self.centeredText(font_m, "Press G to view nerd graphs", "black", 45)
|
||||
|
||||
def update(self, dt):
|
||||
# Credits
|
||||
self.screen.blit(
|
||||
font_h.render("Pendulum simulator 4000", True, "black"), (10, 10)
|
||||
)
|
||||
self.screen.blit(
|
||||
font_m.render("Arne van Iterson, 2023", True, "black"), (1150, 700)
|
||||
)
|
||||
|
||||
# Reset meta printing
|
||||
self.metaPlotY = C_MPLOT_START
|
||||
|
||||
# Draw current status blinking
|
||||
if self.blink:
|
||||
if self.paused:
|
||||
text = "Paused"
|
||||
elif self.controlled:
|
||||
text = "Auto mode"
|
||||
else:
|
||||
text = "Manual mode"
|
||||
|
||||
textObj = font_h.render(text, True, "black")
|
||||
text_rect = textObj.get_rect(right=1270, top=10)
|
||||
self.screen.blit(textObj, text_rect)
|
||||
|
||||
if self.blinkTimer > C_BLINK_TIME:
|
||||
self.blink = not self.blink
|
||||
self.blinkTimer = 0
|
||||
self.blinkTimer += dt
|
||||
|
||||
\end{lstlisting}
|
||||
\newpage
|
||||
\section{Control source code}
|
||||
\begin{lstlisting}[language=Python]
|
||||
#!/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
|
||||
|
||||
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)
|
||||
|
||||
# Motors
|
||||
l_motor = LargeMotor(OUTPUT_B)
|
||||
r_motor = LargeMotor(OUTPUT_C)
|
||||
|
||||
# Sensors
|
||||
gyro = GyroSensor(INPUT_2)
|
||||
|
||||
# Sound
|
||||
sound = Sound()
|
||||
sound.speak('3, 2, 1')
|
||||
|
||||
def reset_gyro():
|
||||
gyro.calibrate()
|
||||
gyro.mode = GyroSensor.MODE_GYRO_G_A
|
||||
# if gyro.mode == gyro.MODE_GYRO_CAL:
|
||||
# gyro.mode = gyro.MODE_GYRO_G_A
|
||||
# gyro.mode = gyro.MODE_GYRO_CAL
|
||||
# else:
|
||||
# mode = gyro.mode
|
||||
# gyro.mode = gyro.MODE_GYRO_CAL
|
||||
# gyro.mode = mode
|
||||
|
||||
# tires are 56mm diameter
|
||||
def balance(target_angle=0, k_p=150, k_i=0, k_d=0.1):
|
||||
integrated_error = 0
|
||||
t = time.time()
|
||||
|
||||
# Calibrate gyro in current position
|
||||
reset_gyro()
|
||||
angle = gyro.angle
|
||||
|
||||
# Stop if the robot has fallen over
|
||||
while abs(angle) < 40:
|
||||
# Keep time
|
||||
dt = time.time() - t
|
||||
t = time.time()
|
||||
|
||||
angle, rate = gyro.angle_and_rate
|
||||
error = angle - target_angle
|
||||
integrated_error = integrated_error + error * dt
|
||||
|
||||
pid = k_p * error + k_i * integrated_error + k_d * rate
|
||||
|
||||
# print(t, angle, integrated_error, rate, pid)
|
||||
logging.debug("%d;%d;%d", angle, rate, pid)
|
||||
|
||||
if abs(pid) > 1050:
|
||||
print("cap")
|
||||
|
||||
speed = min(max(pid, -1050), 1050)
|
||||
l_motor.run_forever(speed_sp=speed)
|
||||
r_motor.run_forever(speed_sp=speed)
|
||||
|
||||
l_motor.stop(stop_action="hold")
|
||||
r_motor.stop(stop_action="hold")
|
||||
|
||||
balance()
|
||||
\end{lstlisting}
|
||||
|
||||
\end{document} % This is the end of the document
|
||||
|
|
|
|||
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