arne/EV5_Modcon
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 2 Wiki Activity

Assignment due

Browse Source
This commit is contained in:
Arne van Iterson 2024-02-11 19:29:49 +01:00
parent a0942f99a0
commit fa0427b564
38 changed files with 2212 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

311
.gitignore vendored
View File

@ -135,12 +135,309 @@ cython_debug/
# Ignore exercises # Ignore exercises
exercises/ exercises/
# LaTeX Garbage # Logs
*.aux logs/
*.fdb_latexmk log/
*.fls *.xlsx
*.log
*.synctex.gz
# LaTeX output # 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 *.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

250
doc/htway.html Normal file
View File

@ -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>
&nbsp;
<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
&nbsp;
<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>
&nbsp;
<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>
&nbsp;
<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>
&nbsp;
<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>
&nbsp;
<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>
&nbsp;
<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>
&nbsp;
<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>
&nbsp;
<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;">&#40;</span><span style="color: black;">&#41;</span>:
devices <span style="color: #66cc66;">=</span> <span style="color: #008000;">list</span><span style="color: black;">&#40;</span>pyudev.<span style="color: black;">Context</span><span style="color: black;">&#40;</span><span style="color: black;">&#41;</span>.<span style="color: black;">list_devices</span><span style="color: black;">&#40;</span>subsystem<span style="color: #66cc66;">=</span><span style="color: #483d8b;">'lego-sensor'</span><span style="color: black;">&#41;</span> \
.<span style="color: black;">match_property</span><span style="color: black;">&#40;</span><span style="color: #483d8b;">&quot;LEGO_DRIVER_NAME&quot;</span><span style="color: #66cc66;">,</span> EV3Gyro.<span style="color: black;">DRIVER_NAME</span><span style="color: black;">&#41;</span> \
.<span style="color: black;">match_property</span><span style="color: black;">&#40;</span><span style="color: #483d8b;">&quot;LEGO_DRIVER_NAME&quot;</span><span style="color: #66cc66;">,</span> HTGyro.<span style="color: black;">DRIVER_NAME</span><span style="color: black;">&#41;</span><span style="color: black;">&#41;</span>
&nbsp;
<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;">&#40;</span><span style="color: #483d8b;">'Gyro not found'</span><span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">if</span> devices<span style="color: black;">&#91;</span><span style="color: #ff4500;">0</span><span style="color: black;">&#93;</span>.<span style="color: black;">attributes</span>.<span style="color: black;">get</span><span style="color: black;">&#40;</span><span style="color: #483d8b;">'driver_name'</span><span style="color: black;">&#41;</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;">&#40;</span>devices<span style="color: black;">&#91;</span><span style="color: #ff4500;">0</span><span style="color: black;">&#93;</span><span style="color: black;">&#41;</span>
<span style="color: #ff7700;font-weight:bold;">elif</span> devices<span style="color: black;">&#91;</span><span style="color: #ff4500;">0</span><span style="color: black;">&#93;</span>.<span style="color: black;">attributes</span>.<span style="color: black;">get</span><span style="color: black;">&#40;</span><span style="color: #483d8b;">'driver_name'</span><span style="color: black;">&#41;</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;">&#40;</span>devices<span style="color: black;">&#91;</span><span style="color: #ff4500;">0</span><span style="color: black;">&#93;</span><span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">def</span> <span style="color: #0000cd;">__init__</span><span style="color: black;">&#40;</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> device<span style="color: black;">&#41;</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;">&#40;</span><span style="color: #dc143c;">os</span>.<span style="color: black;">path</span>.<span style="color: black;">join</span><span style="color: black;">&#40;</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;">&#41;</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;">&#41;</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>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">def</span> calibrate<span style="color: black;">&#40;</span><span style="color: #008000;">self</span><span style="color: black;">&#41;</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;">&#40;</span><span style="color: #ff4500;">10</span><span style="color: black;">&#41;</span>:
total +<span style="color: #66cc66;">=</span> <span style="color: #008000;">self</span>.<span style="color: black;">get_rate</span><span style="color: black;">&#40;</span><span style="color: black;">&#41;</span>
<span style="color: #dc143c;">time</span>.<span style="color: black;">sleep</span><span style="color: black;">&#40;</span><span style="color: #ff4500;">0.01</span><span style="color: black;">&#41;</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>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">def</span> set_mode<span style="color: black;">&#40;</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> value<span style="color: black;">&#41;</span>:
<span style="color: #ff7700;font-weight:bold;">with</span> <span style="color: #008000;">open</span><span style="color: black;">&#40;</span><span style="color: #dc143c;">os</span>.<span style="color: black;">path</span>.<span style="color: black;">join</span><span style="color: black;">&#40;</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;">&#41;</span><span style="color: #66cc66;">,</span> <span style="color: #483d8b;">'w'</span><span style="color: black;">&#41;</span> <span style="color: #ff7700;font-weight:bold;">as</span> f:
f.<span style="color: black;">write</span><span style="color: black;">&#40;</span><span style="color: #008000;">str</span><span style="color: black;">&#40;</span>value<span style="color: black;">&#41;</span><span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">def</span> get_rate<span style="color: black;">&#40;</span><span style="color: #008000;">self</span><span style="color: black;">&#41;</span>:
<span style="color: #008000;">self</span>.<span style="color: black;">value0</span>.<span style="color: black;">seek</span><span style="color: black;">&#40;</span><span style="color: #ff4500;">0</span><span style="color: black;">&#41;</span>
<span style="color: #ff7700;font-weight:bold;">return</span> <span style="color: #008000;">int</span><span style="color: black;">&#40;</span><span style="color: #008000;">self</span>.<span style="color: black;">value0</span>.<span style="color: black;">read</span><span style="color: black;">&#40;</span><span style="color: black;">&#41;</span><span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">class</span> EV3Gyro<span style="color: black;">&#40;</span>Gyro<span style="color: black;">&#41;</span>:
DRIVER_NAME <span style="color: #66cc66;">=</span> <span style="color: #483d8b;">'lego-ev3-gyro'</span>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">def</span> <span style="color: #0000cd;">__init__</span><span style="color: black;">&#40;</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> device<span style="color: black;">&#41;</span>:
Gyro.<span style="color: #0000cd;">__init__</span><span style="color: black;">&#40;</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> device<span style="color: black;">&#41;</span>
<span style="color: #008000;">self</span>.<span style="color: black;">set_mode</span><span style="color: black;">&#40;</span><span style="color: #483d8b;">&quot;GYRO-RATE&quot;</span><span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">def</span> get_data<span style="color: black;">&#40;</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> interval<span style="color: black;">&#41;</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;">&#40;</span><span style="color: black;">&#41;</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;">&#40;</span><span style="color: #ff4500;">1</span> - EMAOFFSET<span style="color: black;">&#41;</span> * <span style="color: #008000;">self</span>.<span style="color: black;">offset</span>
speed <span style="color: #66cc66;">=</span> <span style="color: black;">&#40;</span>gyro_raw - <span style="color: #008000;">self</span>.<span style="color: black;">offset</span><span style="color: black;">&#41;</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>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">class</span> HTGyro<span style="color: black;">&#40;</span>Gyro<span style="color: black;">&#41;</span>:
DRIVER_NAME <span style="color: #66cc66;">=</span> <span style="color: #483d8b;">'ht-nxt-gyro'</span>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">def</span> <span style="color: #0000cd;">__init__</span><span style="color: black;">&#40;</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> device<span style="color: black;">&#41;</span>:
Gyro.<span style="color: #0000cd;">__init__</span><span style="color: black;">&#40;</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> device<span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">def</span> get_data<span style="color: black;">&#40;</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> interval<span style="color: black;">&#41;</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;">&#40;</span><span style="color: black;">&#41;</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;">&#40;</span><span style="color: #ff4500;">1</span> - EMAOFFSET<span style="color: black;">&#41;</span> * <span style="color: #008000;">self</span>.<span style="color: black;">offset</span>
speed <span style="color: #66cc66;">=</span> <span style="color: black;">&#40;</span>gyro_raw - <span style="color: #008000;">self</span>.<span style="color: black;">offset</span><span style="color: black;">&#41;</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>
&nbsp;
<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;">&#40;</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;">&#41;</span>:
devices <span style="color: #66cc66;">=</span> <span style="color: #008000;">list</span><span style="color: black;">&#40;</span>pyudev.<span style="color: black;">Context</span><span style="color: black;">&#40;</span><span style="color: black;">&#41;</span>.<span style="color: black;">list_devices</span><span style="color: black;">&#40;</span>subsystem<span style="color: #66cc66;">=</span><span style="color: #483d8b;">'tacho-motor'</span><span style="color: black;">&#41;</span>.<span style="color: black;">match_attribute</span><span style="color: black;">&#40;</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;">&#41;</span><span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">if</span> which <span style="color: #66cc66;">&gt;=</span> <span style="color: #008000;">len</span><span style="color: black;">&#40;</span>devices<span style="color: black;">&#41;</span>:
<span style="color: #ff7700;font-weight:bold;">raise</span> <span style="color: #008000;">Exception</span><span style="color: black;">&#40;</span><span style="color: #483d8b;">&quot;Motor not found&quot;</span><span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #008000;">self</span>.<span style="color: black;">device</span> <span style="color: #66cc66;">=</span> devices<span style="color: black;">&#91;</span>which<span style="color: black;">&#93;</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;">&#40;</span><span style="color: #dc143c;">os</span>.<span style="color: black;">path</span>.<span style="color: black;">join</span><span style="color: black;">&#40;</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;">&#41;</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;">&#41;</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;">&#40;</span><span style="color: #dc143c;">os</span>.<span style="color: black;">path</span>.<span style="color: black;">join</span><span style="color: black;">&#40;</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;">&#41;</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;">&#41;</span>
<span style="color: #008000;">self</span>.<span style="color: black;">reset</span><span style="color: black;">&#40;</span><span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">def</span> reset<span style="color: black;">&#40;</span><span style="color: #008000;">self</span><span style="color: black;">&#41;</span>:
<span style="color: #008000;">self</span>.<span style="color: black;">set_pos</span><span style="color: black;">&#40;</span><span style="color: #ff4500;">0</span><span style="color: black;">&#41;</span>
<span style="color: #008000;">self</span>.<span style="color: black;">set_duty_cycle_sp</span><span style="color: black;">&#40;</span><span style="color: #ff4500;">0</span><span style="color: black;">&#41;</span>
<span style="color: #008000;">self</span>.<span style="color: black;">send_command</span><span style="color: black;">&#40;</span><span style="color: #483d8b;">&quot;run-direct&quot;</span><span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">def</span> get_pos<span style="color: black;">&#40;</span><span style="color: #008000;">self</span><span style="color: black;">&#41;</span>:
<span style="color: #008000;">self</span>.<span style="color: black;">pos</span>.<span style="color: black;">seek</span><span style="color: black;">&#40;</span><span style="color: #ff4500;">0</span><span style="color: black;">&#41;</span>
<span style="color: #ff7700;font-weight:bold;">return</span> <span style="color: #008000;">int</span><span style="color: black;">&#40;</span><span style="color: #008000;">self</span>.<span style="color: black;">pos</span>.<span style="color: black;">read</span><span style="color: black;">&#40;</span><span style="color: black;">&#41;</span><span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">def</span> set_pos<span style="color: black;">&#40;</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> new_pos<span style="color: black;">&#41;</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;">&#40;</span><span style="color: #008000;">str</span><span style="color: black;">&#40;</span><span style="color: #008000;">int</span><span style="color: black;">&#40;</span>new_pos<span style="color: black;">&#41;</span><span style="color: black;">&#41;</span><span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">def</span> set_duty_cycle_sp<span style="color: black;">&#40;</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> new_duty_cycle_sp<span style="color: black;">&#41;</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;">&#40;</span><span style="color: #008000;">str</span><span style="color: black;">&#40;</span><span style="color: #008000;">int</span><span style="color: black;">&#40;</span>new_duty_cycle_sp<span style="color: black;">&#41;</span><span style="color: black;">&#41;</span><span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">def</span> send_command<span style="color: black;">&#40;</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> new_mode<span style="color: black;">&#41;</span>:
<span style="color: #ff7700;font-weight:bold;">with</span> <span style="color: #008000;">open</span><span style="color: black;">&#40;</span><span style="color: #dc143c;">os</span>.<span style="color: black;">path</span>.<span style="color: black;">join</span><span style="color: black;">&#40;</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;">&#41;</span><span style="color: #66cc66;">,</span><span style="color: #483d8b;">'w'</span><span style="color: black;">&#41;</span> <span style="color: #ff7700;font-weight:bold;">as</span> command:
command.<span style="color: black;">write</span><span style="color: black;">&#40;</span><span style="color: #008000;">str</span><span style="color: black;">&#40;</span>new_mode<span style="color: black;">&#41;</span><span style="color: black;">&#41;</span>
&nbsp;
<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;">&#40;</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;">&#41;</span>:
<span style="color: #008000;">self</span>.<span style="color: black;">left</span> <span style="color: #66cc66;">=</span> EV3Motor<span style="color: black;">&#40;</span>left<span style="color: black;">&#41;</span>
<span style="color: #008000;">self</span>.<span style="color: black;">right</span> <span style="color: #66cc66;">=</span> EV3Motor<span style="color: black;">&#40;</span>right<span style="color: black;">&#41;</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;">&#91;</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;">&#93;</span>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">def</span> get_data<span style="color: black;">&#40;</span><span style="color: #008000;">self</span><span style="color: #66cc66;">,</span> interval<span style="color: black;">&#41;</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;">&#40;</span><span style="color: black;">&#41;</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;">&#40;</span><span style="color: black;">&#41;</span>
&nbsp;
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
&nbsp;
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
&nbsp;
<span style="color: #008000;">self</span>.<span style="color: black;">speed</span> <span style="color: #66cc66;">=</span> <span style="color: black;">&#40;</span>delta + <span style="color: #008000;">sum</span><span style="color: black;">&#40;</span><span style="color: #008000;">self</span>.<span style="color: black;">prev_deltas</span><span style="color: black;">&#41;</span><span style="color: black;">&#41;</span> / <span style="color: black;">&#40;</span><span style="color: #ff4500;">4</span> * interval<span style="color: black;">&#41;</span>
&nbsp;
<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;">&#91;</span>delta<span style="color: black;">&#93;</span> + <span style="color: #008000;">self</span>.<span style="color: black;">prev_deltas</span><span style="color: black;">&#91;</span><span style="color: #ff4500;">0</span>:<span style="color: #ff4500;">2</span><span style="color: black;">&#93;</span>
&nbsp;
<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>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">def</span> steer_control<span style="color: black;">&#40;</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;">&#41;</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;">&#40;</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;">&#41;</span>
power_left <span style="color: #66cc66;">=</span> <span style="color: #008000;">max</span><span style="color: black;">&#40;</span>-<span style="color: #ff4500;">100</span><span style="color: #66cc66;">,</span> <span style="color: #008000;">min</span><span style="color: black;">&#40;</span>power + power_steer<span style="color: #66cc66;">,</span> <span style="color: #ff4500;">100</span><span style="color: black;">&#41;</span><span style="color: black;">&#41;</span>
power_right <span style="color: #66cc66;">=</span> <span style="color: #008000;">max</span><span style="color: black;">&#40;</span>-<span style="color: #ff4500;">100</span><span style="color: #66cc66;">,</span> <span style="color: #008000;">min</span><span style="color: black;">&#40;</span>power - power_steer<span style="color: #66cc66;">,</span> <span style="color: #ff4500;">100</span><span style="color: black;">&#41;</span><span style="color: black;">&#41;</span>
<span style="color: #ff7700;font-weight:bold;">return</span> power_left<span style="color: #66cc66;">,</span> power_right
&nbsp;
&nbsp;
<span style="color: #ff7700;font-weight:bold;">def</span> main<span style="color: black;">&#40;</span><span style="color: black;">&#41;</span>:
wheel_ratio <span style="color: #66cc66;">=</span> WHEEL_RATIO_NXT
&nbsp;
gyro <span style="color: #66cc66;">=</span> Gyro.<span style="color: black;">get_gyro</span><span style="color: black;">&#40;</span><span style="color: black;">&#41;</span>
gyro.<span style="color: black;">calibrate</span><span style="color: black;">&#40;</span><span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">print</span><span style="color: black;">&#40;</span><span style="color: #483d8b;">&quot;balancing in ...&quot;</span><span style="color: black;">&#41;</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;">&#40;</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;">&#41;</span>:
<span style="color: #ff7700;font-weight:bold;">print</span><span style="color: black;">&#40;</span>i<span style="color: black;">&#41;</span>
<span style="color: #dc143c;">os</span>.<span style="color: black;">system</span><span style="color: black;">&#40;</span><span style="color: #483d8b;">&quot;beep -f 440 -l 100&quot;</span><span style="color: black;">&#41;</span>
<span style="color: #dc143c;">time</span>.<span style="color: black;">sleep</span><span style="color: black;">&#40;</span><span style="color: #ff4500;">1</span><span style="color: black;">&#41;</span>
<span style="color: #ff7700;font-weight:bold;">print</span><span style="color: black;">&#40;</span><span style="color: #ff4500;">0</span><span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #dc143c;">os</span>.<span style="color: black;">system</span><span style="color: black;">&#40;</span><span style="color: #483d8b;">&quot;beep -f 440 -l 1000&quot;</span><span style="color: black;">&#41;</span>
<span style="color: #dc143c;">time</span>.<span style="color: black;">sleep</span><span style="color: black;">&#40;</span><span style="color: #ff4500;">1</span><span style="color: black;">&#41;</span>
&nbsp;
motors <span style="color: #66cc66;">=</span> EV3Motors<span style="color: black;">&#40;</span><span style="color: black;">&#41;</span>
start_time <span style="color: #66cc66;">=</span> <span style="color: #dc143c;">time</span>.<span style="color: #dc143c;">time</span><span style="color: black;">&#40;</span><span style="color: black;">&#41;</span>
last_okay_time <span style="color: #66cc66;">=</span> start_time
&nbsp;
avg_interval <span style="color: #66cc66;">=</span> <span style="color: #ff4500;">0.0055</span>
&nbsp;
<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;">&#40;</span><span style="color: black;">&#41;</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;">&#40;</span>avg_interval<span style="color: black;">&#41;</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;">&#40;</span>avg_interval<span style="color: black;">&#41;</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
&nbsp;
power <span style="color: #66cc66;">=</span> <span style="color: black;">&#40;</span>KGYROSPEED * gyro_speed
+ KGYROANGLE * gyro_angle<span style="color: black;">&#41;</span> \
/ wheel_ratio \
+ KPOS * motors_pos \
+ KDRIVE * motors.<span style="color: black;">drive</span> \
+ KSPEED * motors_speed
&nbsp;
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;">&#40;</span>power<span style="color: #66cc66;">,</span> <span style="color: #ff4500;">0</span><span style="color: #66cc66;">,</span> avg_interval<span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #808080; font-style: italic;">#print left_power, right_power</span>
&nbsp;
motors.<span style="color: black;">left</span>.<span style="color: black;">set_duty_cycle_sp</span><span style="color: black;">&#40;</span>left_power<span style="color: black;">&#41;</span>
motors.<span style="color: black;">right</span>.<span style="color: black;">set_duty_cycle_sp</span><span style="color: black;">&#40;</span>right_power<span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #dc143c;">time</span>.<span style="color: black;">sleep</span><span style="color: black;">&#40;</span>WAIT_TIME<span style="color: black;">&#41;</span>
&nbsp;
now_time <span style="color: #66cc66;">=</span> <span style="color: #dc143c;">time</span>.<span style="color: #dc143c;">time</span><span style="color: black;">&#40;</span><span style="color: black;">&#41;</span>
avg_interval <span style="color: #66cc66;">=</span> <span style="color: black;">&#40;</span>now_time - start_time<span style="color: black;">&#41;</span> / <span style="color: black;">&#40;</span>loop_count+<span style="color: #ff4500;">1</span><span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">if</span> <span style="color: #008000;">abs</span><span style="color: black;">&#40;</span>power<span style="color: black;">&#41;</span> <span style="color: #66cc66;">&lt;</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;">&gt;</span> TIME_FALL_LIMIT:
<span style="color: #ff7700;font-weight:bold;">break</span>
&nbsp;
motors.<span style="color: black;">left</span>.<span style="color: black;">send_command</span><span style="color: black;">&#40;</span><span style="color: #483d8b;">'reset'</span><span style="color: black;">&#41;</span>
motors.<span style="color: black;">right</span>.<span style="color: black;">send_command</span><span style="color: black;">&#40;</span><span style="color: #483d8b;">'reset'</span><span style="color: black;">&#41;</span>
&nbsp;
<span style="color: #ff7700;font-weight:bold;">if</span> __name__ <span style="color: #66cc66;">==</span> <span style="color: #483d8b;">&quot;__main__&quot;</span>:
main<span style="color: black;">&#40;</span><span style="color: black;">&#41;</span></pre>
</body>
</html>

