├── .gitignore ├── MANIFEST.in ├── README.rst ├── build.sh ├── development ├── connecting │ ├── deblock_acm0.sh │ ├── deblock_port.sh │ └── deblock_usb0.sh ├── shipment │ ├── local_install.ps1 │ └── local_install.sh └── todo.txt ├── install.sh ├── qQuickLicense.txt ├── setup.cfg ├── setup.py ├── simpylc ├── __init__.py ├── __main__.py ├── accessories │ ├── QuartzMS.TTF │ ├── freeglut32.vc14.dll │ └── freeglut64.vc14.dll ├── base.py ├── chart.py ├── coder.py ├── collisions.py ├── engine.py ├── graphics.py ├── gui.py ├── quaternions.py ├── scene.py ├── scene_transformations.jpg ├── simpylc_howto.odt ├── simpylc_howto.pdf ├── simpylcprog.jpg ├── simulations │ ├── arduino_led_timer │ │ ├── led_timer.py │ │ ├── native.cpp │ │ ├── timing.py │ │ └── world.py │ ├── arduino_traffic_lights │ │ ├── native.cpp │ │ ├── timing.py │ │ ├── traffic_lights.mp4 │ │ ├── traffic_lights.py │ │ ├── visualisation.py │ │ └── world.py │ ├── blinking_light │ │ ├── blinking_light.py │ │ ├── timing.py │ │ └── world.py │ ├── boat │ │ ├── control_client │ │ │ ├── almanac.py │ │ │ ├── captain.py │ │ │ ├── constants.py │ │ │ ├── crew.py │ │ │ ├── current.waypoints │ │ │ ├── deckhand.py │ │ │ ├── deckhand_twin.py │ │ │ ├── helmsman.py │ │ │ ├── kml_to_waypoints.py │ │ │ ├── kralingse_plas.jpg │ │ │ └── kralingse_plas.kml │ │ ├── control_server.py │ │ ├── notes.txt │ │ ├── socket_wrapper.py │ │ ├── vessel.py │ │ ├── visualisation.py │ │ └── world.py │ ├── car │ │ ├── control_client │ │ │ ├── default.samples │ │ │ ├── hardcoded_client.py │ │ │ └── parameters.py │ │ ├── control_server.py │ │ ├── dimensions.py │ │ ├── lidar.track │ │ ├── physics.py │ │ ├── socket_wrapper.py │ │ ├── sonar.track │ │ ├── visualisation.py │ │ └── world.py │ ├── one_armed_robot │ │ ├── control.py │ │ ├── robot.py │ │ ├── robot_plan.jpg │ │ ├── timing.py │ │ ├── visualisation.py │ │ └── world.py │ ├── pid_controller │ │ ├── native.cpp │ │ ├── pid_controller.py │ │ ├── timing.py │ │ └── world.py │ ├── rocket │ │ ├── common.py │ │ ├── control.py │ │ ├── rocket.py │ │ ├── thruster_rotation.jpg │ │ ├── timing.py │ │ ├── transforms.py │ │ ├── visualisation.py │ │ └── world.py │ └── train │ │ ├── control.py │ │ ├── physics.py │ │ ├── timing.py │ │ └── world.py └── vectors.py └── upload.sh /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | *.ino 3 | **.__pycache__ 4 | build 5 | dist 6 | attic 7 | SimPyLC.egg-info 8 | -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | global-include *.py *.cpp *.txt *.odt *.pdf *.jpg *.dll *.track *.TTF 2 | global-exclude *.pyc *.ino 3 | -------------------------------------------------------------------------------- /README.rst: -------------------------------------------------------------------------------- 1 | === Use the `SimPyLC forum `_ to share knowledge and ask questions about SimPyLC. === 2 | 3 | .. figure:: http://www.qquick.org/simpylc/robotvisualisation.jpg 4 | :alt: Screenshot of SimPyLC 5 | 6 | **Simulate your PLC controls and controlled systems to save lots of commissioning time** 7 | 8 | PLC? 9 | ---- 10 | 11 | Real world industrial control systems DO NOT consist of a bunch of communicating sequential processes. Semaphores, threads and priority jugling are far too error prone to control anything else but a model railway. Most control systems are surprisingly simple, consisting of only one program loop that nevertheless seems to do many things in parallel and with reliable timing. Such a control system is called a PLC (Programmable Logic Controller) and all major industries rely on it. PLC's control trains, cranes, ships and your washing machine. 12 | 13 | What SimPyLC is not 14 | ------------------- 15 | 16 | SimPyLC does not attempt to mimic any particular PLC instruction set or graphical representation like ladder logic or graphcet. There are enough tools that do. Anyone with experience in the field and an IT background knows that such archaic, bulky, hard to edit representations get in the way of clear thinking. By the way, graphcet is stateful per definition, which is the absolute enemy of safety. Though its bigger brother written in C++ according to exactly the same principles has been reliably controlling container cranes, grab unloaders and production lines for more than 20 years now, SimPyLC is FUNDAMENTALLY UNSUITABLE for controlling real world systems and should never be used as a definitive validation of anything. You're only allowed to use SimPyLC under the conditions specified in the qQuickLicence that's part of the distribution. 17 | 18 | What it is 19 | ---------- 20 | 21 | SimPyLC functionally behaves like a PLC or a set of interconnected PLC's and controlled systems. It is a very powerful tool to gain insight in the behaviour of real time controls and controlled systems. It allows you to force values, to freeze time, to draw timing charts and to visualize your system. This is all done in a very simple and straightforward way. But make no mistake, simulating systems in this way has a track record of reducing months of commissioning time to mere days. SimPyLC is Form Follows Function at its best, it does what it has to do in a robust no-nonsense way. Its sourcecode is tiny and fully open to understanding. The accompanying document `SimPyLCHowTo `_ condenses decenia of practical experience in control systems in a few clear design rules that can save you lots of trouble and prevent accidents. In addition to this SimPyLC can generate C code for the Arduino processor boards. 22 | 23 | .. figure:: http://www.qquick.org/simpylc/arduinodue.jpg 24 | :alt: Picture of Arduino Due 25 | 26 | **SimPyLC is able to generate C code for Arduino processor boards, making Arduino development MUCH easier** 27 | 28 | So 29 | -- 30 | 31 | Are you looking for impressive graphics: Look elsewhere. Do you want to gain invaluable insight in real time behaviour of controls and control systems with minimal effort: Use SimPyLC, curse at its anachronistic simplicity and grow to love it more and more. 32 | 33 | What's new 34 | ---------- 35 | 36 | - Command line tool *splc* made available 37 | - Parameter attitude added to Thing.__call__ to be able to use rotation matrix rather than Euler angles 38 | - Document simpylc_howto updated and renamed to pothole case 39 | - Boolean circuits can now be switched by pressing the mousewheel 40 | - Registers can now be altered by rotating the mousewheel 41 | - Rocket example added with physically correct moment of inertia to demonstrate e.g. precession 42 | - Some parameters added and some renamed to make Thing.__call__ more consistent 43 | - Function tEva added to evaluate 3D tuples 44 | - Quaternion module added to accurately model rotational movement 45 | - Cones and Ellipsoids added 46 | - Optional moving camera added with synchroneous caching for accurate tracking 47 | - Pure Python controls added, just using the simulator to test without actual controlled hardware 48 | 49 | *REMARK: All complete Arduino examples were tested on the Arduino Due, since that's the one I own, but they should run on the One with only slight I/O modifications (PWM instead of true analog output, using a shift register if you run short of I/O pins etc.)* 50 | 51 | Bugs fixed 52 | ---------- 53 | 54 | - No known bugs currently 55 | 56 | **Bug reports and feature requests are most welcome and will be taken under serious consideration on a non-committal basis** 57 | 58 | Installation 59 | ------------ 60 | 61 | Installation for Windows, Linux and OSX is described in `SimPyLCHowTo `_. 62 | 63 | Usage 64 | ----- 65 | 66 | 1. Go to directory SimPyLC/simulations/oneArmedRobot 67 | 2. Click on world.py or run world.py from the command line 68 | 69 | GUI Operation 70 | ------------- 71 | 72 | - [LEFT CLICK] on a field or [ENTER] gets you into edit mode. 73 | - [LEFT CLICK] or [ENTER] again gets you out of edit mode and into forced mode, values coloured orange are frozen. 74 | - [RIGHT CLICK] or [ESC] gets you into released mode, values are thawed again. 75 | - [PGUP] and [PGDN] change the currently viewed control page. 76 | - [WHEEL PRESSED] on a marker field makes it 1, release makes it 0 again, both without freezing it. 77 | - [WHEEL ROTATION] changes the value of a register field, without freezing it. 78 | 79 | 80 | For a test run of oneArmedRobot 81 | ------------------------------- 82 | 83 | 1. Enter setpoints in degrees for the joint angles (e.g. torAngSet for the torso of the robot) on the movement control page. 84 | 2. After that set 'go' to 1 and watch what happens. 85 | 86 | If you want to experiment yourself, read `SimPyLCHowTo `_ 87 | 88 | .. figure:: http://www.qquick.org/simpylc/robotsimulationsource.jpg 89 | :alt: A sample SimPyLC program 90 | 91 | **Coding is text oriented, enabling simple and fast editing, but functional behaviour resembles circuit logic, with elements like markers, timers, oneshots, latches and registers** 92 | 93 | Other packages you might like 94 | ----------------------------- 95 | 96 | - Lean and mean Python to JavaScript transpiler featuring multiple inheritance https://pypi.python.org/pypi/Transcrypt 97 | - Multi-module Python source code obfuscator https://pypi.python.org/pypi/Opy 98 | - Event driven evaluation nodes https://pypi.python.org/pypi/Eden 99 | - A lightweight Python course taking beginners seriously (under construction): https://pypi.python.org/pypi/LightOn 100 | -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | py39 setup.py bdist bdist_wheel 2 | -------------------------------------------------------------------------------- /development/connecting/deblock_acm0.sh: -------------------------------------------------------------------------------- 1 | sudo chmod a+rw "/dev/ttyACM0" 2 | -------------------------------------------------------------------------------- /development/connecting/deblock_port.sh: -------------------------------------------------------------------------------- 1 | sudo chmod a+rw "/dev/ttyACM0" 2 | -------------------------------------------------------------------------------- /development/connecting/deblock_usb0.sh: -------------------------------------------------------------------------------- 1 | sudo chmod a+rw "/dev/ttyUSB0" 2 | -------------------------------------------------------------------------------- /development/shipment/local_install.ps1: -------------------------------------------------------------------------------- 1 | $destination = "D:\python37\Lib\site-packages\simpylc" 2 | Remove-Item $destination -recurse 3 | Copy-Item ..\..\simpylc $destination -recurse 4 | -------------------------------------------------------------------------------- /development/shipment/local_install.sh: -------------------------------------------------------------------------------- 1 | rm -r ~/.local/lib/python3.10/site-packages/simpylc 2 | rm -r ~/.local/lib/python3.10/site-packages/SimPyLC*.dist-info 3 | cp -r ../../simpylc ~/.local/lib/python3.10/site-packages 4 | ls -lad ~/.local/lib/python3.10/site-packages/simpylc -------------------------------------------------------------------------------- /development/todo.txt: -------------------------------------------------------------------------------- 1 | Align blinking_light and led_timer 2 | -------------------------------------------------------------------------------- /install.sh: -------------------------------------------------------------------------------- 1 | py39 -m pip install SimPyLC --ignore-installed --no-cache-dir --upgrade 2 | -------------------------------------------------------------------------------- /qQuickLicense.txt: -------------------------------------------------------------------------------- 1 | qQuickLicence 2 | ============= 3 | 4 | This license governs use of the accompanying software ("Software"), and your use of 5 | the Software constitutes acceptance of this license. 6 | 7 | You may use the Software for any commercial or noncommercial purpose, including 8 | distributing derivative works. 9 | 10 | In return, it is required that you agree: 11 | 12 | 1. Not to remove any copyright or other notices from the Software. 13 | 2. That if you distribute the Software in source code form you do so only under 14 | this license (i.e. you must include a complete copy of this license with your 15 | distribution in a plain text file named QQuickLicence.txt), 16 | and if you distribute the Software solely in object form 17 | you only do so under a license that complies with this license. 18 | 3. That the Software comes "as is", with no warranties. None whatsoever. This 19 | means no express, implied or statutory warranty, including without limitation, 20 | warranties of merchantability or fitness for a particular purpose or any 21 | warranty of title or non-infringement. Also, you must pass this disclaimer on 22 | whenever you distribute the Software or derivative works. 23 | 4. That neither Geatec Engineering nor any contributor to the Software will be liable for any 24 | of those types of damages known as indirect, special, consequential, or 25 | incidental related to the Software or this license, to the maximum extent the law 26 | permits, no matter what legal theory it's based on. Also, you must pass this 27 | limitation of liability on whenever you distribute the Software or derivative 28 | works. 29 | 5. That you will not use or cause usage of the Software in safety-critical situations 30 | under any circumstances. 31 | 6. That if you sue anyone over patents that you think may apply to the Software for a 32 | person's use of the Software, your license to the Software ends automatically. 33 | 7. That your rights under this License end automatically if you breach it in any way. 34 | 8. That all rights not expressly granted to you in this license are reserved. 35 | 36 | 37 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [wheel] 2 | universal = 1 3 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from setuptools import setup 4 | 5 | import simpylc as sp 6 | 7 | def read (*paths): 8 | with open (os.path.join (*paths), 'r') as aFile: 9 | return aFile.read() 10 | 11 | setup ( 12 | name = 'SimPyLC', 13 | version = sp.base.programVersion, 14 | description = 'SimPyLC PLC simulator, after its C++ big brother that has controlled industrial installations for more than 25 years now. ARDUINO CODE GENERATION ADDED!', 15 | long_description = ( 16 | read ('README.rst') + '\n\n' + 17 | read ('qQuickLicense.txt') 18 | ), 19 | keywords = ['PLC', 'Arduino','simulator', 'SimPyLC', 'emulator', 'GEATEC'], 20 | url = 'http://www.qquick.org/educational', 21 | license = 'qQuickLicence', 22 | author = 'Jacques de Hooge', 23 | author_email = 'jacques.de.hooge@qquick.org', 24 | packages = ['simpylc'], 25 | include_package_data = True, 26 | install_requires = [ 27 | 'numpy', 28 | 'pyopengl', 29 | 'windows-curses; platform_system == "Windows"' 30 | ], 31 | entry_points = {}, 32 | classifiers = [ 33 | 'Development Status :: 5 - Production/Stable', 34 | 'Intended Audience :: Developers', 35 | 'Natural Language :: English', 36 | 'License :: Other/Proprietary License', 37 | 'Topic :: Software Development :: Libraries :: Python Modules', 38 | 'Operating System :: Microsoft :: Windows', 39 | 'Operating System :: POSIX :: Linux', 40 | 'Operating System :: MacOS', 41 | 'Programming Language :: Python :: 3.10', 42 | ], 43 | ) 44 | -------------------------------------------------------------------------------- /simpylc/__init__.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | from .base import * 29 | from .engine import * 30 | from .graphics import * 31 | from .quaternions import * 32 | 33 | -------------------------------------------------------------------------------- /simpylc/__main__.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import argparse as ap 29 | import distutils.dir_util as du 30 | import os 31 | import webbrowser as wb 32 | 33 | simulationsSubdirName = 'simulations' 34 | accessoriesSubdirName = 'accessories' 35 | howtoFileName = 'simpylc_howto.pdf' 36 | 37 | class CommandArgs : 38 | def __init__ (self): 39 | self.argParser = ap.ArgumentParser () 40 | self.argParser.add_argument ('-a', '--acc', help = "Copy accessories to current directory", action = 'store_true') 41 | self.argParser.add_argument ('-d', '--doc', help = "Show documentation in default browser", action = 'store_true') 42 | self.argParser.add_argument ('-s', '--sim', help = "Copy example simulations to directory", action = 'store_true') 43 | self.__dict__.update (self.argParser.parse_args () .__dict__) 44 | 45 | 46 | commandArgs = CommandArgs () 47 | 48 | simulatorDir = os.path.dirname (os.path.realpath (__file__)) 49 | currentDir = os.getcwd () 50 | 51 | if commandArgs.acc: 52 | du.copy_tree (simulatorDir + '/' + accessoriesSubdirName, currentDir + '/' + accessoriesSubdirName) 53 | elif commandArgs.sim: 54 | du.copy_tree (simulatorDir + '/' + simulationsSubdirName, currentDir + '/' + simulationsSubdirName) 55 | elif commandArgs.doc: 56 | wb.open (simulatorDir + '/' + howtoFileName) 57 | else: 58 | commandArgs.argParser.print_help () 59 | 60 | -------------------------------------------------------------------------------- /simpylc/accessories/QuartzMS.TTF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/QQuick/SimPyLC/51bcb8052f06e11a2d9747bb7d0a26a59754deb7/simpylc/accessories/QuartzMS.TTF -------------------------------------------------------------------------------- /simpylc/accessories/freeglut32.vc14.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/QQuick/SimPyLC/51bcb8052f06e11a2d9747bb7d0a26a59754deb7/simpylc/accessories/freeglut32.vc14.dll -------------------------------------------------------------------------------- /simpylc/accessories/freeglut64.vc14.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/QQuick/SimPyLC/51bcb8052f06e11a2d9747bb7d0a26a59754deb7/simpylc/accessories/freeglut64.vc14.dll -------------------------------------------------------------------------------- /simpylc/base.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import os 29 | from math import * 30 | from inspect import * 31 | 32 | programName = 'SimPyLC' 33 | programVersion = '3.10.1' 34 | programNameAndVersion = '{0} {1}'.format (programName, programVersion) 35 | programDir = os.getcwd () .replace ('\\', '/') .rsplit ('/', 3) [-1] 36 | 37 | def getTitle (name): 38 | return '{0} - {1} - {2}' .format (programDir, name, programNameAndVersion) 39 | 40 | def evaluate (anObject): 41 | if hasattr (anObject, '__call__'): 42 | return anObject () 43 | else: 44 | return anObject 45 | 46 | def eva (anObject): 47 | return evaluate (anObject) 48 | 49 | def tEva (v): 50 | return tuple (evaluate (entry) for entry in v) 51 | 52 | def tNeg (v): 53 | return tuple (-entry for entry in v) 54 | 55 | def tAdd (v0, v1): 56 | return tuple (entry0 + entry1 for entry0, entry1 in zip (v0, v1)) 57 | 58 | def tSub (v0, v1): 59 | return tuple (entry0 - entry1 for entry0, entry1 in zip (v0, v1)) 60 | 61 | def tMul (v0, v1): 62 | return tuple (entry0 * entry1 for entry0, entry1 in zip (v0, v1)) 63 | 64 | def tsMul (v, x): 65 | return tuple (entry * x for entry in v) 66 | 67 | def tDiv (v, x): 68 | return tuple (entry0 / entry1 for entry0, entry1 in zip (v0, v1)) 69 | 70 | def tsDiv (v, x): 71 | return tuple (entry / x for entry in v) 72 | 73 | def tNor (v): 74 | return sqrt (sum (entry * entry for entry in v)) 75 | 76 | def tUni (v): 77 | return tsDiv (v, tNor (v)) 78 | 79 | class ColorsHex: 80 | def __init__ (self): 81 | self.panelBackgroundColor = '#000000' 82 | 83 | self.pageCaptionForegroundColor = '#bbffbb' 84 | self.pageCaptionBackgroundColor = self.panelBackgroundColor 85 | 86 | self.groupCaptionForegroundColor = '#bbffbb' 87 | self.groupCaptionBackgroundColor = self.panelBackgroundColor 88 | 89 | self.labelForegroundColor = '#aaaaaa' 90 | self.labelBackgroundColor = self.panelBackgroundColor 91 | 92 | self.entryReleasedForegroundColor = '#00ff00' 93 | self.entryReleasedBackgroundColor = '#002200' 94 | self.entryEditForegroundColor = '#bbbbff' 95 | self.entryEditBackgroundColor = '#000022' 96 | self.entryForcedForegroundColor = '#ffaa00' 97 | self.entryForcedBackgroundColor = '#331100' 98 | 99 | self.white = '#ffffff' 100 | self.silver = '#c0c0c0' 101 | self.gray = '#808080' 102 | self.black = '#000000' 103 | self.red = '#ff0000' 104 | self.maroon = '#800000' 105 | self.yellow = '#ffff00' 106 | self.olive = '#808000' 107 | self.lime = '#00ff00' 108 | self.green = '#008000' 109 | self.aqua = '#00ffff' 110 | self.teal = '#008080' 111 | self.blue = '#0000ff' 112 | self.navy = '#000080' 113 | self.fuchsia = '#ff00ff' 114 | self.purple = '#800080' 115 | 116 | colorsHex = ColorsHex () 117 | 118 | for varName in vars (colorsHex): 119 | vars () [varName + 'Hex'] = getattr (colorsHex, varName) 120 | 121 | for varName in vars (colorsHex): 122 | colorHex = getattr (colorsHex, varName) [1:] 123 | vars () [varName] = (int (colorHex [0:2], 16) / 255., int (colorHex [2:4], 16) / 255., int (colorHex [4:6], 16) / 255.) 124 | 125 | def hexFromRgb (rgb): 126 | rgb = (int (255 * rgb [0]), int (255 * rgb [1]), int (255 * rgb [2])) 127 | return '#{:02x}{:02x}{:02x}'.format (*rgb) 128 | 129 | backgroundColorFactor = 0.25 130 | 131 | def backgroundFromRgb (rgb): 132 | return (backgroundColorFactor * rgb [0], backgroundColorFactor * rgb [1], backgroundColorFactor * rgb [2]) 133 | 134 | def decapitalize (aString): 135 | return aString [0] .lower () + aString [1:] if aString else '' 136 | 137 | def getFileLineClause (frame): 138 | frameInfo = getframeinfo (frame) 139 | return f'in file {frameInfo.filename}, line {frameInfo.lineno}:' 140 | 141 | def abort (): 142 | input () 143 | exit () 144 | 145 | def abortUnderConstruction (frame): 146 | print () 147 | print ('ERROR', getFileLineClause (frame), 'This simulation is under construction') 148 | print () 149 | abort () 150 | 151 | def warnDeprecated (frame, featureOld, featureNew = None): 152 | print () 153 | print ('WARNING', getFileLineClause (frame), featureOld [0].upper () + featureOld [1:], 'will be removed in future versions of', programName, end = '') 154 | if featureNew: 155 | print (', please use', featureNew, 'instead') 156 | else: 157 | print () 158 | 159 | def warnAsyncTrack (frame): 160 | print () 161 | print ('WARNING', getFileLineClause (frame), 'Instance recycling in display function may cause \'jumpy\' camera tracking') 162 | 163 | 164 | -------------------------------------------------------------------------------- /simpylc/chart.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | from time import * 29 | from collections import deque 30 | from itertools import islice 31 | from copy import copy 32 | import builtins 33 | 34 | from OpenGL.GL import * 35 | from OpenGL.GLUT import * 36 | from OpenGL.GLU import * 37 | 38 | from .base import * 39 | 40 | class Entry: 41 | def __init__ (self, chart, index, height): 42 | self.chart = chart 43 | self.index = index 44 | self.height = height 45 | self.top = self.chart.entries [-1] .bottom if self.chart.entries else 0 46 | self.bottom = self.top + self.height 47 | 48 | def adapt (self): 49 | pass 50 | 51 | def _render (self): 52 | pass 53 | 54 | class Group (Entry): 55 | def __init__ (self, chart, index, text, height): 56 | Entry.__init__ (self, chart, index, height) 57 | self.text = text 58 | 59 | class Channel (Entry): 60 | def __init__ (self, chart, index, circuit, min, max, height): 61 | Entry.__init__ (self, chart, index, height) 62 | self.circuit = circuit 63 | 64 | self.min = float (evaluate (min)) 65 | self.max = float (evaluate (max)) 66 | self.mean = (self.min + self.max) / 2 67 | 68 | self.ceiling = self.top + 2 69 | self.floor = self.bottom - 2 70 | self.middle = (self.floor + self.ceiling) / 2 71 | 72 | self.scale = (self.floor - self.ceiling) / (self.max - self.min) 73 | self.values = deque () 74 | 75 | def adapt (self): 76 | if self.values: 77 | self.values.rotate (-1) 78 | self.values [-1] = self.circuit () 79 | 80 | def _render (self): 81 | values = copy (self.values) 82 | 83 | glColor (*backgroundFromRgb (self.circuit.color)) 84 | glBegin (GL_QUADS) 85 | glVertex (0, self.floor) 86 | glVertex (self.chart.width, self.floor) 87 | glVertex (self.chart.width, self.ceiling) 88 | glVertex (0, self.ceiling) 89 | glEnd () 90 | 91 | glColor (*self.circuit.color) 92 | glBegin (GL_LINE_STRIP) 93 | for iValue, value in enumerate (values): 94 | if value != None: 95 | value = max (min (value, self.max), self.min) 96 | glVertex (1 * iValue, self.middle - self.scale * (value - self.mean)) 97 | glEnd () 98 | 99 | glRasterPos (2, self.middle + 5) 100 | glutBitmapString (GLUT_BITMAP_HELVETICA_12, self.circuit._name.encode ('ascii')) 101 | 102 | class Chart: 103 | def __init__ (self, name = None, width = 600, height = 400): 104 | self.name = name if name else self.__class__.__name__.lower () 105 | self.width = width 106 | self.height = height 107 | self.entries = [] 108 | 109 | def _createWindow (self): 110 | glutInitWindowSize (self.width, self.height) 111 | self.window = glutCreateWindow (getTitle (self.name) .encode ('ascii')) 112 | 113 | glEnable (GL_LINE_SMOOTH) 114 | glEnable(GL_BLEND); 115 | glEnable (GL_MULTISAMPLE) 116 | 117 | glShadeModel (GL_SMOOTH) 118 | glHint (GL_LINE_SMOOTH_HINT, GL_DONT_CARE) 119 | glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) 120 | glLineWidth (1.5) 121 | 122 | glDisable (GL_LIGHTING) 123 | 124 | glutDisplayFunc (self._display) 125 | glutReshapeFunc (self._reshape) 126 | 127 | def update (self): 128 | for entry in self.entries: 129 | entry.adapt () 130 | 131 | def _render (self): 132 | glColor (1, 1, 1) 133 | 134 | for entry in self.entries: 135 | entry._render () 136 | 137 | def _display (self): 138 | glMatrixMode (GL_MODELVIEW) 139 | glLoadIdentity () 140 | glClearColor (* (panelBackgroundColor + (0,))) 141 | 142 | glClear (GL_COLOR_BUFFER_BIT) 143 | 144 | glPushMatrix () 145 | glLineWidth (1) 146 | self._render () 147 | glPopMatrix () 148 | 149 | glFlush () 150 | 151 | glutSwapBuffers () 152 | 153 | def _reshape (self, width, height): 154 | self.width = width 155 | self.height = height 156 | 157 | glViewport (0, 0, width, height) 158 | glMatrixMode (GL_PROJECTION); 159 | glLoadIdentity () 160 | glOrtho (0, self.width, self.height, 0, 0, 1) 161 | 162 | for entry in self.entries: 163 | if isinstance (entry, Channel): 164 | if self.width > len (entry.values): 165 | entry.values = deque ([None for i in range (self.width - len (entry.values))] + list (entry.values)) 166 | else: 167 | start = len (entry.values) - self.width 168 | stop = None 169 | entry.values = deque (islice (entry.values, start, stop)) 170 | 171 | def group (self, text = '', height = 10): 172 | self.entries.append (Group (self, len (self.entries), text, height)) 173 | 174 | def channel (self, circuit, color = None, minimum = 0, maximum = 1, height = 15): 175 | circuit.color = color 176 | self.entries.append (Channel (self, len (self.entries), circuit, minimum, maximum, height)) 177 | 178 | -------------------------------------------------------------------------------- /simpylc/collisions.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | ''' 29 | A beam can have a collision id. 30 | Beams with the same collision id are accumulated in a group. 31 | Each group obtains a bounding sphere to avoid some needless work. 32 | 33 | As soon as the bounding spheres of two groups overlap, 34 | the OBB SAT method is applied to all their beams. 35 | 36 | Separation axes are 37 | 3 edges for A 38 | 3 edges for B 39 | 3 x 3 cross products between those edges 40 | ''' 41 | 42 | from .vectors import * 43 | 44 | class Box: 45 | def __init__ (self): 46 | # OpenGL uses row vectors 47 | 48 | self.startPositionVec = (0, 0, 0, 1) 49 | 50 | self.startBaseVecs = msMul (( 51 | (0, 0, 1), 52 | (0, 1, 0), 53 | (1, 0, 0) 54 | ), 0.5) [:3] 55 | 56 | 57 | self.startEdgeVecs = msMul (( 58 | ( 1, 1, 1), 59 | ( 1, 1, -1), 60 | ( 1, -1, 1), 61 | (-1, 1, 1) 62 | ), 0.5) 63 | 64 | def computeCollisionFields (self): 65 | self.positionVec = mMul (self.startPositionVec, self.modelViewMatrix) 66 | 67 | rotationMatrix = self.modelViewMatrix [:3] # Leave out translation row (OpenGL uses row vectors 68 | self.baseVecs = mMul (self.startBaseVecs, rotationMatrix) 69 | self.edgeVecs = mMul (self.startEdgeVecs, rotationMatrix) 70 | 71 | def _separate (distanceVec, separAxisVec, boxPair): 72 | projectedDistance = abs (vIpr (distanceVec, separAxisVec)) # Factor |separAxisVec| cancels out below 73 | 74 | for edgeVec0 in boxPair [0] .edgeVecs: 75 | for edgeVec1 in boxPair [1] .edgeVecs: 76 | 77 | # Test for >= rather than >, since if baseVecs are parallel, separAxisVec will be 0 vec, so inner prods all 0 78 | 79 | if abs (vIpr (edgeVec0, separAxisVec)) + abs (vIpr (edgeVec1, separAxisVec)) >= projectedDistance: 80 | return False 81 | else: 82 | return True 83 | 84 | def collision (*boxPair): 85 | distanceVec = vSub (boxPair [1] .positionVec, boxPair [0] .positionVec) 86 | 87 | # Try 2 x 3 base vectors as possible separation axes 88 | 89 | for box in boxPair: 90 | for baseVec in box.baseVecs: 91 | if _separate (distanceVec, baseVec, boxPair): 92 | return False 93 | 94 | # Try 3 x 3 outer products of base vectors as possible separation axes 95 | 96 | for baseVec0 in boxPair [0] .baseVecs: 97 | for baseVec1 in boxPair [1] .baseVecs: 98 | if _separate (distanceVec, vOpr (baseVec0, baseVec1), boxPair): 99 | return False 100 | 101 | # None of the possible separation axes held up, so it's a collision 102 | return True 103 | 104 | -------------------------------------------------------------------------------- /simpylc/graphics.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | from threading import Thread 29 | from time import * 30 | import builtins 31 | 32 | from OpenGL.GL import * 33 | from OpenGL.GLUT import * 34 | from OpenGL.GLU import * 35 | 36 | from .base import * 37 | 38 | class Graphics (Thread): 39 | def __init__ (self, world): 40 | if world._scenes or world._charts: 41 | Thread.__init__ (self) 42 | self.world = world 43 | self.daemon = True 44 | self.start () 45 | 46 | def idle (self): 47 | for scene in self.world._scenes: 48 | glutSetWindow (scene.window) 49 | glutPostRedisplay () 50 | 51 | for chart in self.world._charts: 52 | glutSetWindow (chart.window) 53 | glutPostRedisplay () 54 | 55 | sleep (self.world.refresh ()) 56 | 57 | def run (self): 58 | glutInit () 59 | glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH | GLUT_MULTISAMPLE) 60 | 61 | for scene in self.world._scenes: 62 | scene._graphics= self 63 | scene._createWindow () 64 | 65 | for chart in self.world._charts: 66 | chart._graphics = self 67 | chart._createWindow () 68 | 69 | glutIdleFunc (self.idle) 70 | glutMainLoop () 71 | 72 | -------------------------------------------------------------------------------- /simpylc/gui.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import sys 29 | import traceback 30 | 31 | from time import * 32 | from tkinter import * 33 | 34 | from .base import * 35 | 36 | global en 37 | 38 | def setEngine (engine): 39 | global en 40 | en = engine 41 | 42 | class Cell: 43 | pageCaption, groupCaption, circuit, filler = range (4) 44 | 45 | def __init__ (self, moduleWindow, module, element): 46 | self.moduleWindow = moduleWindow 47 | self.module = module 48 | self.element = element 49 | 50 | if isinstance (self.element, en._PageCaption): 51 | self.kind = Cell.pageCaption 52 | self.label = Label (self.moduleWindow, text = self.element (), justify = CENTER, width = Gui.windowWidth) 53 | self.label.grid (row = 0, column = 0, columnspan = 2 * self.module._maxNrOfColumns) 54 | self.label.configure (foreground = pageCaptionForegroundColorHex, background = pageCaptionBackgroundColorHex) 55 | self.label.bind ('', lambda event: self.moduleWindow.goPage (self.moduleWindows.pageIndex - 1)) 56 | self.label.bind ('', lambda event: self.moduleWindow.goPage (self.moduleWindows.pageIndex + 1)) 57 | 58 | elif isinstance (self.element, en._GroupCaption): 59 | self.kind = Cell.groupCaption 60 | self.label = Label (self.moduleWindow, text = self.element (), justify = CENTER, width = Gui.labelWidth + Gui.entryWidth) 61 | self.label.grid (row = self.element._rowIndex + 1, column = 2 * self.element._columnIndex, columnspan = 2) 62 | self.label.configure (foreground = groupCaptionForegroundColorHex, background = groupCaptionBackgroundColorHex) 63 | 64 | elif isinstance (self.element, en._Circuit): 65 | self.kind = Cell.circuit 66 | self.label = Label (self.moduleWindow, text = self.element._name, anchor = 'e', justify = RIGHT, width = Gui.labelWidth) 67 | 68 | self.entry = Entry (self.moduleWindow, font = Gui.entryFont, width = Gui.entryWidth) 69 | 70 | self.entry.bind ('', self.force) 71 | self.entry.bind ('', self.select) # Selecting (highlighting) text at ButtonPress-1 has no effect, since it's too early 72 | self.entry.bind ('', self.forceAndSelect) # Select so it will be ready to be overwritten 73 | 74 | self.entry.bind ('', self.edit) 75 | 76 | self.entry.bind ('', self.release) 77 | self.entry.bind ('', self.release) 78 | 79 | self.entry.bind ('', self.set1) 80 | self.entry.bind ('', self.set0) 81 | 82 | if sys.platform in {'win32', 'darwin'}: 83 | self.entry.bind ('', lambda event: self.adapt (1 if event.delta > 0 else -1)) # Windows, OsX 84 | elif sys.platform in {'linux'}: 85 | self.entry.bind ('', self.adapt (1)) # Linux 86 | self.entry.bind( '', self.adapt (-1)) # Linux 87 | else: 88 | print ('Error: Unknown platform') 89 | sys.exit (1) 90 | 91 | self.label.grid (row = self.element._rowIndex + 1, column = 2 * self.element._columnIndex, ipadx= 0.5, sticky = 'NEW') 92 | self.entry.grid (row = self.element._rowIndex + 1, column = 2 * self.element._columnIndex + 1, sticky = 'NEW') 93 | 94 | self.entry.configure (foreground = entryReleasedForegroundColorHex, background = entryReleasedBackgroundColorHex) 95 | self.label.configure (foreground = hexFromRgb (self.element.color) if self.element.color else labelForegroundColorHex, background = labelBackgroundColorHex) 96 | 97 | elif isinstance (self.element, _Filler): 98 | self.kind = Cell.filler 99 | self.label0 = Label (self.moduleWindow, text = '', width = Gui.labelWidth) 100 | self.label1 = Label (self.moduleWindow, text = '', width = Gui.entryWidth) 101 | self.label0.grid (row = self.element._rowIndex + 1, column = 2 * self.element._columnIndex, sticky = 'NEW') 102 | self.label1.grid (row = self.element._rowIndex + 1, column = 2 * self.element._columnIndex + 1, sticky = 'NEW') 103 | self.label0.configure (foreground = panelBackgroundColorHex, background = panelBackgroundColorHex) 104 | self.label1.configure (foreground = panelBackgroundColorHex, background = panelBackgroundColorHex) 105 | 106 | def force (self, event): 107 | self.entry.configure (foreground = entryForcedForegroundColorHex, background = entryForcedBackgroundColorHex) 108 | if self.element._forced: 109 | self.element._write (eval (self.entry.get ())) 110 | else: 111 | self.element._force () 112 | 113 | def select (self, event): 114 | if self.element._forced: 115 | self.entry.selection_range (0, END) 116 | 117 | def forceAndSelect (self, event): 118 | self.force (event) 119 | self.select (event) 120 | 121 | def edit (self, event): 122 | if self.element._forced and event.char.isalnum (): 123 | self.entry.configure (foreground = entryEditForegroundColorHex, background = entryEditBackgroundColorHex) 124 | 125 | def release (self, event): 126 | self.element._write (eval (self.entry.get ())) 127 | self.element._release () 128 | self.entry.configure (foreground = entryReleasedForegroundColorHex, background = entryReleasedBackgroundColorHex) 129 | 130 | def set1 (self, event): 131 | if isinstance (self.element, en.Marker): # A Runner is a Marker 132 | self.element.mark (True) 133 | elif isinstance (self.element, en.Oneshot): 134 | self.element.trigger () 135 | elif isinstance (self.element, en.Latch): 136 | if self.element: 137 | self.element.unlatch () 138 | else: 139 | self.element.latch () 140 | 141 | def set0 (self, event): 142 | if isinstance (self.element, en.Marker): # A Runner is a Marker 143 | self.element.mark (False) 144 | 145 | def adapt (self, delta): 146 | if isinstance (self.element, en.Register) or isinstance (self.element, en.Timer): 147 | try: 148 | self.element._write (round (eval (self.entry.get ())) + delta) 149 | except: # Why is this needed under Linux? 150 | pass 151 | # print (traceback.format_exc ()) 152 | 153 | class _Filler: 154 | def __init__ (self, columnIndex): 155 | self._columnIndex = columnIndex 156 | self._rowIndex = 2 157 | 158 | class ModuleWindow (Toplevel): 159 | def __init__ (self, module): 160 | Toplevel.__init__ (self) 161 | self.module = module 162 | self.title (getTitle (self.module._name)) 163 | 164 | self.pageIndex = 0 165 | self.bind ('', lambda event: self.goPage (self.pageIndex - 1)) 166 | self.bind ('', lambda event: self.goPage (self.pageIndex + 1)) 167 | 168 | self.configure (background = panelBackgroundColorHex) 169 | 170 | for columnIndex in range (2 * self.module._maxNrOfColumns): 171 | self.columnconfigure (columnIndex, weight = 1) 172 | 173 | if self.module._defaultFormat: 174 | self.geometry("%dx%d%+d%+d" % (Gui.windowWidth / 4, 800, 0, 0)) 175 | else: 176 | self.geometry("%dx%d%+d%+d" % (Gui.windowWidth, self.module._maxNrOfRows * Gui.rowHeight, 0, 0)) 177 | 178 | self.pageIndex = None 179 | self.goPage (0) 180 | 181 | def goPage (self, pageIndex): 182 | for child in self.winfo_children (): 183 | child.destroy () 184 | 185 | self.pageIndex = min (max (pageIndex, 0), len (self.module._pages) - 1) 186 | 187 | self.cells = [] 188 | firstEmptyColumnIndex = 0 189 | for element in self.module._pages [self.pageIndex] ._elements: 190 | self.cells.append (Cell (self, self.module, element)) 191 | firstEmptyColumnIndex = 2 * element._columnIndex + 1 192 | 193 | for fillerColumnIndex in range (firstEmptyColumnIndex, self.module._maxNrOfColumns): 194 | self.cells.append (Cell (self, self.module, _Filler (fillerColumnIndex))) 195 | 196 | def readFromEngine (self): 197 | for cell in self.cells: 198 | if cell.kind == Cell.circuit and not cell.element._forced and cell.entry.get () != cell.element (): 199 | cell.entry.delete (0, END) 200 | cell.entry.insert (0, cell.element ()) 201 | 202 | class Gui: 203 | entryFont = ('Quartz MS', 10) 204 | 205 | windowWidth = 800 206 | labelWidth = 20 207 | entryWidth = 10 208 | 209 | rowHeight = 23 210 | 211 | def __init__ (self, world): 212 | self.world = world 213 | self.root = Tk () 214 | self.root.withdraw () 215 | self.moduleWindows = [ModuleWindow (module) for module in self.world._modules] 216 | 217 | while True: 218 | for moduleWindow in self.moduleWindows: 219 | moduleWindow.readFromEngine () 220 | 221 | self.root.update () 222 | 223 | for moduleWindow in self.moduleWindows: 224 | moduleWindow.update () 225 | 226 | sleep (0.1) 227 | 228 | -------------------------------------------------------------------------------- /simpylc/quaternions.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import numpy 29 | 30 | from math import sqrt 31 | 32 | from .engine import * 33 | 34 | # All angles are in degrees 35 | # All non-scalar variables are numpy arrays 36 | 37 | def normized (anArray): 38 | return anArray / numpy.linalg.norm (anArray) 39 | 40 | def quatFromAxAng (axis, angle): 41 | imag = axis * sin (angle / 2) 42 | return numpy.array ((cos (angle / 2), imag [0], imag [1], imag [2])) 43 | 44 | def axAngFromQuat (q): 45 | angle = 2 * acos (q [0]) 46 | denom = math.sqrt (1 - q [0] * q [0]) 47 | axis = (q [1:] / denom) if denom else numpy.array ((1, 0, 0)) 48 | return axis, angle 49 | 50 | def quatMul (q0, q1): 51 | return numpy.array (( 52 | q0 [0] * q1 [0] - q0 [1] * q1 [1] - q0 [2] * q1 [2] - q0 [3] * q1 [3], 53 | q0 [0] * q1 [1] + q0 [1] * q1 [0] + q0 [2] * q1 [3] - q0 [3] * q1 [2], 54 | q0 [0] * q1 [2] - q0 [1] * q1 [3] + q0 [2] * q1 [0] + q0 [3] * q1 [1], 55 | q0 [0] * q1 [3] + q0 [1] * q1 [2] - q0 [2] * q1 [1] + q0 [3] * q1 [0] 56 | )) 57 | 58 | def quatInv (q): 59 | return numpy.array ((q [0], -q [1], -q [2], -q [3])) 60 | 61 | def quatFromVec (v): 62 | return numpy.array ((0, v [0], v [1], v [2])) 63 | 64 | def quatVecRot (q, v): 65 | return (quatMul ( 66 | quatMul ( 67 | q, 68 | quatFromVec (v) 69 | ), 70 | quatInv (q) 71 | )) [1:] 72 | 73 | def rotMatFromQuat (q): 74 | return numpy.array (( 75 | (1 - 2 * q [2] * q [2] - 2 * q [3] * q [3], 2 * q [1] * q [2] - 2 * q [3] * q [0], 2 * q [1] * q [3] + 2 * q [2] * q [0]), 76 | (2 * q [1] * q [2] + 2 * q [3] * q [0], 1 - 2 * q [1] * q [1] - 2 * q [3] * q [3], 2 * q [2] * q [3] - 2 * q [1] * q [0]), 77 | (2 * q [1] * q [3] - 2 * q [2] * q [0], 2 * q [2] * q [3] + 2 * q [1] * q [0], 1 - 2 * q [1] * q [1] - 2 * q [2] * q [2]) 78 | )) 79 | 80 | -------------------------------------------------------------------------------- /simpylc/scene_transformations.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/QQuick/SimPyLC/51bcb8052f06e11a2d9747bb7d0a26a59754deb7/simpylc/scene_transformations.jpg -------------------------------------------------------------------------------- /simpylc/simpylc_howto.odt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/QQuick/SimPyLC/51bcb8052f06e11a2d9747bb7d0a26a59754deb7/simpylc/simpylc_howto.odt -------------------------------------------------------------------------------- /simpylc/simpylc_howto.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/QQuick/SimPyLC/51bcb8052f06e11a2d9747bb7d0a26a59754deb7/simpylc/simpylc_howto.pdf -------------------------------------------------------------------------------- /simpylc/simpylcprog.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/QQuick/SimPyLC/51bcb8052f06e11a2d9747bb7d0a26a59754deb7/simpylc/simpylcprog.jpg -------------------------------------------------------------------------------- /simpylc/simulations/arduino_led_timer/led_timer.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | class LedTimer (sp.Module): 31 | def __init__ (self): 32 | sp.Module.__init__ (self) 33 | 34 | self.rampTimer = sp.Timer () 35 | self.pulse = sp.Oneshot () 36 | self.direction = sp.Marker () 37 | self.blinkTime = sp.Register () 38 | self.blinkTimer = sp.Timer () 39 | self.led = sp.Marker () 40 | self.runner = sp.Runner () 41 | 42 | def sweep (self): 43 | self.rampTimer.reset (self.rampTimer > 9) 44 | self.pulse.trigger (self.rampTimer < 0.1) 45 | self.direction.mark (not self.direction, self.pulse) 46 | self.blinkTime.set (3 - self.rampTimer / 3, self.direction, self.rampTimer / 3) 47 | self.blinkTimer.reset (self.blinkTimer > self.blinkTime) 48 | self.led.mark (self.blinkTimer < 0.05) 49 | -------------------------------------------------------------------------------- /simpylc/simulations/arduino_led_timer/native.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2013 - 2021 GEATEC engineering 3 | 4 | This program is free software. 5 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicence. 6 | 7 | This program is distributed in the hope that it will be useful, 8 | but WITHOUT ANY WARRANTY, without even the implied warranty of 9 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 10 | See the QQuickLicence for details. 11 | 12 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 13 | 14 | __________________________________________________________________________ 15 | 16 | 17 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 18 | 19 | __________________________________________________________________________ 20 | 21 | It is meant for training purposes only. 22 | 23 | Removing this header ends your licence. 24 | */ 25 | 26 | void setup () { 27 | pinMode (13, OUTPUT); 28 | } 29 | 30 | void loop () { 31 | cycle (); 32 | digitalWrite (13, led); 33 | } 34 | 35 | -------------------------------------------------------------------------------- /simpylc/simulations/arduino_led_timer/timing.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | class Timing (sp.Chart): 31 | def __init__ (self): 32 | sp.Chart.__init__ (self) 33 | 34 | def define (self): 35 | self.channel (sp.world.ledTimer.rampTimer, sp.red, 0, 9, 180) 36 | self.channel (sp.world.ledTimer.direction, sp.white, 0, 1, 20) 37 | self.channel (sp.world.ledTimer.pulse, sp.aqua, 0, 1, 20) 38 | self.channel (sp.world.ledTimer.blinkTime, sp.blue, 0, 3, 60) 39 | self.channel (sp.world.ledTimer.blinkTimer, sp.yellow, 0, 3, 60) 40 | self.channel (sp.world.ledTimer.led, sp.green, 0, 1, 20) 41 | 42 | -------------------------------------------------------------------------------- /simpylc/simulations/arduino_led_timer/world.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import os 29 | import sys as ss 30 | 31 | ss.path.append (os.path.abspath ('../..')) # If you want to store your simulations somewhere else, put SimPyLC in your PYTHONPATH environment variable 32 | 33 | import simpylc as sp 34 | import led_timer as lt 35 | import timing as tm 36 | 37 | sp.World (lt.LedTimer, tm.Timing) 38 | -------------------------------------------------------------------------------- /simpylc/simulations/arduino_traffic_lights/native.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2013 - 2021 GEATEC engineering 3 | 4 | This program is free software. 5 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicence. 6 | 7 | This program is distributed in the hope that it will be useful, 8 | but WITHOUT ANY WARRANTY, without even the implied warranty of 9 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 10 | See the QQuickLicence for details. 11 | 12 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 13 | __________________________________________________________________________ 14 | 15 | 16 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 17 | 18 | __________________________________________________________________________ 19 | 20 | It is meant for training purposes only. 21 | 22 | Removing this header ends your licence. 23 | */ 24 | 25 | void setup () { 26 | analogWriteResolution (12); 27 | 28 | pinMode (33, OUTPUT); pinMode (35, OUTPUT); pinMode (37, OUTPUT); pinMode (39, OUTPUT); 29 | pinMode (41, OUTPUT); pinMode (43, OUTPUT); pinMode (45, OUTPUT); pinMode (47, OUTPUT); 30 | pinMode (49, INPUT); pinMode (51, INPUT); 31 | } 32 | 33 | void loop () { 34 | modeButton = !digitalRead (49); brightButton = !digitalRead (51); 35 | 36 | cycle (); 37 | 38 | digitalWrite (39, northGreenLamp); digitalWrite (41, northRedLamp); 39 | digitalWrite (35, eastGreenLamp); digitalWrite (37, eastRedLamp); 40 | digitalWrite (47, southGreenLamp); digitalWrite (33, southRedLamp); 41 | digitalWrite (43, westGreenLamp); digitalWrite (45, westRedLamp); 42 | 43 | analogWrite (DAC0, streetLamp); 44 | } 45 | -------------------------------------------------------------------------------- /simpylc/simulations/arduino_traffic_lights/timing.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | class Timing (sp.Chart): 31 | def __init__ (self): 32 | sp.Chart.__init__ (self) 33 | 34 | def define (self): 35 | self.channel (sp.world.trafficLights.northGreenLamp, sp.green) 36 | self.channel (sp.world.trafficLights.northRedLamp, sp.red) 37 | self.channel (sp.world.trafficLights.eastGreenLamp, sp.green) 38 | self.channel (sp.world.trafficLights.eastRedLamp, sp.red) 39 | self.channel (sp.world.trafficLights.southGreenLamp, sp.green) 40 | self.channel (sp.world.trafficLights.southRedLamp, sp.red) 41 | self.channel (sp.world.trafficLights.westGreenLamp, sp.green) 42 | self.channel (sp.world.trafficLights.westRedLamp, sp.red) 43 | self.channel (sp.world.trafficLights.regularPhaseTimer, sp.aqua, 0, 20, 60) 44 | self.channel (sp.world.trafficLights.cyclePhaseTimer, sp.aqua, 0, 40, 60) 45 | self.channel (sp.world.trafficLights.blinkTimer, sp.aqua, 0, 0.3, 60) 46 | self.channel (sp.world.trafficLights.modeButton, sp.white) 47 | self.channel (sp.world.trafficLights.modePulse, sp.white) 48 | self.channel (sp.world.trafficLights.modeStep, sp.white, 0, 3, 60) 49 | self.channel (sp.world.trafficLights.brightButton, sp.yellow) 50 | self.channel (sp.world.trafficLights.brightPulse, sp.yellow) 51 | self.channel (sp.world.trafficLights.brightDirection, sp.yellow) 52 | self.channel (sp.world.trafficLights.streetLamp, sp.yellow, 0, 5000, 60) 53 | 54 | -------------------------------------------------------------------------------- /simpylc/simulations/arduino_traffic_lights/traffic_lights.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/QQuick/SimPyLC/51bcb8052f06e11a2d9747bb7d0a26a59754deb7/simpylc/simulations/arduino_traffic_lights/traffic_lights.mp4 -------------------------------------------------------------------------------- /simpylc/simulations/arduino_traffic_lights/traffic_lights.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | class TrafficLights (sp.Module): 31 | def __init__ (self): 32 | sp.Module.__init__ (self) 33 | 34 | self.page ('Trafic lights') 35 | 36 | self.group ('Timers', True) 37 | self.regularPhaseTimer = sp.Timer () 38 | self.cyclePhaseTimer = sp.Timer () 39 | self.tBlink = sp.Register (0.3) 40 | self.blinkTimer = sp.Timer () 41 | self.blinkPulse = sp.Oneshot () 42 | self.blink = sp.Marker () 43 | 44 | self.group ('Mode switching') 45 | self.modeButton = sp.Marker () 46 | self.modePulse = sp.Oneshot () 47 | self.modeStep = sp.Register () 48 | self.regularMode = sp.Marker (True) 49 | self.cycleMode = sp.Marker () 50 | self.nightMode = sp.Marker () 51 | self.offMode = sp.Marker () 52 | 53 | self.group ('Night blinking') 54 | self.allowRed = sp.Marker () 55 | 56 | self.group ('Regular mode phases', True) 57 | self.northSouthGreen = sp.Marker (True) 58 | self.northSouthBlink = sp.Marker () 59 | self.eastWestGreen = sp.Marker () 60 | self.eastWestBlink = sp.Marker () 61 | 62 | self.group ('Cycle mode phases') 63 | self.northGreen = sp.Marker () 64 | self.northBlink = sp.Marker () 65 | self.eastGreen = sp.Marker () 66 | self.eastBlink = sp.Marker () 67 | self.southGreen = sp.Marker () 68 | self.southBlink = sp.Marker () 69 | self.westGreen = sp.Marker () 70 | self.westBlink = sp.Marker () 71 | 72 | self.group ('Lamps') 73 | self.northGreenLamp = sp.Marker () 74 | self.northRedLamp = sp.Marker () 75 | self.eastGreenLamp = sp.Marker () 76 | self.eastRedLamp = sp.Marker () 77 | self.southGreenLamp = sp.Marker () 78 | self.southRedLamp = sp.Marker () 79 | self.westGreenLamp = sp.Marker () 80 | self.westRedLamp = sp.Marker () 81 | 82 | self.group ('Regular phase end times', True) 83 | self.tNorthSouthGreen = sp.Register (5) 84 | self.tNorthSouthBlink = sp.Register (7) 85 | self.tEastWestGreen = sp.Register (12) 86 | self.tEastWestBlink = sp.Register (14) 87 | 88 | self.group ('Cycle phase end times') 89 | self.tNorthGreen = sp.Register (5) 90 | self.tNorthBlink = sp.Register (7) 91 | self.tEastGreen = sp.Register (12) 92 | self.tEastBlink = sp.Register (14) 93 | self.tSouthGreen = sp.Register (19) 94 | self.tSouthBlink = sp.Register (21) 95 | self.tWestGreen = sp.Register (26) 96 | self.tWestBlink = sp.Register (28) 97 | 98 | self.group ('Street illumination') 99 | self.brightButton = sp.Marker () 100 | self.brightPulse = sp.Oneshot () 101 | self.brightDirection = sp.Marker (True) 102 | self.brightMin = sp.Register (2047) 103 | self.brightMax = sp.Register (4095) 104 | self.brightFluxus = sp.Register (200) 105 | self.brightDelta = sp.Register () 106 | self.streetLamp = sp.Register (2047) 107 | 108 | self.group ('System') 109 | self.runner = sp.Runner () 110 | 111 | def sweep (self): 112 | self.part ('Timers') 113 | self.regularPhaseTimer.reset (self.regularPhaseTimer > self.tEastWestBlink or self.cycleMode or self.nightMode or self.offMode) 114 | self.cyclePhaseTimer.reset (self.cyclePhaseTimer > self.tWestBlink or self.regularMode or self.nightMode or self.offMode) 115 | self.blinkTimer.reset (self.blinkTimer > self.tBlink) 116 | self.blinkPulse.trigger (self.blinkTimer == 0) 117 | self.blink.mark (not self.blink, self.blinkPulse) 118 | 119 | self.part ('Mode switching') 120 | self.modePulse.trigger (self.modeButton) 121 | self.modeStep.set ((self.modeStep + 1) % 4, self.modePulse) 122 | self.regularMode.mark (self.modeStep == 0) 123 | self.cycleMode.mark (self.modeStep == 1) 124 | self.nightMode.mark (self.modeStep == 2) 125 | self.offMode.mark (self.modeStep == 3) 126 | 127 | self.part ('Regular mode phases') 128 | self.northSouthGreen.mark (0 < self.regularPhaseTimer < self.tNorthSouthGreen) 129 | self.northSouthBlink.mark (self.tNorthSouthGreen < self.regularPhaseTimer < self.tNorthSouthBlink) 130 | self.eastWestGreen.mark (self.tNorthSouthBlink < self.regularPhaseTimer < self.tEastWestGreen) 131 | self.eastWestBlink.mark (self.tEastWestGreen < self.regularPhaseTimer) 132 | 133 | self.part ('Cycle mode phases') 134 | self.northGreen.mark (0 < self.cyclePhaseTimer < self.tNorthGreen) 135 | self.northBlink.mark (self.tNorthGreen < self.cyclePhaseTimer < self.tNorthBlink) 136 | self.eastGreen.mark (self.tNorthBlink < self.cyclePhaseTimer < self.tEastGreen) 137 | self.eastBlink.mark (self.tEastGreen < self.cyclePhaseTimer < self.tEastBlink) 138 | self.southGreen.mark (self.tEastBlink < self.cyclePhaseTimer < self.tSouthGreen) 139 | self.southBlink.mark (self.tSouthGreen < self.cyclePhaseTimer < self.tSouthBlink) 140 | self.westGreen.mark (self.tSouthBlink < self.cyclePhaseTimer < self.tWestGreen) 141 | self.westBlink.mark (self.tWestGreen < self.cyclePhaseTimer) 142 | 143 | self.part ('Night blinking') 144 | self.allowRed.mark (self.regularMode or self.cycleMode or (self.nightMode and self.blink)) 145 | 146 | self.part ('Traffic lamps') 147 | self.northGreenLamp.mark (self.northSouthGreen or self.northGreen or ((self.northSouthBlink or self.northBlink) and self.blink)) 148 | self.northRedLamp.mark (not (self.northSouthGreen or self.northGreen or self.northSouthBlink or self.northBlink) and self.allowRed) 149 | self.eastGreenLamp.mark (self.eastWestGreen or self.eastGreen or ((self.eastWestBlink or self.eastBlink) and self.blink)) 150 | self.eastRedLamp.mark (not (self.eastWestGreen or self.eastGreen or self.eastWestBlink or self.eastBlink) and self.allowRed) 151 | self.southGreenLamp.mark (self.northSouthGreen or self.southGreen or ((self.northSouthBlink or self.southBlink) and self.blink)) 152 | self.southRedLamp.mark (not (self.northSouthGreen or self.southGreen or self.northSouthBlink or self.southBlink) and self.allowRed) 153 | self.westGreenLamp.mark (self.eastWestGreen or self.westGreen or ((self.eastWestBlink or self.westBlink) and self.blink)) 154 | self.westRedLamp.mark (not (self.eastWestGreen or self.westGreen or self.eastWestBlink or self.westBlink) and self.allowRed) 155 | 156 | self.part ('Street illumination') 157 | self.brightPulse.trigger (self.brightButton) 158 | self.brightDirection.mark (not self.brightDirection, self.brightPulse) 159 | self.brightDelta.set (-self.brightFluxus * sp.world.period, self.brightDirection, self.brightFluxus * sp.world.period) 160 | self.streetLamp.set (sp.limit (self.streetLamp + self.brightDelta, self.brightMin, self.brightMax), self.brightButton) 161 | 162 | -------------------------------------------------------------------------------- /simpylc/simulations/arduino_traffic_lights/visualisation.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | class TrafficLamp (sp.Cylinder): 31 | def __init__ (self, green = False): 32 | sp.Cylinder.__init__ (self, size = (0.1, 0.1, 0.2), center = (0, 0, 0.5) if green else (0, 0, 0.7), color = (0, 1, 0) if green else (1, 0, 0)) 33 | self.originalColor = self.color 34 | 35 | def __call__ (self, on): 36 | self.color = self.originalColor if on else sp.tsMul (self.originalColor, 0.2) 37 | return sp.Cylinder.__call__ (self) 38 | 39 | class StreetLamp (sp.Cylinder): 40 | def __init__ (self, green = False): 41 | sp.Cylinder.__init__ (self, size = (0.4, 0.4, 0.4), center = (0, 0, 2), color = (1, 1, 0.2)) 42 | self.originalColor = self.color 43 | 44 | def __call__ (self, brightness): 45 | self.color = sp.tsMul (self.originalColor, 0.2 + 0.8 * brightness) 46 | return sp.Cylinder.__call__ (self) 47 | 48 | class Visualisation (sp.Scene): 49 | def __init__ (self): 50 | sp.Scene.__init__ (self) 51 | 52 | self.crossing = sp.Beam (size = (3, 3, 0.1), pivot = (0, 1, 0), color = (0.1, 0.1, 0.1)) 53 | self.sidewalk = sp.Beam (size = (1, 1, 0.1), center = (-1, -1, 0.1), joint = (1, 1, 0), color = (0, 0.3, 0)) 54 | self.pole = sp.Cylinder (size = (0.05, 0.05, 1), center = (0, 0.45, 0.45), color = (1, 1, 1)) 55 | 56 | self.redLamp = TrafficLamp () 57 | self.greenLamp = TrafficLamp (True) 58 | self.streetLamp = StreetLamp () 59 | 60 | def display (self): 61 | control = sp.world.trafficLights 62 | 63 | self.crossing (rotation = 30, parts = lambda: 64 | self.sidewalk (rotation = 0, parts = lambda: 65 | self.pole (parts = lambda: 66 | self.redLamp (control.northRedLamp) + 67 | self.greenLamp (control.northGreenLamp) 68 | ) 69 | ) + 70 | self.sidewalk (rotation = -90, parts = lambda: 71 | self.pole (parts = lambda: 72 | self.redLamp (control.eastRedLamp) + 73 | self.greenLamp (control.eastGreenLamp) 74 | ) 75 | 76 | ) + 77 | self.sidewalk (rotation = -180, parts = lambda: 78 | self.pole (parts = lambda: 79 | self.redLamp (control.southRedLamp) + 80 | self.greenLamp (control.southGreenLamp) 81 | ) 82 | ) + 83 | self.sidewalk (rotation = -270, parts = lambda: 84 | self.pole (parts = lambda: 85 | self.redLamp (control.westRedLamp) + 86 | self.greenLamp (control.westGreenLamp) 87 | ) 88 | ) + 89 | self.streetLamp ((control.streetLamp - control.brightMin) / (control.brightMax - control.brightMin)) 90 | ) 91 | 92 | -------------------------------------------------------------------------------- /simpylc/simulations/arduino_traffic_lights/world.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import os 29 | import sys as ss 30 | 31 | ss.path.append (os.path.abspath ('../..')) # If you want to store your simulations somewhere else, put SimPyLC in your PYTHONPATH environment variable 32 | 33 | import simpylc as sp 34 | import traffic_lights as tl 35 | import timing as tm 36 | import visualisation as vs 37 | 38 | sp.World (tl.TrafficLights, tm.Timing, vs.Visualisation) 39 | -------------------------------------------------------------------------------- /simpylc/simulations/blinking_light/blinking_light.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | class BlinkingLight (sp.Module): 31 | def __init__ (self): 32 | sp.Module.__init__ (self) 33 | 34 | self.blinkTimer = sp.Timer () 35 | self.pulse = sp.Oneshot () 36 | self.counter = sp.Register () 37 | self.led = sp.Marker () 38 | self.run = sp.Runner () 39 | 40 | def sweep (self): 41 | self.blinkTimer.reset (self.blinkTimer > 8) 42 | self.pulse.trigger (self.blinkTimer > 3) 43 | self.counter.set (self.counter + 1, self.pulse) 44 | self.led.mark (not self.led, self.pulse) 45 | 46 | -------------------------------------------------------------------------------- /simpylc/simulations/blinking_light/timing.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | class Timing (sp.Chart): 31 | def __init__ (self): 32 | sp.Chart.__init__ (self) 33 | 34 | def define (self): 35 | self.channel (sp.world.blinkingLight.blinkTimer, sp.red, 0, 12, 50) 36 | self.channel (sp.world.blinkingLight.pulse, sp.blue, 0, 1, 50) 37 | self.channel (sp.world.blinkingLight.counter, sp.yellow, 0, 20, 100) 38 | self.channel (sp.world.blinkingLight.led, sp.lime, 0, 1, 50) 39 | 40 | -------------------------------------------------------------------------------- /simpylc/simulations/blinking_light/world.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import os 29 | import sys as ss 30 | 31 | ss.path.append (os.path.abspath ('../..')) # If you want to store your simulations somewhere else, put SimPyLC in your PYTHONPATH environment variable 32 | 33 | import simpylc as sp 34 | import blinking_light as bl 35 | import timing as tm 36 | 37 | sp.World (bl.BlinkingLight, tm.Timing) 38 | -------------------------------------------------------------------------------- /simpylc/simulations/boat/control_client/almanac.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Lattitude and longitude are in signed decimal degrees: -90 (south) <= lattitude <= 90 (north), -180 (west) <= longitude <= 180 (east) 3 | The x axis points to the east, the y axis points to the north. 4 | Polar expeditions are not yet) supported. 5 | ''' 6 | 7 | import os 8 | import math as mt 9 | 10 | import constants as cs 11 | 12 | degreePerRadian = 180 / mt.pi 13 | radianPerDegree = 1 / degreePerRadian 14 | 15 | waypointsFilename = f'{os.path.dirname (os.path.abspath (__file__))}/{cs.waypointsFilename}' 16 | boomLength = 0.5 17 | 18 | maxSegmentSpan = 10 # Max step for lattitude and longitude [degree] 19 | targetMargin = 5 # Margin allowed in passing a waypoint [m] 20 | earthPerimeter = 4.0075e7 # On average, high accuracy not needed [m] 21 | meterPerDegreeLattitude = earthPerimeter / 360 22 | 23 | def getMeterPerDegreeLongitude (lattitude): 24 | return earthPerimeter * mt.cos (lattitude * radianPerDegree) / 360 25 | 26 | class Exclude: 27 | def __init__ (self, absThreshold): 28 | self.absThreshold = absThreshold 29 | self.alteredValue = 0 30 | 31 | def __call__ (self, value): 32 | if abs (value) >= self.absThreshold: 33 | self.alteredValue = 0 34 | return value 35 | else: 36 | if self.alteredValue == 0: 37 | if value >= 0: 38 | self.alteredValue = self.absThreshold 39 | else: 40 | self.alteredValue = -self.absThreshold 41 | return self.alteredValue 42 | 43 | def asymmetrize (angle): 44 | return angle % 360 # 0 <= asym < 360 45 | 46 | def symmetrize (angle): 47 | return 180 - asymmetrize (180 - angle) # -180 < sym <= 180 48 | 49 | class SegmentSpanException (Exception): 50 | def __init__ (self, componentName): 51 | super () .__init__ (f'Error: Segment spans a {componentName} difference > {maxSegmentSpan} degrees') 52 | 53 | def getWaypoints (): 54 | return [ 55 | [float (item) for item in line.split ()] 56 | for line in open (waypointsFilename) .readlines () if not line.startswith ('#') 57 | ] # Waypoints are absolute (lattitude: <-180, 180], longitude: [-90, 90]) pairs in decimal degrees 58 | 59 | def getLeg (baseWaypoint, targetWaypoint): 60 | baseLattitude = baseWaypoint [0] 61 | targetLattitude = targetWaypoint [0] 62 | lattitudeDifference = targetLattitude - baseLattitude 63 | legY = meterPerDegreeLattitude * lattitudeDifference 64 | 65 | baseLongitude = baseWaypoint [1] 66 | targetLongitude = targetWaypoint [1] 67 | longitudeDifference = targetLongitude - baseLongitude 68 | 69 | ''' 70 | if abs (longitudeDifference) > maxSegmentSpan: 71 | raise SegmentSpanException ('longitude') 72 | ''' 73 | 74 | if longitudeDifference < -180: 75 | longitudeDifference += 360 76 | elif longitudeDifference > 180: 77 | longitudeDifference -= 360 78 | 79 | ''' 80 | if abs (longitudeDifference) > maxSegmentSpan: 81 | raise SegmentSpanException ('lattitude') 82 | ''' 83 | 84 | averageLattitude = (baseLattitude + targetLattitude) / 2 85 | legX = getMeterPerDegreeLongitude (averageLattitude) * longitudeDifference 86 | 87 | return legX, legY 88 | 89 | def getLegAngle (leg): 90 | return symmetrize (degreePerRadian * mt.atan2 (leg [1], leg [0]) - 90) 91 | 92 | def getLegLength (leg): 93 | return mt.sqrt (leg [0] ** 2 + leg [1] ** 2) 94 | 95 | def getPosition (previousPosition, velocity, deltaTime): 96 | previousLattitude = previousPosition [0] 97 | newLattitude = previousLattitude + velocity [1] * deltaTime / meterPerDegreeLattitude 98 | averageLattitude = (previousLattitude + newLattitude) / 2 99 | previousLongitude = previousPosition [1] 100 | newLongitude = previousLongitude + velocity [0] * deltaTime / getMeterPerDegreeLongitude (averageLattitude) 101 | return newLattitude, newLongitude 102 | 103 | def clip (value, maxAbsValue): 104 | return min (max (value, -maxAbsValue), maxAbsValue) 105 | 106 | -------------------------------------------------------------------------------- /simpylc/simulations/boat/control_client/captain.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | Removing this comment ends your license. 16 | ''' 17 | 18 | import almanac as an 19 | 20 | class Captain: 21 | def __init__ (self, crew): 22 | self.crew = crew 23 | 24 | def navigate (self): 25 | self.waypoints = an.getWaypoints () # To be replaced by dynamic waypoint adaptation, based on e.g. weather data and current boat position 26 | self.crew.helmsman.sail () 27 | -------------------------------------------------------------------------------- /simpylc/simulations/boat/control_client/constants.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | 20 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 21 | 22 | __________________________________________________________________________ 23 | 24 | It is meant for training purposes only. 25 | 26 | Removing this header ends your license. 27 | ''' 28 | 29 | import os 30 | 31 | import simpylc as sp 32 | 33 | finity = 1e20 34 | boomLength = 1.5 35 | waypointsFilename = 'current.waypoints' 36 | -------------------------------------------------------------------------------- /simpylc/simulations/boat/control_client/crew.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Requirements 3 | 4 | The control, symbolized by a "virtual" crew, steers the boat along a sequence of approximately straight line segments. 5 | Since we're dealing with a sailing vessel, it's not always possible to directly follow a given line segment. 6 | If needed the vessel will ply against the wind to get to the end of the line segment. 7 | 8 | ====== Testspecification 9 | 10 | ====== Design 11 | 12 | The crew consists of the following hierarchy: 13 | 14 | --- Captain 15 | 16 | - Plans the overal route depending on e.g. prevalent wind directions. 17 | - Breaks this route down into segments short enough te be assumed straight line segments rather than loxodromes, each between two 'waypoints'. 18 | - The list of waypoints is communicated to the Helmsman. 19 | - Climate change is assumed to go slow enough to postpone or forego any clever tricks at the poles. 20 | 21 | --- Helmsman 22 | 23 | - Steer the boat along a sequence of segments as defined above. 24 | - Each segment ends with rounding a waypoint. 25 | - If the next waypoint is exactly ahead, the current one is rounded at the side the next one should be rounded. 26 | - If a waypoint is in the dead sector, a cross gauge is done on the waypoint, followed by tacking. 27 | - Gibing happens whenever needed by shortening and subsequent lengthening of the sheet and has no influence on the course. 28 | - The boat hardware may (initially ;) ) not support sheet control, in which case the sheet length output is simply ignored. 29 | 30 | --- Deckhand 31 | 32 | - Deals directly with the boat hardware. 33 | - Provides low level safety interlocks to prevent hardware damage. 34 | - Abstracts away hardware peculiarities, thus enabling easy exchange and second sourcing of hardware parts. 35 | - Converts between general engineering conventions and hardware c.q. application dependent conventions. 36 | 37 | Any crew member can directly contact any other crew member. 38 | Any crew member can make use of the Almanac, that embodies basic nautical and geometric knowledge. 39 | 40 | ''' 41 | 42 | import sys as ss 43 | 44 | import captain as ct 45 | import helmsman as hm 46 | import deckhand_twin as dh 47 | 48 | this = ss.modules [__name__] 49 | 50 | deckhand = dh.Deckhand (this) 51 | helmsman = hm.Helmsman (this) 52 | captain = ct.Captain (this) 53 | 54 | deckhand.sweep = helmsman.sweep 55 | captain.navigate () 56 | 57 | -------------------------------------------------------------------------------- /simpylc/simulations/boat/control_client/current.waypoints: -------------------------------------------------------------------------------- 1 | 51.93734181944299 4.507289002070285 2 | 51.93773177103959 4.514761834017166 3 | 51.93596003112272 4.517810751119056 4 | 51.93426034566331 4.514730296591516 5 | 51.93633143177675 4.511830778368795 6 | 51.93712928104243 4.517492383977806 7 | 51.93362043024361 4.522004023982875 8 | -------------------------------------------------------------------------------- /simpylc/simulations/boat/control_client/deckhand.py: -------------------------------------------------------------------------------- 1 | import almanac as an 2 | 3 | class Deckhand: 4 | def __init__ (self, crew): 5 | self.crew = crew 6 | 7 | def work (self, sweep): 8 | # Make connection 9 | # In a loop, call input, sweep, output and sleep in that order 10 | 11 | def input (self): 12 | # Receive sensor data from ship 13 | 14 | def output (self): 15 | # Send actuator data to ship 16 | 17 | def getCurrentLocation (self): 18 | # Read out GPS data 19 | # ... 20 | # Convert to signed lattitude, longitude (see comment in almanac) 21 | # ... 22 | return lattitude, longitude 23 | 24 | def getCourseAngle (self): 25 | # Read out compass 26 | # ... 27 | # Convert to signed angle between -180 and 180 degrees, north == 0, east == 90, west == -90 28 | # ... 29 | return courseAngle 30 | 31 | def getVaneAngle (self): 32 | # Read out wind vane 33 | # ... 34 | # Convert to signed angle between -180 and 180 degrees, stern == 0, counter clockwise == 90, clockwise == -90 35 | # ... 36 | return vaneAngle 37 | 38 | def setRudderAngle (self, courseAngle): 39 | # Convert from signed angle between -180 and 180 degrees, stern == 0, counter clockwise == 90, clockwise == -90 40 | # ... 41 | # Write to rudder servo 42 | # ... 43 | 44 | def setSailAngle (self, sailAngle): 45 | # Convert from signed angle between -180 and 180 degrees, stern == 0, counter clockwise == 90, clockwise == -90 46 | # ... 47 | # Write to sheet winch, to sail servo or to the all encompassing nothing 48 | # ... 49 | 50 | def holdPosition (self): 51 | # Let sail direction freely go along with the wind direction 52 | # ... 53 | # Write to sheet winch, to sail servo or to the all encompassing nothing 54 | # ... 55 | 56 | -------------------------------------------------------------------------------- /simpylc/simulations/boat/control_client/deckhand_twin.py: -------------------------------------------------------------------------------- 1 | import sys as ss 2 | 3 | ss.path.append ('..') 4 | 5 | import time as tm 6 | import math as mt 7 | 8 | import socket as sc 9 | 10 | import simpylc as sp 11 | 12 | import socket_wrapper as sw 13 | import vessel as vs 14 | import almanac as an 15 | import constants as cs 16 | 17 | class Deckhand: 18 | def __init__ (self, crew): 19 | self.crew = crew 20 | 21 | def work (self, sweep): 22 | with sc.socket (*sw.socketType) as self.clientSocket: 23 | self.clientSocket.connect (sw.address) 24 | self.socketWrapper = sw.SocketWrapper (self.clientSocket) 25 | 26 | while True: 27 | self.input () 28 | sweep () 29 | self.output () 30 | tm.sleep (0.02) 31 | 32 | def input (self): 33 | sensors = self.socketWrapper.recv () 34 | 35 | self.courseAngle = sensors ['courseAngle'] 36 | self.vaneAngle = sensors ['vaneAngle'] 37 | self.lattitude = sensors ['lattitude'] 38 | self.longitude = sensors ['longitude'] 39 | 40 | def output (self): 41 | actuators = { 42 | 'rudderAngle': self.rudderAngle, 43 | 'sheetLength': self.sheetLength 44 | } 45 | 46 | self.socketWrapper.send (actuators) 47 | 48 | def getCurrentLocation (self): 49 | return self.lattitude, self.longitude 50 | 51 | def getCourseAngle (self): 52 | return an.symmetrize (self.courseAngle) 53 | 54 | def getVaneAngle (self): 55 | return an.symmetrize (self.vaneAngle) 56 | 57 | def setRudderAngle (self, rudderAngle): 58 | self.rudderAngle = an.symmetrize (rudderAngle) 59 | 60 | def setSailAngle (self, sailAngle): 61 | self.sheetLength = 2 * mt.sin (0.5 * abs (an.symmetrize (sailAngle)) * an.radianPerDegree) * cs.boomLength 62 | 63 | def holdPosition (self): 64 | self.sheetLength = cs.finity 65 | -------------------------------------------------------------------------------- /simpylc/simulations/boat/control_client/helmsman.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import time as tm 29 | import math as mt 30 | import sys as ss 31 | import os 32 | 33 | ss.path += [os.path.abspath (relPath) for relPath in ('..',)] 34 | 35 | import socket_wrapper as sw 36 | import almanac as an 37 | 38 | class Helmsman: 39 | def __init__ (self, crew): 40 | self.crew = crew 41 | self.targetWaypointIndex = 0 42 | self.exclude = an.Exclude (45) 43 | 44 | def sweep (self): 45 | try: 46 | leg = an.getLeg (self.crew.deckhand.getCurrentLocation (), self.crew.captain.waypoints [self.targetWaypointIndex]) 47 | targetAngle = an.getLegAngle (leg) 48 | targetDistance = an.getLegLength (leg) 49 | 50 | courseAngle = self.crew.deckhand.getCourseAngle () 51 | vaneAngle = self.crew.deckhand.getVaneAngle () 52 | windAngle = an.symmetrize (vaneAngle + courseAngle) 53 | 54 | heightAngle = an.symmetrize (targetAngle - windAngle) 55 | sailableHeightAngle = self.exclude (heightAngle) # Will result in plying automagically if needed 56 | sailableTargetAngle = an.symmetrize (sailableHeightAngle + windAngle - courseAngle) 57 | 58 | self.crew.deckhand.setRudderAngle (an.clip (-sailableTargetAngle, 45)) 59 | self.crew.deckhand.setSailAngle (an.clip (vaneAngle / 2, 90)) # Chinese gybes allowed 60 | 61 | if targetDistance < 1: 62 | self.targetWaypointIndex += 1 63 | 64 | except IndexError: # No more waypoints 65 | self.crew.deckhand.holdPosition () 66 | 67 | def sail (self): 68 | self.crew.deckhand.work (self.sweep) 69 | -------------------------------------------------------------------------------- /simpylc/simulations/boat/control_client/kml_to_waypoints.py: -------------------------------------------------------------------------------- 1 | print ('N.B. The .kml file must have been saved in the "decimal degrees" mode!') 2 | filePrename = input ('Prename of .kml file, i.e. without extension: ') 3 | kmlFile = open (filePrename + '.kml') 4 | 5 | while True: 6 | line = kmlFile.readline () 7 | if '' in line: 8 | break 9 | 10 | waypointsFile = open (filePrename + '.waypoints', 'w') 11 | 12 | while True: 13 | line = kmlFile.readline () 14 | if '' in line: 15 | break 16 | 17 | triplets = [item.split (',') for item in line.split ()] 18 | waypoints = [(lattitude, longitude) for longitude, lattitude, height in triplets] 19 | 20 | for waypoint in waypoints: 21 | print (waypoint [0], waypoint [1], file = waypointsFile) 22 | 23 | print (f'Conversion from {filePrename}.kml to {filePrename}.waypoints done.') 24 | -------------------------------------------------------------------------------- /simpylc/simulations/boat/control_client/kralingse_plas.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/QQuick/SimPyLC/51bcb8052f06e11a2d9747bb7d0a26a59754deb7/simpylc/simulations/boat/control_client/kralingse_plas.jpg -------------------------------------------------------------------------------- /simpylc/simulations/boat/control_client/kralingse_plas.kml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Untitled Project 5 | 6 | 27 | 28 | 29 | 49 | 50 | 51 | 52 | normal 53 | #__managed_style_1111B0A98B23C1CB9E3C 54 | 55 | 56 | highlight 57 | #__managed_style_24B8B0C5C523C1CB9E3C 58 | 59 | 60 | 61 | Untitled Path 62 | 63 | 4.51300858646313 64 | 51.9353307503631 65 | -2.154885172803048 66 | 0 67 | 0 68 | 35 69 | 2950.302581416327 70 | absolute 71 | 72 | #__managed_style_0B4DE3CC8623C1CB9E3C 73 | 74 | 75 | 4.507289002070285,51.93734181944299,-4.583627972819892 4.514761834017166,51.93773177103959,-4.581177386594518 4.517810751119056,51.93596003112272,-6.104179338983115 4.514730296591516,51.93426034566331,-0.02302263453026088 4.511830778368795,51.93633143177675,-3.163915755575407 4.517492383977806,51.93712928104243,-4.669694039051392 4.522004023982875,51.93362043024361,-4.583455861037146 76 | 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /simpylc/simulations/boat/control_server.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import socket as sc 29 | import time as tm 30 | 31 | import simpylc as sp 32 | import socket_wrapper as sw 33 | 34 | class ControlServer: 35 | def __init__ (self): 36 | with sc.socket (*sw.socketType) as serverSocket: 37 | serverSocket.bind (sw.address) 38 | serverSocket.listen (sw.maxNrOfConnectionRequests) 39 | 40 | while True: 41 | self.clientSocket = serverSocket.accept ()[0] 42 | self.socketWrapper = sw.SocketWrapper (self.clientSocket) 43 | 44 | with self.clientSocket: 45 | while True: 46 | sensors = { 47 | 'courseAngle': sp.eva (sp.world.vessel.courseAngle), 48 | 'vaneAngle': sp.eva (sp.world.vessel.vaneAngle), 49 | 'lattitude': sp.eva (sp.world.vessel.lattitude), 50 | 'longitude': sp.eva (sp.world.vessel.longitude) 51 | } 52 | 53 | self.socketWrapper.send (sensors) 54 | tm.sleep (0.02) 55 | actuators = self.socketWrapper.recv () 56 | sp.world.vessel.rudderAngle.set (actuators ['rudderAngle']) 57 | sp.world.vessel.sheetLength.set (actuators ['sheetLength']) 58 | 59 | -------------------------------------------------------------------------------- /simpylc/simulations/boat/notes.txt: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /simpylc/simulations/boat/socket_wrapper.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import socket as sc 29 | import json as js 30 | 31 | address = 'localhost', 50012 32 | socketType = sc.AF_INET, sc.SOCK_STREAM 33 | maxNrOfConnectionRequests = 5 34 | maxMessageLength = 2048 35 | 36 | class SocketWrapper: 37 | def __init__ (self, clientSocket): 38 | self.clientSocket = clientSocket 39 | 40 | def send (self, anObject): 41 | buffer = bytes (f'{js.dumps (anObject):<{maxMessageLength}}', 'ascii') 42 | 43 | totalNrOfSentBytes = 0 44 | 45 | while totalNrOfSentBytes < maxMessageLength: 46 | nrOfSentBytes = self.clientSocket.send (buffer [totalNrOfSentBytes:]) 47 | 48 | if not nrOfSentBytes: 49 | self.raiseConnectionError () 50 | 51 | totalNrOfSentBytes += nrOfSentBytes 52 | 53 | def recv (self): 54 | totalNrOfReceivedBytes = 0 55 | receivedChunks = [] 56 | 57 | while totalNrOfReceivedBytes < maxMessageLength: 58 | receivedChunk = self.clientSocket.recv (maxMessageLength - totalNrOfReceivedBytes) 59 | 60 | if not receivedChunk: 61 | self.raiseConnectionError () 62 | 63 | receivedChunks.append (receivedChunk) 64 | totalNrOfReceivedBytes += len (receivedChunk) 65 | 66 | return js.loads (b''.join (receivedChunks) .decode ('ascii')) 67 | 68 | def raiseConnectionError (self): 69 | raise RuntimeError ('Socket connection broken') 70 | -------------------------------------------------------------------------------- /simpylc/simulations/boat/vessel.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2022 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import sys as ss 29 | 30 | ss.path.append ('./control_client') 31 | 32 | import simpylc as sp 33 | 34 | import constants as cs 35 | import almanac as an 36 | 37 | waypoints = an.getWaypoints () 38 | 39 | class Vessel (sp.Module): 40 | def __init__ (self): 41 | sp.Module.__init__ (self) 42 | 43 | self.page ('State [m, s, °]') 44 | 45 | self.group ('Environment', True) 46 | 47 | self.windAngle = sp.Register (135) # SW 48 | self.windSpeed = sp.Register () 49 | 50 | self.group ('Actuators') 51 | 52 | self.rudderAngle = sp.Register () 53 | self.sheetLength = sp.Register (sp.finity) 54 | 55 | self.group ('Sensors') 56 | 57 | self.courseAngle = sp.Register () 58 | self.vaneAngle = sp.Register () 59 | 60 | self.lattitude = sp.Register (waypoints [0][0]) 61 | self.longitude = sp.Register (waypoints [0][1]) 62 | 63 | self.group ('Kinematics', True) 64 | 65 | self.acceleration = sp.Register () 66 | self.velocity = sp.Register () 67 | 68 | self.group ('Camera') 69 | 70 | self.soccerView = sp.Latch (True) 71 | self.heliView = sp.Latch () 72 | self.helmsmanView = sp.Latch () 73 | 74 | self.page ('Internals [m, s, °]') 75 | 76 | self.group ('Dynamics settings', True) 77 | 78 | self.boatMass = sp.Register (10) 79 | self.sailFactor = sp.Register (3) 80 | self.rudderFactor = sp.Register (1) 81 | self.dragFactor = sp.Register (2) 82 | 83 | self.group ('Dynamics helpers') 84 | 85 | self.carthesianAngle = sp.Register () 86 | self.absSailAngle = sp.Register () 87 | self.sailAngle = sp.Register () 88 | self.attackAngle = sp.Register () 89 | self.sailForce = sp.Register () 90 | self.forwardForce = sp.Register () 91 | self.dragForce = sp.Register () 92 | 93 | self.group ('Kinematics helpers', True) 94 | 95 | self.velocityX = sp.Register () 96 | self.velocityY = sp.Register () 97 | 98 | self.group ('Camera settings') 99 | 100 | self.soccerFocusDist = sp.Register (9) 101 | self.heliFocusDist = sp.Register (30) 102 | self.helmsmanFocusDist = sp.Register (6) 103 | 104 | self.group ('Camera helpers') 105 | 106 | self.helmsmanFocusDistX = sp.Register () 107 | self.helmsmanFocusDistY = sp.Register () 108 | 109 | self.soccerViewOneshot = sp.Oneshot () 110 | self.heliViewOneshot = sp.Oneshot () 111 | self.helmsmanViewOneshot = sp.Oneshot () 112 | 113 | def sweep (self): 114 | self.page ('Boat physics') 115 | 116 | self.group ('Dynamics') 117 | 118 | self.courseAngle.set (sp.sym (self.courseAngle - self.rudderFactor * self.rudderAngle * self.velocity / self.boatMass)) 119 | self.carthesianAngle.set (sp.asym (self.courseAngle + 90)) 120 | self.vaneAngle.set (sp.sym (self.windAngle - self.courseAngle)) 121 | 122 | try: 123 | self.absSailAngle.set (sp.min (2 * sp.asin (0.5 * self.sheetLength / cs.boomLength), sp.abs (self.vaneAngle), 90)) # 1 isosceles triangle == 2 right angled triangles 124 | except ValueError: # Sheetlength too large, assume hold position 125 | self.absSailAngle.set (sp.abs (self.vaneAngle)) 126 | 127 | self.sailAngle.set (self.absSailAngle if self.vaneAngle >= 0 else -self.absSailAngle) 128 | self.attackAngle.set (self.vaneAngle - self.sailAngle) 129 | 130 | self.sailForce.set (self.sailFactor * self.windSpeed * sp.sin (self.attackAngle)) 131 | self.forwardForce.set (self.sailForce * sp.sin (self.sailAngle)) 132 | self.dragForce.set (self.dragFactor * sp.pow (self.velocity, 2)) 133 | 134 | self.group ('Kinematics') 135 | 136 | self.acceleration.set ((self.forwardForce - self.dragForce) / self.boatMass) 137 | self.velocity.set (self.velocity + self.acceleration * sp.world.period) 138 | 139 | self.velocityX.set (self.velocity * sp.cos (self.carthesianAngle)) 140 | self.velocityY.set (self.velocity * sp.sin (self.carthesianAngle)) 141 | 142 | lattitude, longitude = an.getPosition (sp.tEva ((self.lattitude, self.longitude)), (self.velocityX, self.velocityY), sp.world.period) 143 | self.lattitude.set (lattitude) 144 | self.longitude.set (longitude) 145 | 146 | self.group ('Camera') 147 | 148 | self.soccerView.unlatch (self.heliViewOneshot or self.helmsmanViewOneshot) 149 | self.heliView.unlatch (self.soccerViewOneshot or self.helmsmanViewOneshot) 150 | self.helmsmanView.unlatch (self.soccerViewOneshot or self.heliViewOneshot) 151 | 152 | self.soccerViewOneshot.trigger (self.soccerView) 153 | self.heliViewOneshot.trigger (self.heliView) 154 | self.helmsmanViewOneshot.trigger (self.helmsmanView) 155 | 156 | self.helmsmanFocusDistX.set (self.helmsmanFocusDist * sp.cos (self.carthesianAngle)) 157 | self.helmsmanFocusDistY.set (self.helmsmanFocusDist * sp.sin (self.carthesianAngle)) 158 | 159 | -------------------------------------------------------------------------------- /simpylc/simulations/boat/visualisation.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | ''' 29 | 30 | z 31 | | 32 | o -- y 33 | / 34 | x 35 | 36 | ''' 37 | 38 | import random as rd 39 | import sys as ss 40 | 41 | ss.path.append ('./control_client') 42 | 43 | import simpylc as sp 44 | 45 | import constants as cs 46 | import almanac as an 47 | 48 | seaColor = (0.001, 0.001, 0.006) 49 | stripeColor = (0.5, 0.5, 1) 50 | hullColor = (1, 0, 0) 51 | mastColor = (1, 0, 1) 52 | sailColor = (1, 1, 1) 53 | keelColor = (0, 1, 0) 54 | vaneColor = (1, 1, 0) 55 | 56 | class Boat: 57 | def __init__ (self): 58 | self.keel = sp.Beam (size = (0.6, 0.03, 1.2), center = (0, 0, -0.75), color = keelColor) 59 | self.hull = sp.Beam (size = (1.5, 0.6, 0.45), center = (-0.75, 0, 0.75), color = hullColor) 60 | self.bowLeft = sp.Beam (size = (1.5, 0.3, 0.45), center = (1.47, 0.075, 0), angle = -5.5, color = hullColor) 61 | self.bowRight = sp.Beam (size = (1.5, 0.3, 0.45), center = (1.47, -0.075, 0), angle = 5.5, color = hullColor) 62 | self.bowFront = sp.Cylinder (size = (0.3, 0.3, 0.45), center = (2.19, 0, 0), color = hullColor) 63 | self.rudder = sp.Beam (size = (0.39, 0.03, 1.2), center = (-0.75, 0, -0.3), color = keelColor) 64 | self.mast = sp.Cylinder (size = (0.075, 0.075, 3.6), center = (1.2, 0, 1.8), color = mastColor) 65 | self.sail = sp.Beam (size = (cs.boomLength, 0.09, 2.7), center = (-0.84, 0, 0.15), joint = (0.75, 0, 0), color = sailColor) 66 | self.vane = sp.Beam (size = (0.6, 0.06, 0.09), center = (-0.3, 0, 1.65), joint = (0.3, 0, 0), color = vaneColor) 67 | 68 | def __call__ (self): 69 | return self.keel (position = (0, 0, 0), rotation = sp.world.vessel.carthesianAngle, parts = lambda: 70 | self.hull (parts = lambda: 71 | self.bowLeft () + 72 | self.bowRight () + 73 | self.bowFront () + 74 | self.rudder (rotation = sp.world.vessel.rudderAngle) + 75 | self.mast (parts = lambda: 76 | self.sail (rotation = sp.world.vessel.sailAngle) + 77 | self.vane (rotation = sp.world.vessel.vaneAngle) 78 | ) 79 | ) 80 | ) 81 | 82 | class Sea (sp.Beam): 83 | side = 1600 84 | spacing = 4 85 | halfSteps = round (0.5 * side / spacing) 86 | 87 | class Stripe (sp.Beam): 88 | def __init__ (self, **arguments): 89 | super () .__init__ (size = (0.05, Sea.side, 0.001), **arguments) 90 | 91 | def __init__ (self, lattitude, longitude): 92 | super () .__init__ (size = (self.side, self.side, 0.0005), color = seaColor) 93 | 94 | self.lattitude = lattitude 95 | self.longitude = longitude 96 | 97 | self.xStripes = [self.Stripe (center = (0, nr * self.spacing, 0.0001), angle = 90, color = stripeColor) for nr in range (-self.halfSteps, self.halfSteps)] 98 | self.yStripes = [self.Stripe (center = (nr * self.spacing, 0, 0), color = stripeColor) for nr in range (-self.halfSteps, self.halfSteps)] 99 | 100 | def __call__ (self, lattitudeBoat, longitudeBoat): 101 | distanceX, distanceY = an.getLeg ( 102 | (lattitudeBoat, longitudeBoat), 103 | (self.lattitude, self.longitude) 104 | ) 105 | 106 | return super () .__call__ (color = seaColor, position = (distanceX, distanceY, 0), parts = lambda: 107 | sum (xStripe () for xStripe in self.xStripes) + 108 | sum (yStripe () for yStripe in self.yStripes) 109 | ) 110 | 111 | class Buoy: 112 | def __init__ (self, lattitude, longitude): 113 | self.lattitude = lattitude 114 | self.longitude = longitude 115 | 116 | self.cone = sp.Cone ( 117 | size = (0.3, 0.3, 0.3), 118 | center = (0, 0, 0.3), 119 | color = (1, 1, 0), 120 | group = 1 121 | ) 122 | 123 | def __call__ (self, lattitudeBoat, longitudeBoat): 124 | distanceX, distanceY = an.getLeg ( 125 | (lattitudeBoat, longitudeBoat), 126 | (self.lattitude, self.longitude) 127 | ) 128 | 129 | return self.cone ( 130 | position = (distanceX, distanceY, 0), 131 | scale = (10, 10, 1) if sp.eva (sp.world.vessel.heliView) and sp.eva (sp.world.vessel.heliFocusDist > 100) else (1, 1, 1) 132 | ) 133 | 134 | class Visualisation (sp.Scene): 135 | def __init__ (self): 136 | super () .__init__ () 137 | self.buoys = [] 138 | 139 | self.camera = sp.Camera () 140 | self.boat = Boat () 141 | 142 | waypoints = an.getWaypoints () 143 | 144 | self.sea = Sea (waypoints [0][0], waypoints [1][1]) 145 | 146 | for waypoint in waypoints: 147 | self.buoys.append (Buoy (waypoint [0], waypoint [1])) 148 | 149 | def display (self): 150 | if sp.world.vessel.soccerView: 151 | self.camera ( 152 | position = sp.tEva ((0, -sp.world.vessel.soccerFocusDist, 2)), 153 | focus = sp.tEva ((0, 0, 0.3)) 154 | ) 155 | elif sp.world.vessel.heliView: 156 | self.camera ( 157 | position = sp.tEva ((0, -0.0000001, sp.world.vessel.heliFocusDist)), 158 | focus = sp.tEva ((0, 0, 0)) 159 | ) 160 | elif sp.world.vessel.helmsmanView: 161 | self.camera ( 162 | position = sp.tEva ((0, 0, 0.5)), 163 | focus = sp.tEva ((sp.world.vessel.helmsmanFocusDistX, sp.world.vessel.helmsmanFocusDistY, 0)) 164 | ) 165 | 166 | self.boat () 167 | self.sea (*sp.tEva ((sp.world.vessel.lattitude, sp.world.vessel.longitude))) 168 | 169 | for buoy in self.buoys: 170 | buoy (*sp.tEva ((sp.world.vessel.lattitude, sp.world.vessel.longitude))) 171 | 172 | -------------------------------------------------------------------------------- /simpylc/simulations/boat/world.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import os 29 | import sys as ss 30 | import time as tm 31 | 32 | ss.path += [os.path.abspath (relPath) for relPath in ('../../..', '..')] # If you want to store your simulations somewhere else, put SimPyLC in your PYTHONPATH environment variable 33 | scannerType = 'lidar' 34 | 35 | import simpylc as sp 36 | 37 | import control_server as cs 38 | import vessel as vl 39 | import visualisation as vs 40 | 41 | vs.scannerType = scannerType 42 | 43 | sp.World ( 44 | cs.ControlServer, 45 | vl.Vessel, 46 | vs.Visualisation 47 | ) 48 | -------------------------------------------------------------------------------- /simpylc/simulations/car/control_client/hardcoded_client.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import time as tm 29 | import traceback as tb 30 | import math as mt 31 | import sys as ss 32 | import os 33 | import socket as sc 34 | 35 | ss.path += [os.path.abspath (relPath) for relPath in ('..',)] 36 | 37 | import socket_wrapper as sw 38 | import parameters as pm 39 | 40 | class HardcodedClient: 41 | def __init__ (self): 42 | self.steeringAngle = 0 43 | 44 | with open (pm.sampleFileName, 'w') as self.sampleFile: 45 | with sc.socket (*sw.socketType) as self.clientSocket: 46 | self.clientSocket.connect (sw.address) 47 | self.socketWrapper = sw.SocketWrapper (self.clientSocket) 48 | self.halfApertureAngle = False 49 | 50 | while True: 51 | self.input () 52 | self.sweep () 53 | self.output () 54 | self.logTraining () 55 | tm.sleep (0.02) 56 | 57 | def input (self): 58 | sensors = self.socketWrapper.recv () 59 | 60 | if not self.halfApertureAngle: 61 | self.halfApertureAngle = sensors ['halfApertureAngle'] 62 | self.sectorAngle = 2 * self.halfApertureAngle / pm.lidarInputDim 63 | self.halfMiddleApertureAngle = sensors ['halfMiddleApertureAngle'] 64 | 65 | if 'lidarDistances' in sensors: 66 | self.lidarDistances = sensors ['lidarDistances'] 67 | else: 68 | self.sonarDistances = sensors ['sonarDistances'] 69 | 70 | def lidarSweep (self): 71 | nearestObstacleDistance = pm.finity 72 | nearestObstacleAngle = 0 73 | 74 | nextObstacleDistance = pm.finity 75 | nextObstacleAngle = 0 76 | 77 | for lidarAngle in range (-self.halfApertureAngle, self.halfApertureAngle): 78 | lidarDistance = self.lidarDistances [lidarAngle] 79 | 80 | if lidarDistance < nearestObstacleDistance: 81 | nextObstacleDistance = nearestObstacleDistance 82 | nextObstacleAngle = nearestObstacleAngle 83 | 84 | nearestObstacleDistance = lidarDistance 85 | nearestObstacleAngle = lidarAngle 86 | 87 | elif lidarDistance < nextObstacleDistance: 88 | nextObstacleDistance = lidarDistance 89 | nextObstacleAngle = lidarAngle 90 | 91 | targetObstacleDistance = (nearestObstacleDistance + nextObstacleDistance) / 2 92 | 93 | self.steeringAngle = (nearestObstacleAngle + nextObstacleAngle) / 2 94 | self.targetVelocity = pm.getTargetVelocity (self.steeringAngle) 95 | 96 | def sonarSweep (self): 97 | obstacleDistances = [pm.finity for sectorIndex in range (3)] 98 | obstacleAngles = [0 for sectorIndex in range (3)] 99 | 100 | for sectorIndex in (-1, 0, 1): 101 | sonarDistance = self.sonarDistances [sectorIndex] 102 | sonarAngle = 2 * self.halfMiddleApertureAngle * sectorIndex 103 | 104 | if sonarDistance < obstacleDistances [sectorIndex]: 105 | obstacleDistances [sectorIndex] = sonarDistance 106 | obstacleAngles [sectorIndex] = sonarAngle 107 | 108 | if obstacleDistances [-1] > obstacleDistances [0]: 109 | leftIndex = -1 110 | else: 111 | leftIndex = 0 112 | 113 | if obstacleDistances [1] > obstacleDistances [0]: 114 | rightIndex = 1 115 | else: 116 | rightIndex = 0 117 | 118 | self.steeringAngle = (obstacleAngles [leftIndex] + obstacleAngles [rightIndex]) / 2 119 | self.targetVelocity = pm.getTargetVelocity (self.steeringAngle) 120 | 121 | def sweep (self): 122 | if hasattr (self, 'lidarDistances'): 123 | self.lidarSweep () 124 | else: 125 | self.sonarSweep () 126 | 127 | def output (self): 128 | actuators = { 129 | 'steeringAngle': self.steeringAngle, 130 | 'targetVelocity': self.targetVelocity 131 | } 132 | 133 | self.socketWrapper.send (actuators) 134 | 135 | def logLidarTraining (self): 136 | sample = [pm.finity for entryIndex in range (pm.lidarInputDim + 1)] 137 | 138 | for lidarAngle in range (-self.halfApertureAngle, self.halfApertureAngle): 139 | sectorIndex = round (lidarAngle / self.sectorAngle) 140 | sample [sectorIndex] = min (sample [sectorIndex], self.lidarDistances [lidarAngle]) 141 | 142 | sample [-1] = self.steeringAngle 143 | print (*sample, file = self.sampleFile) 144 | 145 | def logSonarTraining (self): 146 | sample = [pm.finity for entryIndex in range (pm.sonarInputDim + 1)] 147 | 148 | for entryIndex, sectorIndex in ((2, -1), (0, 0), (1, 1)): 149 | sample [entryIndex] = self.sonarDistances [sectorIndex] 150 | 151 | sample [-1] = self.steeringAngle 152 | print (*sample, file = self.sampleFile) 153 | 154 | def logTraining (self): 155 | if hasattr (self, 'lidarDistances'): 156 | self.logLidarTraining () 157 | else: 158 | self.logSonarTraining () 159 | 160 | HardcodedClient () 161 | -------------------------------------------------------------------------------- /simpylc/simulations/car/control_client/parameters.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | finity = 20.0 # Needs to be float to obtain ditto numpy array 29 | 30 | lidarInputDim = 16 31 | sonarInputDim = 3 32 | 33 | sampleFileName = 'default.samples' 34 | 35 | def getTargetVelocity (steeringAngle): 36 | return (90 - abs (steeringAngle)) / 60 37 | -------------------------------------------------------------------------------- /simpylc/simulations/car/control_server.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import socket as sc 29 | import time as tm 30 | 31 | import simpylc as sp 32 | import socket_wrapper as sw 33 | 34 | class ControlServer: 35 | def __init__ (self): 36 | with sc.socket (*sw.socketType) as serverSocket: 37 | serverSocket.bind (sw.address) 38 | serverSocket.listen (sw.maxNrOfConnectionRequests) 39 | 40 | while True: 41 | self.clientSocket = serverSocket.accept ()[0] 42 | self.socketWrapper = sw.SocketWrapper (self.clientSocket) 43 | 44 | with self.clientSocket: 45 | while True: 46 | sensors = { 47 | 'halfApertureAngle': sp.world.visualisation.scanner.halfApertureAngle, 48 | 'halfMiddleApertureAngle': sp.world.visualisation.scanner.halfMiddleApertureAngle 49 | } | ( 50 | {'lidarDistances': sp.world.visualisation.scanner.lidarDistances} 51 | if hasattr (sp.world.visualisation.scanner, 'lidarDistances') else 52 | {'sonarDistances': sp.world.visualisation.scanner.sonarDistances} 53 | ) 54 | 55 | self.socketWrapper.send (sensors) 56 | tm.sleep (0.02) 57 | actuators = self.socketWrapper.recv () 58 | 59 | sp.world.physics.steeringAngle.set (actuators ['steeringAngle']) 60 | sp.world.physics.targetVelocity.set (actuators ['targetVelocity']) 61 | 62 | -------------------------------------------------------------------------------- /simpylc/simulations/car/dimensions.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | 20 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 21 | 22 | __________________________________________________________________________ 23 | 24 | It is meant for training purposes only. 25 | 26 | Removing this header ends your license. 27 | ''' 28 | 29 | import simpylc as sp 30 | 31 | wheelDiameter = 0.10 32 | wheelRadius = wheelDiameter / 2 33 | displacementPerWheelAngle = sp.radiansPerDegree * wheelRadius 34 | 35 | wheelBase = 0.40 36 | wheelShift = wheelBase / 2 37 | 38 | apertureAngle = 120 39 | middleApertureAngle = 45 -------------------------------------------------------------------------------- /simpylc/simulations/car/lidar.track: -------------------------------------------------------------------------------- 1 | - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 2 | - - - - - - - * - - - * - - - - - - - - - - - - - - - - - - - - 3 | - - - - - - - - - - - - - - * - - - - - - - - - - - * - - - - - 4 | - - - * - - - * - - * - - - - - - - * - - - * - - - - - - - * - 5 | - - - - - * - - - - - - - * - - - - - - @ - - - - * - - - - - - 6 | - - - - - - - - - - - - - - - - - * - - - - * - - - - - * - - - 7 | - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 8 | - * - * - - - - - - - - - - - - - - - - - - - - - - - - - - * - 9 | - - - - - - - - - - - - - - - - - - - - - - - - - - - - * - - - 10 | - - - - - - - - - - * - - * - - - - - - - - - - - - - - - - - - 11 | - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 12 | - - * - * - - * - - - - * - - - - * - - - - - - - - - - * - * - 13 | - - - - - - - - - * - - - - - * - - - - - - - - - - - - - - - - 14 | - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 15 | - * - * - - - - - - - - - - - - - - - - - - - - - - - - - - - - 16 | - - - - - - - * - * - - - - - - - * - * - - - - - - - * - * - - 17 | - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 18 | - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 19 | - * - * - - - - * - * - - - - - - - - - - - - - - - - - - - - - 20 | - - - - - - - - - - - - - - - - - * - * - - - - - - * - * - - - 21 | - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 22 | - - - - - - - - - * - * - - - - - - - - - - - - - - - - - - - - 23 | - - * - * - - - - - - - - - - - - - - - - - - - - - - - - - - - 24 | - - - - - - - - - - - - - - - - - * - * - - - - - - * - * - - - 25 | - - - - - - - - - * - * - - - - - - - - - - - - - - - - - - - - 26 | - - - * - - - - - - - - - - - - - - - - - - - - - - - - - - - - 27 | - * - - - - - - - - - - - - - - - - - - * - - - - - - - - - - - 28 | - - - - * - - - - * - - - - - - - - * - - - - - - - - * - * - - 29 | - - - - - - * - * - - * - - - - - - - - - * - - - - - - - - - - 30 | - - * - - - - - - - - - - - - - - - - - - - - - * - * - - - - - 31 | - - - - - - * - - * - - - - - - - - - - * - - - - - - - * - - - 32 | - - - - - - - - - - - - - - - - - - - - - - - - * - - - - - - - 33 | -------------------------------------------------------------------------------- /simpylc/simulations/car/physics.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | import dimensions as dm 31 | 32 | class Physics (sp.Module): 33 | def __init__ (self): 34 | sp.Module.__init__ (self) 35 | 36 | self.page ('car physics dashboard') 37 | 38 | self.group ('wheels', True) 39 | 40 | self.acceleration = sp.Register (2) 41 | 42 | self.targetVelocity= sp.Register () 43 | self.velocity = sp.Register () 44 | 45 | self.steeringAngle = sp.Register () 46 | 47 | self.group ('position', True) 48 | 49 | self.attitudeAngle = sp.Register (50) 50 | self.courseAngle = sp.Register () 51 | 52 | self.positionX = sp.Register () 53 | self.positionY = sp.Register () 54 | 55 | self.group('slip', True) 56 | 57 | self.radialAcceleration = sp.Register () 58 | self.slipping = sp.Marker () 59 | 60 | self.group ('camera') 61 | 62 | self.soccerView = sp.Latch (True) 63 | self.heliView = sp.Latch () 64 | self.driverView = sp.Latch () 65 | 66 | self.driverFocusDist = sp.Register (2) 67 | 68 | self.page ('car physics internals') 69 | 70 | self.group ('wheels', True) 71 | 72 | self.midWheelAngularVelocity = sp.Register () 73 | self.midWheelAngle = sp.Register (30) 74 | 75 | self.midSteeringAngle = sp.Register () 76 | self.inverseMidCurveRadius = sp.Register (20) 77 | self.midAngularVelocity = sp.Register () 78 | 79 | self.group ('position', True) 80 | 81 | self.tangentialVelocity = sp.Register () 82 | self.velocityX = sp.Register () 83 | self.velocityY = sp.Register () 84 | 85 | self.group('slip', True) 86 | self.radialVelocity = sp.Register () 87 | 88 | self.group ('camera') 89 | 90 | self.soccerViewOneshot = sp.Oneshot () 91 | self.heliViewOneshot = sp.Oneshot () 92 | self.driverViewOneshot = sp.Oneshot () 93 | 94 | self.driverFocusX = sp.Register () 95 | self.driverFocusY = sp.Register () 96 | 97 | def sweep (self): 98 | self.page ('car physics') 99 | 100 | self.group ('speed') 101 | 102 | self.velocity.set (self.velocity + self.acceleration * sp.world.period, self.velocity < self.targetVelocity, self.velocity - self.acceleration * sp.world.period) 103 | self.midWheelAngularVelocity.set (self.velocity / dm.displacementPerWheelAngle) 104 | self.midWheelAngle.set (self.midWheelAngle + self.midWheelAngularVelocity * sp.world.period) 105 | self.tangentialVelocity.set (self.midWheelAngularVelocity * dm.displacementPerWheelAngle) 106 | 107 | self.group ('direction') 108 | 109 | self.midSteeringAngle.set (sp.atan (0.5 * sp.tan (self.steeringAngle))) 110 | 111 | self.inverseMidCurveRadius.set (sp.sin (self.midSteeringAngle) / dm.wheelShift) 112 | self.midAngularVelocity.set (sp.degreesPerRadian * self.tangentialVelocity * self.inverseMidCurveRadius) 113 | 114 | self.attitudeAngle.set (self.attitudeAngle + self.midAngularVelocity * sp.world.period) 115 | self.courseAngle.set (self.attitudeAngle + self.midSteeringAngle) 116 | 117 | self.radialAcceleration.set (sp.max (abs (self.tangentialVelocity * self.tangentialVelocity * self.inverseMidCurveRadius) - 0.5, 0)) 118 | self.slipping.mark (sp.abs (self.radialAcceleration) > 0.55) 119 | self.radialVelocity.set (self.radialVelocity + self.radialAcceleration * sp.world.period, self.slipping, 0) 120 | 121 | self.velocityX.set (self.tangentialVelocity * sp.cos (self.courseAngle) + self.radialVelocity * sp.sin (self.courseAngle)) 122 | self.velocityY.set (self.tangentialVelocity * sp.sin (self.courseAngle) + self.radialVelocity * sp.cos (self.courseAngle)) 123 | 124 | self.group ('position') 125 | 126 | self.positionX.set (self.positionX + self.velocityX * sp.world.period) 127 | self.positionY.set (self.positionY + self.velocityY * sp.world.period) 128 | 129 | self.group ('camera') 130 | 131 | self.soccerView.unlatch (self.heliViewOneshot or self.driverViewOneshot) 132 | self.heliView.unlatch (self.soccerViewOneshot or self.driverViewOneshot) 133 | self.driverView.unlatch (self.soccerViewOneshot or self.heliViewOneshot) 134 | 135 | self.soccerViewOneshot.trigger (self.soccerView) 136 | self.heliViewOneshot.trigger (self.heliView) 137 | self.driverViewOneshot.trigger (self.driverView) 138 | 139 | self.driverFocusX.set (self.positionX + self.driverFocusDist * sp.cos (self.attitudeAngle)) 140 | self.driverFocusY.set (self.positionY + self.driverFocusDist * sp.sin (self.attitudeAngle)) 141 | -------------------------------------------------------------------------------- /simpylc/simulations/car/socket_wrapper.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import socket as sc 29 | import json as js 30 | 31 | address = 'localhost', 50012 32 | socketType = sc.AF_INET, sc.SOCK_STREAM 33 | maxNrOfConnectionRequests = 5 34 | maxMessageLength = 2048 35 | 36 | class SocketWrapper: 37 | def __init__ (self, clientSocket): 38 | self.clientSocket = clientSocket 39 | 40 | def send (self, anObject): 41 | buffer = bytes (f'{js.dumps (anObject):<{maxMessageLength}}', 'ascii') 42 | 43 | totalNrOfSentBytes = 0 44 | 45 | while totalNrOfSentBytes < maxMessageLength: 46 | nrOfSentBytes = self.clientSocket.send (buffer [totalNrOfSentBytes:]) 47 | 48 | if not nrOfSentBytes: 49 | self.raiseConnectionError () 50 | 51 | totalNrOfSentBytes += nrOfSentBytes 52 | 53 | def recv (self): 54 | totalNrOfReceivedBytes = 0 55 | receivedChunks = [] 56 | 57 | while totalNrOfReceivedBytes < maxMessageLength: 58 | receivedChunk = self.clientSocket.recv (maxMessageLength - totalNrOfReceivedBytes) 59 | 60 | if not receivedChunk: 61 | self.raiseConnectionError () 62 | 63 | receivedChunks.append (receivedChunk) 64 | totalNrOfReceivedBytes += len (receivedChunk) 65 | 66 | return js.loads (b''.join (receivedChunks) .decode ('ascii')) 67 | 68 | def raiseConnectionError (self): 69 | raise RuntimeError ('Socket connection broken') 70 | -------------------------------------------------------------------------------- /simpylc/simulations/car/sonar.track: -------------------------------------------------------------------------------- 1 | - - - - - - - - - - - - - * * * - - - - - - - - - - - - - - - - 2 | - - - - - - - * * * * * * - - - * * * - - - - - - - - - - - - - 3 | - - - - - - * - - - - - - - - - - - - * * * * - - - - - - - - - 4 | - - - - - * - - - - - - - - - - - - - - - - - * - - - - - - - - 5 | - - - - * - - - - - - - - * * * - - - - @ - - - * - - - - - - - 6 | - - - * - - - - * * * * * - - - * * * - - - - - - * - - - - - - 7 | - - * - - - - * - - - - - - - - - - - * * * - - - - * - - - - - 8 | - * - - - - * - - - - - - - - - - - - - - - * - - - - * - - - - 9 | * - - - - * - - - - - - - - - - - - - - - - - * - - - - * - - - 10 | * - - - * - - - - - - - - - - - - - - - - - - - * - - - - * - - 11 | * - - - * - - - - - - - - - - - - - - - - - - - - * - - - - * - 12 | * - - - * - - - - - - - - - - - - - - - - - - - - - * - - - * - 13 | - * - - - * - - - - - - - - - - - - - - - - - - - - * - - - * - 14 | - * - - - * - - - - - - - - - - - - - - - - - - - - - * - - - * 15 | - * - - - * - - - - - - - - - - - - - - - - - - - - - * - - - * 16 | - - * - - - * - - - - - - - - - - - - - - - - - - - - * - - - * 17 | - - * - - - * - - - - - - - - - - - - - - - - - - - - * - - - * 18 | - - * - - - * - - - - - - - - - - - - - - - - - - - - * - - - * 19 | - * - - - * - - - - - - - - - - - - - - - - - - - - * - - - * - 20 | - * - - - * - - - - - - - - - - - - - - - - - - - * - - - * - - 21 | - * - - - * - - - - - - - - - - - - - - - - - - * - - - -*- - - 22 | * - - - * - - - - - - - - - - - - - - - - - - * - - - - * - - - 23 | * - - - * - - - - - - - - - - - - - - - - - * - - - - * - - - - 24 | * - - - * - - - - - - - - - - * * * * * * * - - - - * - - - - - 25 | - * - - - * - - - - - - - - * - - - - - - - - - - * - - - - - - 26 | - * - - - * - - - - - - - * - - - - - - - - - - * - - - - - - - 27 | - * - - - - * - - - - - * - - - - - - - - - - * - - - - - - - - 28 | - - * - - - - * - - - * - - - - * * * * * * * - - - - - - - - - 29 | - - - * - - - - * * * - - - - * - - - - - - - - - - - - - - - - 30 | - - - - * - - - - - - - - - * - - - - - - - - - - - - - - - - - 31 | - - - - - * - - - - - - - * - - - - - - - - - - - - - - - - - - 32 | - - - - - - * * * * * * * - - - - - - - - - - - - - - - - - - - 33 | -------------------------------------------------------------------------------- /simpylc/simulations/car/visualisation.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | ''' 29 | 30 | z 31 | | 32 | o -- y 33 | / 34 | x 35 | 36 | ''' 37 | 38 | import random as rd 39 | import os 40 | 41 | import simpylc as sp 42 | 43 | import dimensions as dm 44 | 45 | normalFloorColor = (0, 0.001, 0) 46 | collisionFloorColor = (1, 0, 0.3) 47 | normalTireColor = (0.03, 0.03, 0.03) 48 | 49 | class Scanner (sp.Cylinder): 50 | def __init__ (self, apertureAngle, middleApertureAngle, obstacles, **arguments): 51 | super () .__init__ (color = (0, 0, 0), **arguments) 52 | 53 | self.apertureAngle = apertureAngle 54 | self.halfApertureAngle = self.apertureAngle // 2 55 | 56 | self.middleApertureAngle = middleApertureAngle 57 | self.halfMiddleApertureAngle = self.middleApertureAngle // 2 58 | 59 | self.obstacles = obstacles 60 | 61 | if scannerType == 'lidar': 62 | self.lidarDistances = [sp.finity for angle in range (self.apertureAngle)] # 0, ..., halfApertureAngle - 1, -halfApertureAngle, ..., -1 63 | else: 64 | self.sonarDistances = [sp.finity for sectorIndex in range (3)] 65 | 66 | def scan (self, mountPosition, mountAngle): 67 | if scannerType == 'lidar': 68 | self.lidarDistances = [sp.finity for angle in range (self.apertureAngle)] 69 | else: 70 | self.sonarDistances = [sp.finity for sectorIndex in range (3)] 71 | 72 | for obstacle in self.obstacles: 73 | relativePosition = sp.tSub (obstacle.center, mountPosition) 74 | distance = sp.tNor (relativePosition) 75 | absoluteAngle = sp.atan2 (relativePosition [1], relativePosition [0]) 76 | relativeAngle = (round (absoluteAngle - mountAngle) + 180) % 360 - 180 77 | 78 | if -self.halfApertureAngle <= relativeAngle < self.halfApertureAngle - 1: # In case of coincidence, favor nearby obstacle 79 | if scannerType == 'lidar': 80 | self.lidarDistances [relativeAngle] = round (min (distance, self.lidarDistances [relativeAngle]), 4) 81 | else: 82 | sectorIndex = ( 83 | -1 84 | if relativeAngle < -self.halfMiddleApertureAngle else 85 | 0 86 | if relativeAngle < self.halfMiddleApertureAngle else 87 | 1 88 | ) 89 | 90 | self.sonarDistances [sectorIndex] = round (min (distance, self.sonarDistances [sectorIndex]), 4) 91 | 92 | class Line (sp.Cylinder): 93 | def __init__ (self, **arguments): 94 | super () .__init__ (size = (0.01, 0.01, 0), axis = (1, 0, 0), angle = 90, color = (0, 1, 1), **arguments) 95 | 96 | class BodyPart (sp.Beam): 97 | def __init__ (self, **arguments): 98 | super () .__init__ (color = (0.7, 0, 0), **arguments) 99 | 100 | class Wheel: 101 | def __init__ (self, **arguments): 102 | self.suspension = sp.Cylinder (size = (0.01, 0.01, 0.001), axis = (1, 0, 0), angle = 90, pivot = (0, 0, 1), **arguments) 103 | self.rim = sp.Beam (size = (0.08, 0.06, 0.02), pivot = (0, 1, 0), color = (0.2, 0, 0)) 104 | self.tire = sp.Cylinder (size = (dm.wheelDiameter, dm.wheelDiameter, 0.04), axis = (1, 0, 0), angle = 90, color = normalTireColor) 105 | self.line = Line () 106 | 107 | def __call__ (self, wheelAngle, slipping, steeringAngle = 0): 108 | return self.suspension (rotation = steeringAngle, parts = lambda: 109 | self.rim (rotation = wheelAngle, parts = lambda: 110 | self.tire (color = (rd.random (), 0.5 * rd.random (), 0.5 * rd.random ()) if slipping else normalTireColor) + 111 | self.line () 112 | ) ) 113 | 114 | class Window (sp.Beam): 115 | def __init__ (self, **arguments): 116 | super () .__init__ (axis = (0, 1, 0), color = (0, 0, 0.2), **arguments) 117 | 118 | class Floor (sp.Beam): 119 | side = 16 120 | spacing = 0.5 121 | halfSteps = round (0.5 * side / spacing) 122 | 123 | class Stripe (sp.Beam): 124 | def __init__ (self, **arguments): 125 | super () .__init__ (size = (0.004, Floor.side, 0.001), **arguments) 126 | 127 | def __init__ (self, **arguments): 128 | super () .__init__ (size = (self.side, self.side, 0.0005), color = normalFloorColor) 129 | self.xStripes = [self.Stripe (center = (0, nr * self.spacing, 0.0001), angle = 90, color = (0, 0.004, 0)) for nr in range (-self.halfSteps, self.halfSteps)] 130 | self.yStripes = [self.Stripe (center = (nr * self.spacing, 0, 0), color = (0, 0.004, 0)) for nr in range (-self.halfSteps, self.halfSteps)] 131 | 132 | def __call__ (self, parts): 133 | return super () .__call__ (color = collisionFloorColor if self.scene.collided else normalFloorColor, parts = lambda: 134 | parts () + 135 | sum (xStripe () for xStripe in self.xStripes) + 136 | sum (yStripe () for yStripe in self.yStripes) 137 | ) 138 | 139 | class Visualisation (sp.Scene): 140 | def __init__ (self): 141 | super () .__init__ () 142 | self.roadCones = [] 143 | trackFileName = 'lidar.track' if scannerType == 'lidar' else 'sonar.track' 144 | 145 | with open (f'{os.path.dirname (os.path.abspath (__file__))}/{trackFileName}') as trackFile: 146 | track = trackFile.readlines () 147 | 148 | for rowIndex, row in enumerate (track): 149 | for columnIndex, column in enumerate (row): 150 | if column == '*': 151 | self.roadCones.append (sp.Cone ( 152 | size = (0.07, 0.07, 0.15), 153 | center = (columnIndex / 4 - 7.75, rowIndex / 2 - 7.75, 0.15), 154 | color = (1, 0.3, 0), 155 | group = 1 156 | )) 157 | elif column == "@": 158 | self.startX = columnIndex / 4 - 8 159 | self.startY = rowIndex / 2 - 8 160 | self.init = True 161 | 162 | self.camera = sp.Camera () 163 | 164 | self.floor = Floor (scene = self) 165 | 166 | self.fuselage = BodyPart (size = (0.65, 0.165, 0.09), center = (0, 0, 0.07), pivot = (0, 0, 1), group = 0) 167 | self.fuselageLine = Line () 168 | 169 | self.wheelFrontLeft = Wheel (center = (dm.wheelShift, 0.08, -0.02)) 170 | self.wheelFrontRight = Wheel (center = (dm.wheelShift, -0.08, -0.02)) 171 | 172 | self.wheelRearLeft = Wheel (center = (-dm.wheelShift, 0.08, -0.02)) 173 | self.wheelRearRight = Wheel (center = (-dm.wheelShift, -0.08, -0.02)) 174 | 175 | self.cabin = BodyPart (size = (0.20, 0.16, 0.06), center = (-0.06, 0, 0.07)) 176 | self.windowFront = Window (size = (0.045, 0.158, 0.14), center = (0.15, 0, -0.025), angle = -56) 177 | self.windowRear = Window (size = (0.042, 0.158, 0.18), center = (-0.18, 0, -0.025),angle = 72) 178 | 179 | self.scanner = Scanner (dm.apertureAngle, dm.middleApertureAngle, self.roadCones, size = (0.02, 0.02, 0.03), center = (0.05, 0, 0.03)) 180 | 181 | def display (self): 182 | if self.init: 183 | self.init = False 184 | sp.world.physics.positionX.set (self.startX) 185 | sp.world.physics.positionY.set (self.startY) 186 | 187 | if sp.world.physics.soccerView: 188 | self.camera ( 189 | position = sp.tEva ((sp.world.physics.positionX + 2, sp.world.physics.positionY, 2)), 190 | focus = sp.tEva ((sp.world.physics.positionX + 0.001, sp.world.physics.positionY, 0)) 191 | ) 192 | elif sp.world.physics.heliView: 193 | self.camera ( 194 | position = sp.tEva ((0.0000001, 0, 20)), 195 | focus = sp.tEva ((0, 0, 0)) 196 | ) 197 | elif sp.world.physics.driverView: 198 | self.camera ( 199 | position = sp.tEva ((sp.world.physics.positionX, sp.world.physics.positionY, 1)), 200 | focus = sp.tEva ((sp.world.physics.driverFocusX, sp.world.physics.driverFocusY, 0)) 201 | ) 202 | 203 | self.floor (parts = lambda: 204 | self.fuselage (position = (sp.world.physics.positionX, sp.world.physics.positionY, 0), rotation = sp.world.physics.attitudeAngle, parts = lambda: 205 | self.cabin (parts = lambda: 206 | self.windowFront () + 207 | self.windowRear () + 208 | self.scanner () 209 | ) + 210 | 211 | self.wheelFrontLeft ( 212 | wheelAngle = sp.world.physics.midWheelAngle, 213 | slipping = sp.world.physics.slipping, 214 | steeringAngle = sp.world.physics.steeringAngle 215 | ) + 216 | self.wheelFrontRight ( 217 | wheelAngle = sp.world.physics.midWheelAngle, 218 | slipping = sp.world.physics.slipping, 219 | steeringAngle = sp.world.physics.steeringAngle 220 | ) + 221 | 222 | self.wheelRearLeft ( 223 | wheelAngle = sp.world.physics.midWheelAngle, 224 | slipping = sp.world.physics.slipping 225 | ) + 226 | self.wheelRearRight ( 227 | wheelAngle = sp.world.physics.midWheelAngle, 228 | slipping = sp.world.physics.slipping 229 | ) + 230 | 231 | self.fuselageLine () 232 | ) + 233 | 234 | sum (roadCone () for roadCone in self.roadCones) 235 | ) 236 | 237 | if hasattr (self.fuselage, 'position'): 238 | self.scanner.scan (self.fuselage.position, self.fuselage.rotation) 239 | -------------------------------------------------------------------------------- /simpylc/simulations/car/world.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import os 29 | import sys as ss 30 | import time as tm 31 | 32 | ss.path += [os.path.abspath (relPath) for relPath in ('../../..', '..')] # If you want to store your simulations somewhere else, put SimPyLC in your PYTHONPATH environment variable 33 | scannerType = 'lidar' if input ('Lidar or sonar : ') == 'l' else 'sonar' # Should be done prior to any SimPyLC related imports due to concurrency 34 | 35 | import simpylc as sp 36 | 37 | import control_server as cs 38 | import physics as ps 39 | import visualisation as vs 40 | 41 | vs.scannerType = scannerType 42 | 43 | sp.World ( 44 | cs.ControlServer, 45 | ps.Physics, 46 | vs.Visualisation 47 | ) 48 | -------------------------------------------------------------------------------- /simpylc/simulations/one_armed_robot/control.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | class Control (sp.Module): 31 | def __init__ (self): 32 | sp.Module.__init__ (self) 33 | 34 | self.page ('movement control') 35 | 36 | self.group ('torso drive control', True) 37 | self.torVoltFac = sp.Register (0.8) 38 | self.torVoltMax = sp.Register (10) 39 | self.torVolt = sp.Register () 40 | self.torEnab = sp.Marker () 41 | 42 | self.group ('torso angle') 43 | self.torAngSet = sp.Register () 44 | self.torAng = sp.Register () 45 | self.torAngOld = sp.Register () 46 | self.torAngDif = sp.Register () 47 | self.torMarg = sp.Register (15) 48 | self.torRound = sp.Marker () 49 | self.torSpeedFac = sp.Register (0.5) 50 | self.torSpeedMax = sp.Register (20) 51 | self.torSpeedSet = sp.Register() 52 | self.torSpeed = sp.Register () 53 | self.torSpeedDif = sp.Register () 54 | 55 | self.group ('general') 56 | self.go = sp.Marker () 57 | 58 | self.group ('upper arm drive control', True) 59 | self.uppVoltFac = sp.Register (0.25) 60 | self.uppVoltMax = sp.Register (10) 61 | self.uppVolt = sp.Register () 62 | self.uppEnab = sp.Marker () 63 | 64 | self.group ('upper arm angle') 65 | self.uppAngSet = sp.Register () 66 | self.uppAng = sp.Register () 67 | self.uppAngOld = sp.Register () 68 | self.uppAngDif = sp.Register () 69 | self.uppMarg = sp.Register (15) 70 | self.uppRound = sp.Marker () 71 | self.uppSpeedFac = sp.Register (0.5) 72 | self.uppSpeedMax = sp.Register (20) 73 | self.uppSpeedSet = sp.Register () 74 | self.uppSpeed = sp.Register () 75 | self.uppSpeedDif = sp.Register () 76 | 77 | self.group ('fore arm drive control', True) 78 | self.forVoltFac = sp.Register (0.25) 79 | self.forVoltMax = sp.Register (10) 80 | self.forVolt = sp.Register () 81 | self.forEnab = sp.Marker () 82 | 83 | self.group ('fore arm angle') 84 | self.forAngSet = sp.Register () 85 | self.forAng = sp.Register () 86 | self.forAngOld = sp.Register () 87 | self.forAngDif = sp.Register () 88 | self.forMarg = sp.Register (15) 89 | self.forRound = sp.Marker () 90 | self.forSpeedFac = sp.Register (0.5) 91 | self.forSpeedMax = sp.Register (20) 92 | self.forSpeedSet = sp.Register () 93 | self.forSpeed = sp.Register () 94 | self.forSpeedDif = sp.Register () 95 | 96 | self.group ('wrist drive control', True) 97 | self.wriVoltFac = sp.Register (0.25) 98 | self.wriVoltMax = sp.Register (10) 99 | self.wriVolt = sp.Register () 100 | self.wriEnab = sp.Marker () 101 | 102 | self.group ('wrist angle') 103 | self.wriAngSet = sp.Register () 104 | self.wriAng = sp.Register () 105 | self.wriAngOld = sp.Register () 106 | self.wriAngDif = sp.Register () 107 | self.wriMarg = sp.Register (3) 108 | self.wriRound = sp.Marker () 109 | self.wriSpeedFac = sp.Register (0.5) 110 | self.wriSpeedMax = sp.Register (20) 111 | self.wriSpeedSet = sp.Register () 112 | self.wriSpeed = sp.Register () 113 | self.wriSpeedDif = sp.Register () 114 | 115 | self.group ('hand and fingers setpoints', True) 116 | self.hanAngSet = sp.Register () 117 | self.hanEnab = sp.Marker () 118 | self.finAngSet = sp.Register () 119 | self.finEnab = sp.Marker () 120 | self.finDelay = sp.Register (1) 121 | self.finTimer = sp.Timer () 122 | self.finLatch = sp.Latch () 123 | 124 | self.group ('sweep time measurement') 125 | self.sweepMin = sp.Register (1000) 126 | self.sweepMax = sp.Register () 127 | self.sweepWatch = sp.Timer () 128 | self.run = sp.Runner () 129 | 130 | def input (self): 131 | self.part ('true angles') 132 | self.torAng.set (sp.world.robot.torAng) 133 | self.uppAng.set (sp.world.robot.uppAng) 134 | self.forAng.set (sp.world.robot.forAng) 135 | self.wriAng.set (sp.world.robot.wriAng) 136 | 137 | def sweep (self): 138 | self.part ('torso') 139 | self.torAngDif.set (self.torAngSet - self.torAng) 140 | self.torRound.mark (sp.abs (self.torAngDif) < self.torMarg) 141 | self.torSpeedSet.set (sp.limit (self.torSpeedFac * self.torAngDif, self.torSpeedMax)) 142 | self.torSpeed.set ((self.torAng - self.torAngOld) / sp.world.period) 143 | self.torSpeedDif.set (self.torSpeedSet - self.torSpeed) 144 | self.torVolt.set (sp.limit (self.torVoltFac * self.torSpeedDif, self.torVoltMax)) 145 | self.torEnab.mark (self.go) 146 | self.torAngOld.set (self.torAng) 147 | 148 | self.part ('upper arm') 149 | self.uppAngDif.set (self.uppAngSet - self.uppAng) 150 | self.uppRound.mark (sp.abs (self.uppAngDif) < self.uppMarg) 151 | self.uppSpeedSet.set (sp.limit (self.uppSpeedFac * self.uppAngDif, self.uppSpeedMax)) 152 | self.uppSpeed.set ((self.uppAng - self.uppAngOld) / sp.world.period) 153 | self.uppSpeedDif.set (self.uppSpeedSet - self.uppSpeed) 154 | self.uppVolt.set (sp.limit (self.uppVoltFac * self.uppSpeedDif, self.uppVoltMax)) 155 | self.uppEnab.mark (self.go and self.torRound) 156 | self.uppAngOld.set (self.uppAng) 157 | 158 | self.part ('fore arm') 159 | self.forAngDif.set (self.forAngSet - self.forAng) 160 | self.forRound.mark (sp.abs (self.forAngDif) < self.forMarg) 161 | self.forSpeedSet.set (sp.limit (self.forSpeedFac * self.forAngDif, self.forSpeedMax)) 162 | self.forSpeed.set ((self.forAng - self.forAngOld) / sp.world.period) 163 | self.forSpeedDif.set (self.forSpeedSet - self.forSpeed) 164 | self.forVolt.set (sp.limit (self.forVoltFac * self.forSpeedDif, self.forVoltMax)) 165 | self.forEnab.mark (self.go and self.torRound and self.uppRound) 166 | self.forAngOld.set (self.forAng) 167 | 168 | self.part ('wrist') 169 | self.wriAngDif.set (self.wriAngSet - self.wriAng) 170 | self.wriRound.mark (sp.abs (self.wriAngDif) < self.wriMarg) 171 | self.wriSpeedSet.set (sp.limit (self.wriSpeedFac * self.wriAngDif, self.wriSpeedMax)) 172 | self.wriSpeed.set ((self.wriAng - self.wriAngOld) / sp.world.period) 173 | self.wriSpeedDif.set (self.wriSpeedSet - self.wriSpeed) 174 | self.wriVolt.set (sp.limit (self.wriVoltFac * self.wriSpeedDif, self.wriVoltMax)) 175 | self.wriEnab.mark (self.go and self.torRound and self.uppRound and self.forRound) 176 | self.wriAngOld.set (self.wriAng) 177 | 178 | self.part ('hand and fingers') 179 | self.hanEnab.mark (self.go and self.torRound and self.uppRound and self.forRound and self.wriRound) 180 | self.finTimer.reset (not self.hanEnab) 181 | self.finEnab.mark (self.finTimer > self.finDelay) 182 | self.finLatch.latch (self.finTimer > 0.01) 183 | 184 | self.part ('sweep time measurement') 185 | self.sweepMin.set (sp.world.period, sp.world.period < self.sweepMin) 186 | self.sweepMax.set (sp.world.period, sp.world.period > self.sweepMax) 187 | self.sweepWatch.reset (self.sweepWatch > 2) 188 | self.sweepMin.set (1000, not self.sweepWatch) 189 | self.sweepMax.set (0, not self.sweepWatch) 190 | 191 | -------------------------------------------------------------------------------- /simpylc/simulations/one_armed_robot/robot.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | class Robot (sp.Module): 31 | def __init__ (self): 32 | sp.Module.__init__ (self) 33 | 34 | self.page ('robot physics') 35 | 36 | self.group ('torso electronics', True) 37 | self.torVolt = sp.Register () 38 | self.torEnab = sp.Marker () 39 | self.torGain = sp.Register (2) 40 | self.torMax = sp.Register (20) 41 | 42 | self.group ('torso mechanics') 43 | self.torInert = sp.Register (8) 44 | self.torTorq = sp.Register () 45 | self.torBrake = sp.Marker () 46 | self.torAccel = sp.Register () 47 | self.torSpeed = sp.Register () 48 | self.torAng = sp.Register () 49 | 50 | self.group ('upper arm electronics', True) 51 | self.uppVolt = sp.Register () 52 | self.uppEnab = sp.Marker () 53 | self.uppGain = sp.Register (2) 54 | self.uppMax = sp.Register (20) 55 | 56 | self.group ('upper arm mechanics') 57 | self.uppInert = sp.Register (4) 58 | self.uppTorq = sp.Register () 59 | self.uppBrake = sp.Marker () 60 | self.uppAccel = sp.Register () 61 | self.uppSpeed = sp.Register () 62 | self.uppAng = sp.Register () 63 | 64 | self.group ('fore arm electronics', True) 65 | self.forVolt = sp.Register () 66 | self.forEnab = sp.Marker () 67 | self.forGain = sp.Register (2) 68 | self.forMax = sp.Register (20) 69 | 70 | self.group ('fore arm mechanics') 71 | self.forInert = sp.Register (2) 72 | self.forTorq = sp.Register () 73 | self.forBrake = sp.Marker () 74 | self.forAccel = sp.Register () 75 | self.forSpeed = sp.Register () 76 | self.forAng = sp.Register () 77 | self.forShift = sp.Register () 78 | 79 | self.group ('wrist electronics', True) 80 | self.wriVolt = sp.Register () 81 | self.wriEnab = sp.Marker () 82 | self.wriGain = sp.Register (2) 83 | self.wriMax = sp.Register (20) 84 | 85 | self.group ('wrist mechanics') 86 | self.wriInert = sp.Register (1) 87 | self.wriTorq = sp.Register () 88 | self.wriBrake = sp.Marker () 89 | self.wriAccel = sp.Register () 90 | self.wriSpeed = sp.Register () 91 | self.wriAng = sp.Register () 92 | 93 | self.group ('hand and finger servos', True) 94 | self.hanAngSet = sp.Register () 95 | self.hanAng = sp.Register () 96 | self.hanEnab = sp.Marker () 97 | self.finAngSet = sp.Register () 98 | self.finAng = sp.Register () 99 | self.finEnab = sp.Marker () 100 | 101 | def input (self): 102 | self.part ('torso') 103 | self.torVolt.set (sp.world.control.torVolt) 104 | self.torEnab.mark (sp.world.control.torEnab) 105 | 106 | self.part ('upper arm') 107 | self.uppVolt.set (sp.world.control.uppVolt) 108 | self.uppEnab.mark (sp.world.control.uppEnab) 109 | 110 | self.part ('fore arm') 111 | self.forVolt.set (sp.world.control.forVolt) 112 | self.forEnab.mark (sp.world.control.forEnab) 113 | 114 | self.part ('wrist') 115 | self.wriVolt.set (sp.world.control.wriVolt) 116 | self.wriEnab.mark (sp.world.control.wriEnab) 117 | 118 | self.part ('hand and fingers') 119 | self.hanAngSet.set (sp.world.control.hanAngSet) 120 | self.hanEnab.mark (sp.world.control.hanEnab) 121 | self.finAngSet.set (sp.world.control.finAngSet) 122 | self.finEnab.mark (sp.world.control.finEnab) 123 | 124 | def sweep (self): 125 | self.part ('Torso') 126 | self.torTorq.set (sp.limit (self.torGain * self.torVolt, self.torMax), self.torEnab, 0) 127 | self.torBrake.mark (not self.torEnab) 128 | self.torAccel.set (self.torSpeed / -sp.world.period, self.torBrake, self.torTorq / self.torInert) 129 | self.torSpeed.set (self.torSpeed + self.torAccel * sp.world.period) 130 | self.torAng.set (self.torAng + self.torSpeed * sp.world.period) 131 | 132 | self.part ('upper arm') 133 | self.uppTorq.set (sp.limit (self.uppGain * self.uppVolt, self.uppMax), self.uppEnab, 0) 134 | self.uppBrake.mark (not self.uppEnab) 135 | self.uppAccel.set (self.uppSpeed / -sp.world.period, self.uppBrake, self.uppTorq / self.uppInert) 136 | self.uppSpeed.set (self.uppSpeed + self.uppAccel * sp.world.period) 137 | self.uppAng.set (self.uppAng + self.uppSpeed * sp.world.period) 138 | 139 | self.part ('fore arm') 140 | self.forTorq.set (sp.limit (self.forGain * self.forVolt, self.forMax), self.forEnab, 0) 141 | self.forBrake.mark (not self.forEnab) 142 | self.forAccel.set (self.forSpeed / -sp.world.period, self.forBrake, self.forTorq / self.forInert) 143 | self.forSpeed.set (self.forSpeed + self.forAccel * sp.world.period) 144 | self.forAng.set (self.forAng + self.forSpeed * sp.world.period) 145 | 146 | self.part ('wrist') 147 | self.wriTorq.set (sp.limit (self.wriGain * self.wriVolt, self.wriMax), self.wriEnab, 0) 148 | self.wriBrake.mark (not self.wriEnab) 149 | self.wriAccel.set (self.wriSpeed / -sp.world.period, self.wriBrake, self.wriTorq / self.wriInert) 150 | self.wriSpeed.set (self.wriSpeed + self.wriAccel * sp.world.period) 151 | self.wriAng.set (self.wriAng + self.wriSpeed * sp.world.period) 152 | 153 | self.part ('hand and fingers') 154 | self.hanAng.set (self.hanAngSet, self.hanEnab) 155 | self.finAng.set (self.finAngSet, self.finEnab) 156 | 157 | -------------------------------------------------------------------------------- /simpylc/simulations/one_armed_robot/robot_plan.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/QQuick/SimPyLC/51bcb8052f06e11a2d9747bb7d0a26a59754deb7/simpylc/simulations/one_armed_robot/robot_plan.jpg -------------------------------------------------------------------------------- /simpylc/simulations/one_armed_robot/timing.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | class Timing (sp.Chart): 31 | def __init__ (self): 32 | sp.Chart.__init__ (self) 33 | 34 | def define (self): 35 | self.channel (sp.world.robot.torEnab, sp.white) 36 | self.channel (sp.world.robot.torVolt, sp.red, -10, 10, 20) 37 | self.channel (sp.world.robot.torAng, sp.red, -220, 220, 70) 38 | self.channel (sp.world.robot.torBrake, sp.red) 39 | 40 | self.channel (sp.world.robot.uppEnab, sp.white) 41 | self.channel (sp.world.robot.uppVolt, sp.lime, -10, 10, 20) 42 | self.channel (sp.world.robot.uppAng, sp.lime, -110, 110, 35) 43 | self.channel (sp.world.robot.uppBrake, sp.lime) 44 | 45 | self.channel (sp.world.robot.forEnab, sp.white) 46 | self.channel (sp.world.robot.forVolt, sp.blue, -10, 10, 20) 47 | self.channel (sp.world.robot.forAng, sp.blue, -110, 110, 35) 48 | self.channel (sp.world.robot.forBrake, sp.blue) 49 | 50 | self.channel (sp.world.robot.wriEnab, sp.white) 51 | self.channel (sp.world.robot.wriVolt, sp.yellow, -10, 10, 20) 52 | self.channel (sp.world.robot.wriAng, sp.yellow, -110, 110, 35) 53 | self.channel (sp.world.robot.wriBrake, sp.yellow) 54 | 55 | self.channel (sp.world.robot.hanEnab, sp.white) 56 | self.channel (sp.world.robot.hanAng, sp.aqua, -110, 110, 35) 57 | 58 | self.channel (sp.world.robot.finEnab, sp.white) 59 | self.channel (sp.world.robot.finAng, sp.fuchsia, -55, 55, 35) 60 | 61 | 62 | -------------------------------------------------------------------------------- /simpylc/simulations/one_armed_robot/visualisation.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | class Visualisation (sp.Scene): 31 | def __init__ (self): 32 | sp.Scene.__init__ (self) 33 | self.base = sp.Cylinder (size = (0.3, 0.3, 0.4), center = (0, 0, 0.2), pivot = (0, 0, 1), color = (1, 1, 0.2)) 34 | self.torso = sp.Beam (size = (0.4, 0.4, 0.6), center = (0, 0, 0.5), pivot = (0, 0, 1), color = (0.5, 0.5, 0.5)) 35 | 36 | armColor = (0.7, 0.7, 0.7) 37 | self.upperArm = sp.Beam (size = (1, 0.2, 0.2), center = (0.4, -0.3, 0.1), joint = (-0.4, 0, 0), pivot = (0, 1, 0), color = armColor) 38 | self.foreArm = sp.Beam (size = (0.7, 0.15, 0.15), center = (0.65, 0.175, 0), joint = (-0.25, 0, 0), pivot = (0, 1, 0), color = armColor) 39 | self.wrist = sp.Beam (size = (0.3, 0.1, 0.1), center = (0.40, -0.125, 0), joint = (-0.05, 0, 0), pivot = (0, 1, 0), color = armColor) 40 | 41 | handColor = (1, 0.01, 0.01) 42 | handSideSize = (0.1, 0.1, 0.1) 43 | self.handCenter = sp.Beam (size = (0.1, 0.09, 0.09), center = (0.15, 0, 0), pivot = (1, 0, 0), color = handColor) 44 | self.handSide0 = sp.Beam (size = handSideSize, center = (0, -0.075, -0.075), color = handColor) 45 | self.handSide1 = sp.Beam (size = handSideSize, center = (0, 0.075, -0.075), color = handColor) 46 | self.handSide2 = sp.Beam (size = handSideSize, center = (0, 0.075, 0.075), color = handColor) 47 | self.handSide3 = sp.Beam (size = handSideSize, center = (0, -0.075, 0.075), color = handColor) 48 | 49 | fingerColor = (0.01, 1, 0.01) 50 | fingerSize = (0.3, 0.05, 0.05) 51 | fingerJoint = (-0.125, 0, 0) 52 | self.finger0 = sp.Beam (size = fingerSize, center = (0.15, 0, -0.1), joint = fingerJoint, pivot = (0, -1, 0), color = fingerColor) 53 | self.finger1 = sp.Beam (size = fingerSize, center = (0.15, 0, 0.1), joint = fingerJoint, pivot = (0, 1, 0), color = fingerColor) 54 | self.finger2 = sp.Beam (size = fingerSize, center = (0.15, -0.1, 0), joint = fingerJoint, pivot = (0, 0, 1), color = fingerColor) 55 | self.finger3 = sp.Beam (size = fingerSize, center = (0.15, 0.1, 0), joint = fingerJoint, pivot = (0, 0, -1), color = fingerColor) 56 | 57 | def display (self): 58 | self.base (parts = lambda: 59 | self.torso (rotation = sp.world.robot.torAng, parts = lambda: 60 | self.upperArm (rotation = sp.world.robot.uppAng, parts = lambda: 61 | self.foreArm (rotation = sp.world.robot.forAng, shift = (sp.world.robot.forShift, 0, 0), parts = lambda: 62 | self.wrist (rotation = sp.world.robot.wriAng, parts = lambda: 63 | self.handCenter (rotation = sp.world.robot.hanAng, parts = lambda: 64 | self.handSide0 () + 65 | self.handSide1 () + 66 | self.handSide2 () + 67 | self.handSide3 () + 68 | self.finger0 (rotation = sp.world.robot.finAng) + 69 | self.finger1 (rotation = sp.world.robot.finAng) + 70 | self.finger2 (rotation = sp.world.robot.finAng) + 71 | self.finger3 (rotation = sp.world.robot.finAng) 72 | ) ) ) ) ) ) 73 | 74 | -------------------------------------------------------------------------------- /simpylc/simulations/one_armed_robot/world.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import os 29 | import sys as ss 30 | 31 | ss.path.append (os.path.abspath ('../..')) # If you want to store your simulations somewhere else, put SimPyLC in your PYTHONPATH environment variable 32 | 33 | import simpylc as sp 34 | import robot as rb 35 | import control as ct 36 | import visualisation as vs 37 | import timing as tm 38 | 39 | sp.World (ct.Control, rb.Robot, vs.Visualisation, tm.Timing) 40 | -------------------------------------------------------------------------------- /simpylc/simulations/pid_controller/native.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicence. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicence for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your licence. 26 | */ 27 | 28 | int const analogInPin = A3; 29 | int const analogOutPin = 2; 30 | 31 | void setup () { 32 | // pinMode (analogInPin, INPUT); 33 | pinMode (analogOutPin, OUTPUT); 34 | Serial.begin(9600); 35 | } 36 | 37 | void loop () { 38 | nActIn = analogRead (analogInPin); 39 | cycle (); 40 | analogWrite (analogOutPin, nOut); 41 | Serial.println (nActIn); 42 | } 43 | 44 | -------------------------------------------------------------------------------- /simpylc/simulations/pid_controller/pid_controller.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | class PidController (sp.Module): 31 | def __init__ (self): 32 | sp.Module.__init__ (self) 33 | 34 | self.page ('p i d controller') 35 | 36 | self.group ('inputs', True) 37 | self.uRefIn = sp.Register () 38 | self.uActIn = sp.Register () 39 | 40 | self.group ('outputs') 41 | self.uOut = sp.Register () 42 | 43 | self.group ('p i d settings') 44 | self.cProp = sp.Register () 45 | self.cInt = sp.Register (1) 46 | self.cDif = sp.Register () 47 | 48 | self.group ('raw correction terms', True) 49 | self.uCorIn = sp.Register () 50 | self.uCorIntIn = sp.Register () 51 | self.uCorDifIn = sp.Register () 52 | 53 | self.group ('scaled correction terms') 54 | self.uCorOut = sp.Register () 55 | self.uCorIntOut = sp.Register () 56 | self.uCorDifOut = sp.Register () 57 | 58 | self.group ('auxiliary', True) 59 | self.uMax = sp.Register (3.5) 60 | self.nMax = sp.Register (1024) 61 | self.uCorOldIn = sp.Register () 62 | self.nActIn = sp.Register () 63 | self.nOut = sp.Register () 64 | 65 | self.group ('physics') 66 | self.simulatePhysics = sp.Marker () 67 | self.transferFactor = sp.Register (1) 68 | 69 | def sweep (self): 70 | self.uRefIn.set (self.uMax / 2) 71 | self.nActIn.set (self.transferFactor * self.nOut, self.simulatePhysics) 72 | self.uActIn.set (self.nActIn * self.uMax / self.nMax) 73 | 74 | self.uCorOldIn.set (self.uCorIn) 75 | self.uCorIn.set (self.uRefIn - self.uActIn) 76 | self.uCorIntIn.set (sp.limit (self.uCorIntIn + self.uCorIn * sp.world.period, self.uMax)) 77 | self.uCorDifIn.set ((self.uCorIn - self.uCorOldIn) / sp.world.period) 78 | 79 | self.uCorOut.set (self.cProp * self.uCorIn) 80 | self.uCorIntOut.set (self.cInt * self.uCorIntIn) 81 | self.uCorDifOut.set (self.cDif * self.uCorDifIn) 82 | 83 | self.uOut.set (self.uCorOut + self.uCorIntOut + self.uCorDifOut) 84 | self.nOut.set (self.uOut * self.nMax / self.uMax) -------------------------------------------------------------------------------- /simpylc/simulations/pid_controller/timing.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | absRange = 5 31 | absHeight = 75 32 | 33 | class Timing (sp.Chart): 34 | def __init__ (self): 35 | sp.Chart.__init__ (self) 36 | 37 | def define (self): 38 | self.channel (sp.world.pidController.uRefIn, sp.lime, 0, absRange, absHeight) 39 | self.channel (sp.world.pidController.uActIn, sp.lime, 0, absRange, absHeight) 40 | 41 | self.channel (sp.world.pidController.uCorOut, sp.white, -absRange, absRange, 2 * absHeight) 42 | self.channel (sp.world.pidController.uCorIntOut, sp.white, -absRange, absRange, 2 * absHeight) 43 | self.channel (sp.world.pidController.uCorDifOut, sp.white, -absRange, absRange, 2 *absHeight) 44 | 45 | self.channel (sp.world.pidController.uOut, sp.lime, 0, absRange, absHeight) 46 | -------------------------------------------------------------------------------- /simpylc/simulations/pid_controller/world.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import os 29 | import sys as ss 30 | 31 | ss.path.append (os.path.abspath ('../..')) # If you want to store your simulations somewhere else, put SimPyLC in your PYTHONPATH environment variable 32 | 33 | import simpylc as sp 34 | import pid_controller as pc 35 | import timing as tm 36 | 37 | sp.World (pc.PidController, tm.Timing) 38 | -------------------------------------------------------------------------------- /simpylc/simulations/rocket/common.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | useQuaternions = True 31 | useGramSchmidt = True # Only matters if useQuaternions == False 32 | 33 | g = 10 34 | 35 | earthMoonDist = 500 36 | 37 | earthDiam = 50 38 | earthMass = 8e7 39 | 40 | moonDiam = 15 41 | moonMass = 1e6 42 | 43 | # Gravity made proportional to r^-0.5 instead of r^-2 to get a more "telling" simulation 44 | 45 | gamma = g * (earthDiam / 2) * (earthDiam / 2) / earthMass 46 | 47 | def getGravVec (mass0, mass1, diam, relPos): 48 | relPos = sp.tEva (relPos) 49 | dist = sp.tNor (relPos) 50 | factor = -1 if dist > diam / 2 else 0.1 51 | return sp.tsMul (sp.tUni (relPos), factor * gamma * mass0 * mass1 / (dist * dist)) 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /simpylc/simulations/rocket/control.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | class Control (sp.Module): 31 | def __init__ (self): 32 | sp.Module.__init__ (self) 33 | 34 | self.page ('rocket control') 35 | 36 | self.group ('gimbal angle controls blue/yellow', True) 37 | self.toYellow = sp.Marker () 38 | self.toBlue = sp.Marker () 39 | 40 | self.group ('gimbal angle state blue/yellow') 41 | self.blueYellowDelta = sp.Register () 42 | self.blueYellowAngle = sp.Register () 43 | 44 | self.group ('thruster angle controls green/red', True) 45 | self.toRed = sp.Marker () 46 | self.toGreen = sp.Marker () 47 | 48 | self.group ('thruster angle state green/red') 49 | self.greenRedDelta = sp.Register () 50 | self.greenRedAngle = sp.Register () 51 | 52 | self.group ('fuel throttle controls', True) 53 | self.throttleOpen = sp.Marker () 54 | self.throttleClose = sp.Marker () 55 | 56 | self.group ('fuel throttle state') 57 | self.throttleDelta = sp.Register () 58 | self.throttlePercent = sp.Register () 59 | 60 | def input (self): 61 | self.part ('gimbal angle blue/yellow') 62 | self.blueYellowAngle.set (sp.world.rocket.blueYellowAngle) 63 | 64 | self.part ('thruster angle green/red') 65 | self.greenRedAngle.set (sp.world.rocket.greenRedAngle) 66 | 67 | self.part ('fuel throttle') 68 | self.throttlePercent.set (sp.world.rocket.throttlePercent) 69 | 70 | def sweep (self): 71 | self.part ('gimbal angle blue/yellow') 72 | self.blueYellowDelta.set (-1 if self.toBlue else 1 if self.toYellow else 0) 73 | 74 | self.part ('thruster angle green/red') 75 | self.greenRedDelta.set (-1 if self.toGreen else 1 if self.toRed else 0) 76 | 77 | self.part ('fuel throttle') 78 | self.throttleDelta.set (-1 if self.throttleClose else 1 if self.throttleOpen else 0) 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /simpylc/simulations/rocket/rocket.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import numpy as np 29 | 30 | import simpylc as sp 31 | 32 | import common as cm 33 | 34 | import transforms as tf 35 | 36 | # General remarks: 37 | # - The physical reality of precession indicates that rotation matrix cannot be 38 | # found by applying angular acceleration in x, y and z direction successively. 39 | # - To avoid gimbal lock, non-unique Euler angles and numerical instability of 40 | # (modified) Gram Schmidt, the use of quaternions seems the simplest way to go. 41 | 42 | class Rocket (sp.Module): 43 | def __init__ (self): 44 | sp.Module.__init__ (self) 45 | 46 | self.page ('rocket physics') 47 | 48 | self.group ('gimbal angle blue/yellow', True) 49 | self.blueYellowDelta = sp.Register () 50 | self.blueYellowRoughAngle = sp.Register () 51 | self.blueYellowAngle = sp.Register () 52 | 53 | self.group ('thruster angle green/red') 54 | self.greenRedDelta = sp.Register () 55 | self.greenRedRoughAngle = sp.Register () 56 | self.greenRedAngle = sp.Register () 57 | 58 | self.group ('fuel throttle') 59 | self.throttleDelta = sp.Register () 60 | self.throttlePercent = sp.Register () 61 | self.thrust = sp.Register () 62 | 63 | self.group ('ship') 64 | self.shipMass = sp.Register (5000) 65 | self.effectiveRadius = sp.Register (0.15) 66 | self.effectiveHeight = sp.Register (1.5) 67 | self.thrusterTiltSpeed = sp.Register (30) 68 | self.thrusterMaxAngle = sp.Register (90) 69 | self.throttleSpeed = sp.Register (20) 70 | self.thrusterMaxForce = sp.Register (100000) 71 | 72 | self.group ('sweep time measurement') 73 | self.sweepMin = sp.Register (1000) 74 | self.sweepMax = sp.Register () 75 | self.sweepWatch = sp.Timer () 76 | self.run = sp.Runner () 77 | 78 | self.group ('linear accelleration', True) 79 | self.linAccelX = sp.Register () 80 | self.linAccelY = sp.Register () 81 | self.linAccelZ = sp.Register () 82 | 83 | self.group ('linear velocity') 84 | self.linVelocX = sp.Register () 85 | self.linVelocY = sp.Register () 86 | self.linVelocZ = sp.Register () 87 | 88 | self.group ('position') 89 | self.positionX = sp.Register () 90 | self.positionY = sp.Register () 91 | self.positionZ = sp.Register (cm.earthDiam / 2) 92 | 93 | self.group ('thrust in ship frame') 94 | self.forwardThrust = sp.Register () 95 | self.blueYellowThrust = sp.Register () 96 | self.greenRedThrust = sp.Register () 97 | 98 | self.group ('thrust in world frame') 99 | self.thrustX = sp.Register () 100 | self.thrustY = sp.Register () 101 | self.thrustZ = sp.Register () 102 | 103 | self.group ('angular acceleration', True) 104 | self.angAccelX = sp.Register () 105 | self.angAccelY = sp.Register () 106 | self.angAccelZ = sp.Register () 107 | 108 | self.group ('angular velocity') 109 | self.angVelocX = sp.Register () 110 | self.angVelocY = sp.Register () 111 | self.angVelocZ = sp.Register () 112 | 113 | self.group ('torques in ship frame') 114 | self.blueYellowTorque = sp.Register () 115 | self.greenRedTorque = sp.Register () 116 | 117 | self.group ('torques in world frame') 118 | self.torqueX = sp.Register () 119 | self.torqueY = sp.Register () 120 | self.torqueZ = sp.Register () 121 | 122 | if cm.useQuaternions: 123 | self._shipRotQuat = sp.quatFromAxAng (np.array ((1, 0, 0)), 0) 124 | 125 | self.group ('ship rotation quaternion') 126 | self.shipRotQuat0 = sp.Register () 127 | self.shipRotQuat1 = sp.Register () 128 | self.shipRotQuat2 = sp.Register () 129 | self.shipRotQuat3 = sp.Register () 130 | self._shipRotMat = sp.rotMatFromQuat (self._shipRotQuat) 131 | else: 132 | self._shipRotMat = np.array ([ # Columns are tangent (front), normal (up) and binormal (starboard) of ship 133 | [1, 0, 0], 134 | [0, 1, 0], 135 | [0, 0, 1] 136 | ]) 137 | 138 | self.group ('attitude') 139 | self.attitudeX = sp.Register () 140 | self.attitudeY = sp.Register () 141 | self.attitudeZ = sp.Register () 142 | 143 | self.group ('earth gravity', True) 144 | self.distEarthSurf = sp.Register () 145 | self.earthGravX = sp.Register () 146 | self.earthGravY = sp.Register () 147 | self.earthGravZ = sp.Register () 148 | 149 | self.group ('moon gravity') 150 | self.distMoonSurf = sp.Register () 151 | self.moonGravX = sp.Register () 152 | self.moonGravY = sp.Register () 153 | self.moonGravZ = sp.Register () 154 | 155 | self.group ('total force') 156 | self.totalForceX = sp.Register () 157 | self.totalForceY = sp.Register () 158 | self.totalForceZ = sp.Register () 159 | 160 | def input (self): 161 | self.part ('gimbal angle blue/yellow') 162 | self.blueYellowDelta.set (sp.world.control.blueYellowDelta) 163 | 164 | self.part ('thruster angle green/red') 165 | self.greenRedDelta.set (sp.world.control.greenRedDelta) 166 | 167 | self.part ('fuel throttle') 168 | self.throttleDelta.set (sp.world.control.throttleDelta) 169 | 170 | def sweep (self): 171 | self.part ('gimbal angle blue/yellow') 172 | self.blueYellowRoughAngle.set ( 173 | sp.limit ( 174 | self.blueYellowRoughAngle + self.blueYellowDelta * self.thrusterTiltSpeed * sp.world.period, 175 | self.thrusterMaxAngle 176 | ) 177 | ) 178 | self.blueYellowAngle.set (sp.snap (self.blueYellowRoughAngle, 0, 3)) 179 | 180 | self.part ('thruster angle green/red') 181 | self.greenRedRoughAngle.set ( 182 | sp.limit ( 183 | self.greenRedRoughAngle + self.greenRedDelta * self.thrusterTiltSpeed * sp.world.period, 184 | self.thrusterMaxAngle 185 | ) 186 | ) 187 | self.greenRedAngle.set (sp.snap (self.greenRedRoughAngle, 0, 3)) 188 | 189 | self.part ('fuel throttle') 190 | self.throttlePercent.set ( 191 | sp.limit ( 192 | self.throttlePercent + self.throttleDelta * self.throttleSpeed * sp.world.period, 193 | 0, 194 | 100 195 | ) 196 | ) 197 | self.thrust.set (self.throttlePercent * self.thrusterMaxForce / 100) 198 | 199 | self.part ('linear movement') 200 | 201 | thrusterForceVec = np.array ((0, 0, self.thrust ())) 202 | 203 | if cm.useQuaternions: 204 | thrusterRotQuat = sp.quatMul ( 205 | sp.quatFromAxAng (np.array ((1, 0, 0)), self.blueYellowAngle), 206 | sp.quatFromAxAng (np.array ((0, 1, 0)), -self.greenRedAngle) 207 | ) 208 | shipForceVec = sp.quatVecRot (thrusterRotQuat, thrusterForceVec) 209 | else: 210 | # Local coord sys, so "forward" order 211 | thrusterRotMat = tf.getRotXMat (self.blueYellowAngle) @ tf.getRotYMat (-self.greenRedAngle) 212 | shipForceVec = thrusterRotMat @ thrusterForceVec 213 | 214 | self.forwardThrust.set (shipForceVec [2]) 215 | self.blueYellowThrust.set (shipForceVec [1]) 216 | self.greenRedThrust.set (shipForceVec [0]) 217 | 218 | if cm.useQuaternions: 219 | worldForceVec = sp.quatVecRot (self._shipRotQuat, shipForceVec) 220 | else: 221 | worldForceVec = self._shipRotMat @ shipForceVec 222 | 223 | self.thrustX.set (worldForceVec [0]) 224 | self.thrustY.set (worldForceVec [1]) 225 | self.thrustZ.set (worldForceVec [2]) 226 | 227 | earthGravVec = cm.getGravVec (self.shipMass, cm.earthMass, cm.earthDiam, sp.tEva ((self.positionX, self.positionY, self.positionZ))) 228 | self.earthGravX.set (earthGravVec [0]) 229 | self.earthGravY.set (earthGravVec [1]) 230 | self.earthGravZ.set (earthGravVec [2]) 231 | 232 | moonGravVec = cm.getGravVec (self.shipMass, cm.moonMass, cm.moonDiam, sp.tSub (sp.tEva ((self.positionX, self.positionY, self.positionZ)), (0, 0, cm.earthMoonDist))) 233 | self.moonGravX.set (moonGravVec [0]) 234 | self.moonGravY.set (moonGravVec [1]) 235 | self.moonGravZ.set (moonGravVec [2]) 236 | 237 | self.totalForceX.set (self.thrustX + self.earthGravX + self.moonGravX) 238 | self.totalForceY.set (self.thrustY + self.earthGravY + self.moonGravY) 239 | self.totalForceZ.set (self.thrustZ + self.earthGravZ + self.moonGravZ) 240 | 241 | self.linAccelX.set (self.totalForceX / self.shipMass) 242 | self.linAccelY.set (self.totalForceY / self.shipMass) 243 | self.linAccelZ.set (self.totalForceZ / self.shipMass) 244 | 245 | self.linVelocX.set (self.linVelocX + self.linAccelX * sp.world.period) 246 | self.linVelocY.set (self.linVelocY + self.linAccelY * sp.world.period) 247 | self.linVelocZ.set (self.linVelocZ + self.linAccelZ * sp.world.period) 248 | 249 | self.positionX.set (self.positionX + self.linVelocX * sp.world.period) 250 | self.positionY.set (self.positionY + self.linVelocY * sp.world.period) 251 | self.positionZ.set (self.positionZ + self.linVelocZ * sp.world.period) 252 | 253 | self.part ('angular movement') 254 | 255 | rSq = self.effectiveRadius * self.effectiveRadius 256 | hSq = self.effectiveHeight * self.effectiveHeight 257 | 258 | # Source: https://en.wikipedia.org/wiki/List_of_moments_of_inertia#List_of_3D_inertia_tensors 259 | shipInertMat = self.shipMass () / 12 * np.array ( 260 | ( 261 | ((3 * rSq + hSq) / 12 , 0 , 0 ), 262 | (0 , (3 * rSq + hSq) / 12 , 0 ), 263 | (0 , 0 , rSq / 6) 264 | ) 265 | ) 266 | invInertMat = np.linalg.inv (self._shipRotMat @ shipInertMat @ self._shipRotMat.T) 267 | 268 | self.blueYellowTorque.set (self.blueYellowThrust * self.effectiveHeight / 2) 269 | self.greenRedTorque.set (-self.greenRedThrust * self.effectiveHeight / 2) 270 | shipTorqueVec = np.array ((self.blueYellowTorque (), self.greenRedTorque (), 0)) 271 | 272 | if cm.useQuaternions: 273 | rawTorqueVec = sp.quatVecRot (self._shipRotQuat, shipTorqueVec) 274 | else: 275 | rawTorqueVec = self._shipRotMat @ shipTorqueVec 276 | self.torqueX.set (rawTorqueVec [0]) 277 | self.torqueY.set (rawTorqueVec [1]) 278 | self.torqueZ.set (rawTorqueVec [2]) 279 | torqueVec = np.array ((self.torqueX (), self.torqueY (), self.torqueZ ())) 280 | 281 | rawAngAccelVec = sp.degreesPerRadian * invInertMat @ torqueVec 282 | 283 | self.angAccelX.set (rawAngAccelVec [0]) 284 | self.angAccelY.set (rawAngAccelVec [1]) 285 | self.angAccelZ.set (rawAngAccelVec [2]) 286 | 287 | self.angVelocX.set (self.angVelocX + self.angAccelX * sp.world.period) 288 | self.angVelocY.set (self.angVelocY + self.angAccelY * sp.world.period) 289 | self.angVelocZ.set (self.angVelocZ + self.angAccelZ * sp.world.period) 290 | angVelocVec = sp.radiansPerDegree * np.array ((self.angVelocX (), self.angVelocY (), self.angVelocZ ())) 291 | 292 | # Actual integration over one timestep 293 | # Source: Friendly F# and C++ (fun with game physics), by Dr Giuseppe Maggiore and Dino Dini, May 22, 2014 294 | if cm.useQuaternions: 295 | # Quaternions are much more numerically stable 296 | self._shipRotQuat = sp.normized (self._shipRotQuat + sp.quatMul (sp.quatFromVec (angVelocVec), self._shipRotQuat) / 2 * sp.world.period ()) 297 | 298 | self.shipRotQuat0.set (self._shipRotQuat [0]) 299 | self.shipRotQuat1.set (self._shipRotQuat [1]) 300 | self.shipRotQuat2.set (self._shipRotQuat [2]) 301 | self.shipRotQuat3.set (self._shipRotQuat [3]) 302 | 303 | self._shipRotQuat [0] = self.shipRotQuat0 () 304 | self._shipRotQuat [1] = self.shipRotQuat1 () 305 | self._shipRotQuat [2] = self.shipRotQuat2 () 306 | self._shipRotQuat [3] = self.shipRotQuat3 () 307 | 308 | self._shipRotMat = sp.rotMatFromQuat (self._shipRotQuat) 309 | else: 310 | # N.B. The rotation matrix cannot be found by applying angular velocity in x, y and z direction successively 311 | self._shipRotMat = self._shipRotMat + np.cross (angVelocVec, self._shipRotMat, axisb = 0, axisc = 0) * sp.world.period () 312 | if cm.useGramSchmidt: 313 | tf.modifiedGramSchmidt (self._shipRotMat) 314 | 315 | rawAttitudeVec = tf.getXyzAngles (self._shipRotMat) 316 | self.attitudeX.set (rawAttitudeVec [0]) 317 | self.attitudeY.set (rawAttitudeVec [1]) 318 | self.attitudeZ.set (rawAttitudeVec [2]) 319 | 320 | self.part ('sweep time measurement') 321 | self.sweepMin.set (sp.world.period, sp.world.period < self.sweepMin) 322 | self.sweepMax.set (sp.world.period, sp.world.period > self.sweepMax) 323 | self.sweepWatch.reset (self.sweepWatch > 2) 324 | self.sweepMin.set (1000, not self.sweepWatch) 325 | 326 | -------------------------------------------------------------------------------- /simpylc/simulations/rocket/thruster_rotation.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/QQuick/SimPyLC/51bcb8052f06e11a2d9747bb7d0a26a59754deb7/simpylc/simulations/rocket/thruster_rotation.jpg -------------------------------------------------------------------------------- /simpylc/simulations/rocket/timing.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | import common as cm 31 | 32 | class Timing (sp.Chart): 33 | def __init__ (self): 34 | sp.Chart.__init__ (self) 35 | 36 | def define (self): 37 | ''' 38 | if useQuaternions: 39 | self.channel (sp.world.rocket.shipRotQuat0, sp.white, -1, 1, 100) 40 | self.channel (sp.world.rocket.shipRotQuat1, sp.white, -1, 1, 100) 41 | self.channel (sp.world.rocket.shipRotQuat2, sp.white, -1, 1, 100) 42 | self.channel (sp.world.rocket.shipRotQuat3, sp.white, -1, 1, 100) 43 | ''' 44 | 45 | self.channel (sp.world.rocket.attitudeX, sp.red, -180, 180, 100) 46 | self.channel (sp.world.rocket.attitudeY, sp.green, -180, 180, 100) 47 | self.channel (sp.world.rocket.attitudeZ, sp.blue, -180, 180, 100) 48 | 49 | -------------------------------------------------------------------------------- /simpylc/simulations/rocket/transforms.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import numpy as np 29 | 30 | import simpylc as sp 31 | 32 | # Angles are in degrees, gonio functions are defined accordingly in SimPyLC, 33 | # and will also call evaluate 34 | 35 | def getRotXMat (angleX): 36 | c = sp.cos (angleX) 37 | s = sp.sin (angleX) 38 | return np.array ([ 39 | [1, 0, 0], 40 | [0, c, -s], 41 | [0, s, c] 42 | ]) 43 | 44 | def getRotYMat (angleY): 45 | c = sp.cos (angleY) 46 | s = sp.sin (angleY) 47 | return np.array ([ 48 | [c, 0, s], 49 | [0, 1, 0], 50 | [-s, 0, c] 51 | ]) 52 | 53 | def getRotZMat (angleZ): 54 | c = sp.cos (angleZ) 55 | s = sp.sin (angleZ) 56 | return np.array ([ 57 | [c, -s, 0], 58 | [s, c, 0], 59 | [0, 0, 1] 60 | ]) 61 | 62 | def isClose(x, y): 63 | return abs (x - y) <= 1e-8 + 1e-5 * abs (y) 64 | 65 | def getXyzAngles (rotMat): # rotMat == rotMatZ @ rotMatY @ rotMatX 66 | # Source: Computing Euler angles from a rotation matrix, by Gregory G. Slabaugh 67 | # http://thomasbeatty.com/MATH%20PAGES/ARCHIVES%20-%20NOTES/Applied%20Math/euler%20angles.pdf 68 | angleZ = 0 69 | if isClose (rotMat [2, 0], -1): 70 | angleY = sp.pi / 2.0 71 | angleX = sp.atan2 (rotMat [0, 1], rotMat [0, 2]) 72 | elif isClose (rotMat [2, 0], 1): 73 | angleY = -sp.pi / 2 74 | angleX = sp.atan2 (-rotMat [0, 1], -rotMat [0, 2]) 75 | else: 76 | angleY = -sp.asin (rotMat [2, 0]) 77 | cosAngleY = sp.cos (angleY) 78 | angleX = sp.atan2 (rotMat [2, 1] / cosAngleY, rotMat [2, 2] / cosAngleY) 79 | angleZ = sp.atan2 (rotMat [1, 0] / cosAngleY, rotMat [0, 0] / cosAngleY) 80 | return np.array ([angleX, angleY, angleZ]) 81 | 82 | def modifiedGramSchmidt (rotMat): # Numpy QR factorization can't be used, since it gives a Q with a changed orientation 83 | 84 | # Create references to column vectors 85 | t = rotMat [ : , 0] 86 | n = rotMat [ : , 1] 87 | b = rotMat [ : , 2] 88 | 89 | # Normalize T 90 | t /= np.linalg.norm (t) 91 | 92 | # Remove projection of T from N and normalize 93 | n -= t * np.dot (n, t) 94 | n /= np.linalg.norm (n) 95 | 96 | # Compute B perpendicular to T and N with right orientation 97 | # Remove projection of 98 | b -= t * np.dot (b, t) 99 | b -= n * np.dot (b, n) 100 | b /= np.linalg.norm (b) 101 | -------------------------------------------------------------------------------- /simpylc/simulations/rocket/visualisation.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import random as rd 29 | 30 | import simpylc as sp 31 | 32 | import common as cm 33 | 34 | rd.seed () 35 | 36 | ''' 37 | 38 | z 39 | | 40 | o -- y 41 | / 42 | x 43 | 44 | ''' 45 | 46 | 47 | class Visualisation (sp.Scene): 48 | def __init__ (self): 49 | sp.Scene.__init__ (self) 50 | 51 | self.camera = sp.Camera () 52 | 53 | self.earth = sp.Ellipsoid (size = 3 * (cm.earthDiam,), center = (0, 0, 0), color = (0, 0, 0.9)) 54 | self.moon = sp.Ellipsoid (size = 3 * (cm.moonDiam,), center = (0, 0, cm.earthMoonDist), color = (0.6, 0.6, 0.6)) 55 | 56 | self.body = sp.Cylinder (size = (0.3, 0.3, 1), center = (0, 0, 0.85 + 0.4), pivot = (0, 0, 1), color = (1, 1, 0.2)) 57 | self.nose = sp.Cone (size = (0.3, 0.3, 0.5), center = (0, 0, 0.75), color = (1, 1, 0.2)) 58 | self.bracket = sp.Cylinder (size = (0.1, 0.1, 0.1), center = (0, 0, -0.55), color = (1, 1, 0.2)) 59 | self.gimbal = sp.Ellipsoid (size = 3 * (0.12,), center = (0, 0, -0.05), pivot = (1, 0, 0), color = (1, 1, 0.2)) 60 | self.thruster = sp.Cone (size = (0.2, 0.2, 0.3), pivot = (0, -1, 0), center = (0, 0, -0.09), joint = (0, 0, 0.09), color = (1, 1, 0.2)) # See thruster_rotation.jpg for pivot 61 | # Center at -(0.3/2 - 0.12/2) 62 | self.flame = sp.Cone (size = (0.1, 0.1, 1), center = (0, 0, -0.65), joint = (0, 0, 0.5), axis = (0, 1, 0), angle = 180, color = (1, 0.7, 0)) 63 | self.tankRed = sp.Ellipsoid (size = 3 * (0.1,), center = (0.16, 0, 0), color = (1, 0, 0)) 64 | self.tankGreen = sp.Ellipsoid (size = 3 * (0.1,), center = (-0.16, 0, 0), color = (0, 1, 0)) 65 | self.tankYellow = sp.Ellipsoid (size = 3 * (0.1,), center = (0, 0.16, 0), color = (1, 1, 0)) 66 | self.tankBlue = sp.Ellipsoid (size = 3 * (0.1,), center = (0, -0.16, 0), color = (0, 0, 1)) 67 | 68 | def display (self): 69 | self.camera ( 70 | position = sp.tEva ((sp.world.rocket.positionX + 4, sp.world.rocket.positionY, sp.world.rocket.positionZ)), 71 | focus = sp.tEva ((sp.world.rocket.positionX, sp.world.rocket.positionY, sp.world.rocket.positionZ + 1.5)) 72 | ) 73 | 74 | self.earth () 75 | self.moon () 76 | 77 | self.body ( 78 | position = sp.tEva ((sp.world.rocket.positionX, sp.world.rocket.positionY, sp.world.rocket.positionZ)), 79 | attitude = sp.world.rocket._shipRotMat, 80 | parts = lambda: 81 | self.nose () + 82 | self.bracket ( 83 | parts = lambda: 84 | self.tankGreen () + 85 | self.tankRed () + 86 | self.tankBlue () + 87 | self.tankYellow () + 88 | self.gimbal ( 89 | rotation = sp.world.rocket.blueYellowAngle, 90 | parts = lambda: 91 | self.thruster ( 92 | rotation = sp.world.rocket.greenRedAngle, 93 | parts = lambda: 94 | self.flame ( 95 | scale = sp.tsMul ((1, 1, 1), 96 | sp.world.rocket.thrust / sp.world.rocket.thrusterMaxForce * (0.9 + 0.1 * rd.random ())), 97 | color = (1, 0.3 + 0.7 * rd.random (), 0)) 98 | ) ) ) ) 99 | 100 | -------------------------------------------------------------------------------- /simpylc/simulations/rocket/world.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import os 29 | import sys as ss 30 | 31 | ss.path.append (os.path.abspath ('../..')) # If you want to store your simulations somewhere else, put SimPyLC in your PYTHONPATH environment variable 32 | 33 | import simpylc as sp 34 | import rocket as rk 35 | import control as ct 36 | import visualisation as vs 37 | import timing as tm 38 | 39 | sp.World (rk.Rocket, ct.Control, vs.Visualisation, tm.Timing) 40 | -------------------------------------------------------------------------------- /simpylc/simulations/train/control.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | class Control (sp.Module): 31 | def __init__ (self): 32 | sp.Module.__init__ (self) 33 | 34 | self.page ('train control') 35 | 36 | self.group ('control buttons', True) 37 | self.brakeLiftButton = sp.Marker () 38 | self.driveEnableButton = sp.Marker () 39 | 40 | self.group ('warning lamps') 41 | self.brakeWarnLamp = sp.Marker () 42 | self.speedWarnLamp = sp.Marker () 43 | 44 | self.group ('state') 45 | self.accel = sp.Register () 46 | self.speed = sp.Register () 47 | self.position = sp.Register () 48 | 49 | self.group ('auxiliary', True) 50 | self.maxSpeed = sp.Register () 51 | self.speedWarnFraction = sp.Register (0.8) 52 | self.oldSpeed = sp.Register () 53 | self.blinkTime = sp.Register (0.5) 54 | self.blinkTimer = sp.Timer () 55 | self.blinkEdge = sp.Oneshot () 56 | self.blinkOn = sp.Marker () 57 | 58 | self.group ('sweep time measurement') 59 | self.sweepMin = sp.Register (sp.finity) 60 | self.sweepMax = sp.Register () 61 | self.watchTime = sp.Register (2) 62 | self.watchTimer = sp.Timer () 63 | self.run = sp.Runner () 64 | 65 | def input (self): 66 | self.part ('speed') 67 | self.speed.set (sp.world.physics.speed) 68 | self.maxSpeed.set (sp.world.physics.maxSpeed) 69 | 70 | self.part ('position') 71 | self.position.set (sp.world.physics.position) 72 | 73 | def sweep (self): 74 | self.part ('dynamics') 75 | self.accel.set ((self.speed - self.oldSpeed) / sp.world.period) 76 | self.oldSpeed.set (self.speed) 77 | 78 | self.part ('warnings') 79 | self.blinkTimer.reset (self.blinkTimer > self.blinkTime) 80 | self.blinkEdge.trigger (not self.blinkTimer) 81 | self.blinkOn.mark (not self.blinkOn, self.blinkEdge) 82 | self.brakeWarnLamp.mark (not self.brakeLiftButton and self.driveEnableButton and self.blinkOn) 83 | self.speedWarnLamp.mark (self.speed > self.speedWarnFraction * self.maxSpeed and self.blinkOn) 84 | 85 | self.part ('sweep time masurement') 86 | self.sweepMin.set (sp.world.period, sp.world.period < self.sweepMin) 87 | self.sweepMax.set (sp.world.period, sp.world.period > self.sweepMax) 88 | self.watchTimer.reset (self.watchTimer > self.watchTime) 89 | self.sweepMin.set (sp.finity, not self.watchTimer) 90 | self.sweepMax.set (0, not self.watchTimer) 91 | 92 | def output (self): 93 | self.part ('control signals') 94 | sp.world.physics.brakeLift.mark (self.brakeLiftButton) 95 | sp.world.physics.driveEnable.mark (self.driveEnableButton) 96 | 97 | -------------------------------------------------------------------------------- /simpylc/simulations/train/physics.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | class Physics (sp.Module): 31 | def __init__ (self): 32 | sp.Module.__init__ (self) 33 | 34 | self.page ('train physics') 35 | 36 | self.group ('control signals', True) 37 | self.brakeLift = sp.Marker () 38 | self.driveEnable = sp.Marker () 39 | 40 | self.group ('state') 41 | self.targetAccel = sp.Register () 42 | self.speed = sp.Register () 43 | self.position = sp.Register () 44 | 45 | self.group ('limits', True) 46 | self.maxBrakeDecel = sp.Register (5) 47 | self.maxDriveAccel= sp.Register (2) 48 | self.maxDriveDecel = sp.Register (3) 49 | self.maxSpeed = sp.Register (30) 50 | self.maxPosition = sp.Register (20_000) 51 | 52 | self.group ('auxiliary') 53 | self.brakeAccel = sp.Register () 54 | self.driveAccel = sp.Register () 55 | 56 | def sweep (self): 57 | self.part ('acceleration') 58 | self.brakeAccel.set (0, self.brakeLift, -self.maxBrakeDecel) 59 | self.driveAccel.set (self.maxDriveAccel, self.driveEnable, -self.maxDriveDecel) 60 | self.targetAccel.set (self.brakeAccel + self.driveAccel) 61 | 62 | self.part ('integration') 63 | self.speed.set (sp.limit (self.speed + self.targetAccel * sp.world.period, 0, self.maxSpeed)) 64 | self.position.set (self.position + self.speed * sp.world.period) 65 | -------------------------------------------------------------------------------- /simpylc/simulations/train/timing.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import simpylc as sp 29 | 30 | class Timing (sp.Chart): 31 | def __init__ (self): 32 | sp.Chart.__init__ (self) 33 | 34 | def define (self): 35 | self.channel (sp.world.control.brakeLiftButton, sp.lime) 36 | self.channel (sp.world.control.driveEnableButton, sp.lime) 37 | 38 | self.channel (sp.world.control.brakeWarnLamp, sp.red) 39 | self.channel (sp.world.control.speedWarnLamp, sp.red) 40 | 41 | self.channel (sp.world.physics.targetAccel, sp.yellow, -10, 5, 100) 42 | 43 | self.channel (sp.world.control.speed, sp.white, 0, 50, 200) 44 | self.channel (sp.world.control.accel, sp.white, -10, 5, 100) 45 | -------------------------------------------------------------------------------- /simpylc/simulations/train/world.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | import os 29 | import sys as ss 30 | 31 | ss.path.append (os.path.abspath ('../..')) # If you want to store your simulations somewhere else, put SimPyLC in your PYTHONPATH environment variable 32 | 33 | import simpylc as sp 34 | import physics as ph 35 | import control as ct 36 | import timing as tm 37 | 38 | sp.World (ph.Physics, ct.Control, tm.Timing) 39 | -------------------------------------------------------------------------------- /simpylc/vectors.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====== Legal notices 3 | 4 | Copyright (C) 2013 - 2021 GEATEC engineering 5 | 6 | This program is free software. 7 | You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY, without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | See the QQuickLicense for details. 13 | 14 | The QQuickLicense can be accessed at: http://www.qquick.org/license.html 15 | 16 | __________________________________________________________________________ 17 | 18 | 19 | THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! 20 | 21 | __________________________________________________________________________ 22 | 23 | It is meant for training purposes only. 24 | 25 | Removing this header ends your license. 26 | ''' 27 | 28 | from numpy import * 29 | 30 | #from .engine import * 31 | 32 | # 3D vectors 33 | 34 | def vEva (v): 35 | return (evaluate (v [0]), evaluate (v [1]), evaluate (v [2])) 36 | 37 | def vNeg (v): 38 | return (-v [0], -v [1], -v [2]) 39 | 40 | def vAdd (v0, v1): 41 | return (v0 [0] + v1 [0], v0 [1] + v1 [1], v0 [2] + v1 [2]) 42 | 43 | def vSub (v0, v1): 44 | return (v0 [0] - v1 [0], v0 [1] - v1 [1], v0 [2] - v1 [2]) 45 | 46 | def vMul (v0, v1): 47 | return (x [0] * v [0], x [1] * v [1], x [2] * v [2]) 48 | 49 | def vsMul (v, x): 50 | return (v [0] * x, v [1] * x, v [2] * x) 51 | 52 | def vDiv (v0, v1): 53 | return (v0 [0] / v1 [0], v0 [1] / v1 [1], v0 [2] / v1 [2]) 54 | 55 | def vsDiv (v, x): 56 | return (v [0] / x, v [1] / x, v [2] / x) 57 | 58 | def vNor (v): 59 | return sqrt (v [0] * v [0] + v [1] * v[1] + v [2] * v [2]) 60 | 61 | def vUni (v): 62 | return divide (v, tNor (v)) 63 | 64 | def vIpr (v0, v1): 65 | return v0 [0] * v1 [0] + v0 [1] * v1 [1] + v0 [2] * v1 [2] 66 | 67 | def vOpr (v0, v1): 68 | return ( 69 | v0 [1] * v1 [2] - v0 [2] * v1 [1], 70 | v0 [2] * v1 [0] - v0 [0] * v1 [2], 71 | v0 [0] * v1 [1] - v0 [1] * v1 [0] 72 | ) 73 | 74 | # Square matrices 75 | 76 | ''' 77 | def mInv (m): 78 | return invert (array (m)) .tolist () 79 | ''' 80 | 81 | def msMul (m, s): 82 | return (array (m) * s) .tolist () 83 | 84 | def mMul (m0, m1): 85 | return matmul (array (m0), array (m1)) .tolist () 86 | 87 | def mTra (m): 88 | return transpose (array (m)) .tolist () 89 | 90 | -------------------------------------------------------------------------------- /upload.sh: -------------------------------------------------------------------------------- 1 | py310 -m twine upload dist/* 2 | --------------------------------------------------------------------------------