BIN
doc/img/ang_run_1.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 18 KiB

BIN
doc/img/ang_sim_1.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 30 KiB

BIN
doc/img/cart_2.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 80 KiB

BIN
doc/img/pendulum_2.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 80 KiB

BIN
doc/img/run1.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 91 KiB

BIN
doc/img/run2.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 71 KiB

BIN
doc/img/run3.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 73 KiB

BIN
doc/img/sim_run_alllimits.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 21 KiB

BIN
doc/img/sim_run_alllimits2.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 20 KiB

BIN
doc/img/sim_run_pidcontroldelay.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 22 KiB

BIN
doc/img/sim_run_resolutionlimit1.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 41 KiB

BIN
doc/img/sim_run_speedlimit.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 20 KiB

BIN
doc/img/sim_run_speedlimit_ms2.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 16 KiB

32
doc/pendulum.bib Normal file
View File

@ -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
View File

@ -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 \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[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{multicol}
\usepackage{textcomp} \usepackage{textcomp}
\usepackage{graphicx} \usepackage{graphicx}
\usepackage{float} \usepackage{float}
\usepackage{lipsum}
\usepackage{hyperref}
\usepackage{listings}
\title{ \title{
Single inverted pendulum\\~\ Single inverted pendulum\linebreak
\large{Modeling and Control course} \large{EV5 - Modelling and Control}
} }
\author{Arne van Iterson} % Sets authors name \author{Arne van Iterson} % Sets authors name
@ -23,47 +27,886 @@
\begin{abstract} \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} \end{abstract}
\noindent\makebox[\linewidth]{\rule{\linewidth}{0.4pt}}
\begin{multicols}{2} % creates two columns \begin{multicols}{2} % creates two columns
% Theory % Theory
\section{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.
\begin{figure}[H] 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.
\includegraphics[width=\linewidth]{../res/segway.jpg}
\caption{Segway\textsuperscript{\tiny\textregistered} 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. \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}
\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 % Hardware setup
\section{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] \begin{figure}[H]
\includegraphics[width=\linewidth]{../res/bricklink/modcon_balancer2.png} \includegraphics[width=\linewidth]{../res/bricklink/modcon_balancer2.png}
\caption{Balancing robot} \caption{Balancing robot}
\label{fig:balancer} \label{fig:balancer}
\end{figure} \end{figure}
\section{Model} \section{Control}
\subsection{Linearisation} \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} 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.
dsaf
\section{Validation} 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.
fsafdfa
\section{Conclusion} 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.
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} \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 \end{document} % This is the end of the document

68
doc/plot.ipynb Normal file
View File

File diff suppressed because one or more lines are too long

BIN
res/bricklink/modcon_balancer.io
View File

Binary file not shown.

100
res/drawio/system_diagram.drawio Normal file
View File

@ -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="&lt;font color=&quot;#ff7f2a&quot;&gt;$$M$$&lt;/font&gt;" 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="&lt;font color=&quot;#ff7f2a&quot;&gt;$$m$$&lt;/font&gt;" 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="&lt;font color=&quot;#ff7f2a&quot;&gt;$$\alpha$$&lt;/font&gt;" 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="&lt;font color=&quot;#ff7f2a&quot;&gt;$$\alpha$$&lt;/font&gt;" 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="&lt;font color=&quot;#ff7f2a&quot;&gt;$$m$$&lt;/font&gt;" 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>

BIN
res/drawio/system_diagram.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 12 KiB

BIN
res/screenshot2.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 32 KiB

26
src/ev3.py
View File

@ -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
src/ev3/CMakeLists.txt → src/ev3_c/CMakeLists.txt
View File

0
src/ev3/cc.ps1 → src/ev3_c/cc.ps1
View File

0
src/ev3/cli.ps1 → src/ev3_c/cli.ps1
View File

0
src/ev3/deploy.ps1 → src/ev3_c/deploy.ps1
View File

0
doc/ethics.tex → src/ev3_c/src/delay.c
View File

0
src/ev3/src/main.c → src/ev3_c/src/main.c
View File

90
src/ev3_py/ev3.py Normal file
View File

@ -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()

44
src/ev3_py/getGyro.py Normal file
View File

@ -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))

4
src/ev3_py/helpers.py Normal file
View File

@ -0,0 +1,4 @@
def exit():
print('Adios')
tank_drive.stop()
sys.exit(130)

36
src/htway.py → src/ev3_py/htway.py
View File

@ -3,6 +3,8 @@
# Python port of the HiTechnic HTWay for ev3dev # Python port of the HiTechnic HTWay for ev3dev
# Copyright (c) 2014-2015 G33kDude, David Lechner # Copyright (c) 2014-2015 G33kDude, David Lechner
# HiTechnic HTWay is Copyright (c) 2010 HiTechnic # HiTechnic HTWay is Copyright (c) 2010 HiTechnic
import itertools import itertools
import os import os
import struct import struct
@ -22,14 +24,14 @@ WHEEL_RATIO_RCX = 1.4 # 81.6mm
# the wheel size. # the wheel size.
KGYROANGLE = 7.5 KGYROANGLE = 7.5
KGYROSPEED = 1.15 KGYROSPEED = 1.15
KPOS = 0.07 KPOS = 0
KSPEED = 0.1 KSPEED = 0
# This constant aids in drive control. When the robot starts moving # This constant aids in drive control. When the robot starts moving
# because of user control, this constant helps get the robot leaning in # because of user control, this constant helps get the robot leaning in
# the right direction. Similarly, it helps bring robot to a stop when # the right direction. Similarly, it helps bring robot to a stop when
# stopping. # stopping.
KDRIVE = -0.02 KDRIVE = 0
# Power differential used for steering based on difference of target # Power differential used for steering based on difference of target
# steering and actual motor difference. # steering and actual motor difference.
@ -84,6 +86,7 @@ class Gyro:
self.value0.seek(0) self.value0.seek(0)
return int(self.value0.read()) return int(self.value0.read())
class EV3Gyro(Gyro): class EV3Gyro(Gyro):
DRIVER_NAME = 'lego-ev3-gyro' DRIVER_NAME = 'lego-ev3-gyro'
@ -111,23 +114,27 @@ class HTGyro(Gyro):
self.angle += speed * interval self.angle += speed * interval
return speed, self.angle return speed, self.angle
class EV3Motor: class EV3Motor:
def __init__(self, which=0): 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): if which >= len(devices):
raise Exception("Motor not found") raise Exception("Motor not found")
self.device = devices[which] self.device = devices[which]
self.pos = open(os.path.join(self.device.sys_path, 'position'), 'r+',0) self.pos = open(os.path.join(self.device.sys_path, 'position'), 'r+',
self.duty_cycle_sp = open(os.path.join(self.device.sys_path,'duty_cycle_sp'), 'w', 0) 0)
self.duty_cycle_sp = open(os.path.join(self.device.sys_path,
'duty_cycle_sp'), 'w', 0)
self.reset() self.reset()
def reset(self): def reset(self):
self.set_pos(0) self.set_pos(0)
self.set_duty_cycle_sp(0) self.set_duty_cycle_sp(0)
self.send_command("run-direct") self.send_command("run-direct")
def get_pos(self): def get_pos(self):
self.pos.seek(0) self.pos.seek(0)
return int(self.pos.read()) return int(self.pos.read())
@ -139,7 +146,8 @@ class EV3Motor:
return self.duty_cycle_sp.write(str(int(new_duty_cycle_sp))) return self.duty_cycle_sp.write(str(int(new_duty_cycle_sp)))
def send_command(self, new_mode): 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)) command.write(str(new_mode))
class EV3Motors: class EV3Motors:
@ -185,23 +193,18 @@ def main():
gyro = Gyro.get_gyro() gyro = Gyro.get_gyro()
gyro.calibrate() gyro.calibrate()
print "balancing in ..."
print("balancing in ...")
for i in range(5, 0, -1): for i in range(5, 0, -1):
print(i) print i
os.system("beep -f 440 -l 100") os.system("beep -f 440 -l 100")
time.sleep(1) time.sleep(1)
print(0) print 0
os.system("beep -f 440 -l 1000") os.system("beep -f 440 -l 1000")
time.sleep(1) time.sleep(1)
motors = EV3Motors() motors = EV3Motors()
start_time = time.time() start_time = time.time()
last_okay_time = start_time last_okay_time = start_time
avg_interval = 0.0055 avg_interval = 0.0055
for loop_count in itertools.count(): for loop_count in itertools.count():
gyro_speed, gyro_angle = gyro.get_data(avg_interval) gyro_speed, gyro_angle = gyro.get_data(avg_interval)
motors_speed, motors_pos = motors.get_data(avg_interval) motors_speed, motors_pos = motors.get_data(avg_interval)
@ -225,6 +228,7 @@ def main():
time.sleep(WAIT_TIME) time.sleep(WAIT_TIME)
now_time = time.time() now_time = time.time()
avg_interval = (now_time - start_time) / (loop_count+1) avg_interval = (now_time - start_time) / (loop_count+1)

244
src/ev3_py/htway_og.py Normal file
View File

@ -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()

125
src/ev3_py/test.py Normal file
View File

@ -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)

0
src/gyro.py
View File

73
src/sim/pendulum.py
View File

@ -10,6 +10,9 @@ C_MTPRATIO = 100 # Pixels per meter
C_P_ANG_START = 1 / 1000 * math.pi C_P_ANG_START = 1 / 1000 * math.pi
C_FALL_ANG = 52.5 / 100 * 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: class Pendulum:
def __init__(self, theta, length, dx, mass, color): def __init__(self, theta, length, dx, mass, color):
@ -36,6 +39,9 @@ class Pendulum:
self.a_ang = [0] # Angular acceleration self.a_ang = [0] # Angular acceleration
self.v_ang = [0] # Angular velocity self.v_ang = [0] # Angular velocity
## Gyro offset
self.theta_offset = 0
self.dx = dx # Horizontal displacement of "cart" from center self.dx = dx # Horizontal displacement of "cart" from center
self.a_cart = [0] # Acceleration of cart self.a_cart = [0] # Acceleration of cart
self.v_cart = [0] # Velocity of cart self.v_cart = [0] # Velocity of cart
@ -49,9 +55,16 @@ class Pendulum:
## PID variables ## PID variables
self.pid = True 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.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): def update(self, dt):
self.doMath(dt) self.doMath(dt)
@ -60,7 +73,12 @@ class Pendulum:
) )
if self.pid: 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: if abs(self.theta[self.index]) == C_FALL_ANG:
self.fallen = True self.fallen = True
@ -89,14 +107,14 @@ class Pendulum:
def doMath(self, dt): def doMath(self, dt):
### ANGLE ### ### ANGLE ###
ang_term1 = self.a_cart[self.index] * math.cos(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]) ang_term2 = self.v_cart[self.index] * math.sin(self.theta[self.index] + self.theta_offset)
ang_term3 = ( ang_term3 = (
self.v_cart[self.index] # Previous cart velocity self.v_cart[self.index] # Previous cart velocity
* self.v_ang[self.index] # previous angle 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 # Angular acceleration
self.a_ang.append( self.a_ang.append(
@ -110,10 +128,13 @@ class Pendulum:
) )
# Angular displacement # Angular displacement
self.theta.append( theta_res = self.theta[self.index] + (self.v_ang[self.index + 1] * (dt / 1000))
self.theta[self.index] # Previous angle theta_round = theta_res % C_ANG_RES_LIMIT
+ (self.v_ang[self.index + 1] * (dt / 1000)) 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 # Limit fall of pendulum
self.theta[self.index + 1] = self.clamp( self.theta[self.index + 1] = self.clamp(
@ -135,13 +156,20 @@ class Pendulum:
) )
# Cart acceleration # 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 # Integrate acceleration to get velocity
self.v_cart.append( v_res = self.v_cart[self.index] + (self.a_cart[self.index + 1] * (dt / 1000)) # Previous velocity
self.v_cart[self.index] # Previous velocity
+ (self.a_cart[self.index + 1] * (dt / 1000)) 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 # Cart displacement
self.s_cart.append( self.s_cart.append(
@ -167,6 +195,7 @@ class Pendulum:
self.a_cart = [0] self.a_cart = [0]
self.v_cart = [0] self.v_cart = [0]
self.s_cart = [0] self.s_cart = [0]
self.theta_offset = 0
self.fallen = False self.fallen = False
self.update(0) self.update(0)
@ -193,7 +222,13 @@ class Pendulum:
plt.show() plt.show()
def pidControl(self): def pidControl(self, dt):
error = self.theta[self.index] error = self.theta[self.index]
result = (self.kp * error) + (self.ki * sum(self.theta)) + (self.kd * self.v_ang[self.index]) dt = (dt/1000)
self.a_cart[self.index] = result * 10 # 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)

8
src/sim/sim.py
View File

@ -25,7 +25,7 @@ ui = SimUI(screen, pole)
# Pendulum setup # Pendulum setup
# Start angle in radians, length, mass, color # 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() pendulum.reset()
dx = 0 # x offset dx = 0 # x offset
dt = 1 # delta time dt = 1 # delta time
@ -100,9 +100,9 @@ while running:
ui.grid(50, 0, 15) ui.grid(50, 0, 15)
# Update PID values # Update PID values
pendulum.kp = slider_kp.getValue() # pendulum.kp = slider_kp.getValue()
pendulum.ki = slider_ki.getValue() # pendulum.ki = slider_ki.getValue()
pendulum.kd = slider_kd.getValue() # pendulum.kd = slider_kd.getValue()
# Update pendulum # Update pendulum
if not pendulum.fallen: if not pendulum.fallen: