Halfway lidar, non-functional

Author: geatec

simpylc/base.py

@@ -43,31 +43,31 @@ def evaluate (anObject):
         return anObject      
 
 def tEva (v):
-    return (evaluate (v [0]), evaluate (v [1]), evaluate (v [2]))
+    return tuple (evaluate (entry) for entry in v)
 
 def tNeg (v):
-    return (-v [0], -v [1], -v [2])
+    return tuple (-entry for entry in v)
 
 def tAdd (v0, v1):
-    return (v0 [0] + v1 [0], v0 [1] + v1 [1], v0 [2] + v1 [2])
+    return tuple (entry0 + entry1 for entry0, entry1 in zip (v0, v1))
 
 def tSub (v0, v1):
-    return (v0 [0] - v1 [0], v0 [1] - v1 [1], v0 [2] - v1 [2])
+    return tuple (entry0 - entry1 for entry0, entry1 in zip (v0, v1))
 
 def tMul (v0, v1):
-    return (x [0] * v [0], x [1] * v [1], x [2] * v [2])
+    return tuple (entry0 * entry1 for entry0, entry1 in zip (v0, v1))
 
 def tsMul (v, x):
-    return (v [0] * x, v [1] * x, v [2] * x)
+    return tuple (entry * x for entry in v)
 
 def tDiv (v, x):
-    return (v [0] / x [0], v [1] / x [1], v [2] / x [2])
+    return tuple (entry0 / entry1 for entry0, entry1 in zip (v0, v1))
 
 def tsDiv (v, x):
-    return (v [0] / x, v [1] / x, v [2] / x)
+    return tuple (entry / x for entry in v)
 
 def tNor (v):
-    return sqrt (v [0] * v [0] + v [1] * v[1] + v [2] * v [2])
+    return sqrt (sum (entry * entry for entry in v))
 
 def tUni (v):
     return tsDiv (v, tNor (v))
@@ -156,4 +156,4 @@ def warnAsyncTrack (frame):
     print ()
     print ('WARNING', getFileLineClause (frame), 'Instance recycling in display function may cause \'jumpy\' camera tracking')
 
-        
+        

simpylc/engine.py

@@ -336,13 +336,15 @@ def __init__ (self, *parameters):
         for parameter in parameters:
             if issubclass (parameter, Module):
                 World._modules.append (parameter ())
-            elif issubclass (parameter, Scene):
+
+        for parameter in parameters:
+            if issubclass (parameter, Scene):
                 World._scenes.append (parameter ())
-            elif issubclass (parameter, Chart):
+
+        for parameter in parameters:
+            if issubclass (parameter, Chart):
                 World._charts.append (parameter ())
-            else:
-                World._Agents.append (parameter)
-                
+
         Module._current = None  # Place further elements outside any module
 
         if generateCode (self):
@@ -357,12 +359,18 @@ def __init__ (self, *parameters):
             module._setPublicElementNames ()
 
         for scene in World._scenes:
+            setattr (World, scene.name, scene)
             scene._registerWithCamera ()
             scene._registerWithThings ()
 
         for chart in World._charts:
             chart.define ()
 
+        # Construct agents last, since they may depend on any other parameters because they have least restrictions
+        for parameter in parameters:
+            if not (issubclass (parameter, Module) or issubclass (parameter, Scene) or issubclass (parameter, Chart)):
+                World._Agents.append (parameter)
+                
         World.first = Marker (True)
         World.sleep = Register (0.02)
         World.refresh = Register (0.013)
@@ -425,6 +433,8 @@ def main (self, window):
         self.Agent.getKey = window.getkey    # No typo
         self.Agent ()
 
+finity = 1e20
+        
 pi = math.pi
 radiansPerDegree = math.pi / 180
 degreesPerRadian = 180 / math.pi
@@ -486,9 +496,11 @@ def snap (anObject, target, margin):
 def digit (anObject, index):
     return int (('000000000000' + str (int (evaluate (anObject)))) [-evaluate (index + 1)])
 
+'''
 _print = print
 
 def cursesCompatiblePrint (*args, **kwargs):
     _print (*args, **kwargs, end = '\n\r')
     
 builtins.print = cursesCompatiblePrint
+'''

simpylc/simulations/car/.visualisation.py.kate-swp

@@ -35,11 +35,11 @@ def __init__ (self):
         self.page ('motion control')
 
         self.group ('driver input', True)
-        self.velocityStepper = sp.Register (0)
-        self.steeringStepper = sp.Register (2)
+        self.targetVelocityStep = sp.Register (0)
+        self.steeringAngleStep = sp.Register (2)
 
         self.group ('control output')
-        self.velocity = sp.Register ()
+        self.targetVelocity = sp.Register ()
         self.steeringAngle = sp.Register ()
 
         self.group ('sweep time measurement', True)
@@ -48,13 +48,14 @@ def __init__ (self):
         self.sweepWatch = sp.Timer ()
         self.run = sp.Runner ()
 
-    def input (self):
-        pass
+    def output (self):
+        sp.world.physics.targetVelocity.set (self.targetVelocity)
+        sp.world.physics.steeringAngle.set (self.steeringAngle)
 
     def sweep (self):
         # Input to output
-        self.velocity = 0.2 * self.velocityStepper
-        self.steeringAngle = 10 * self.steeringStepper
+        self.targetVelocity.set (0.2 * self.targetVelocityStep)
+        self.steeringAngle.set (10 * self.steeringAngleStep)
 
         # Sweep time measurement
         self.sweepMin.set (sp.world.period, sp.world.period < self.sweepMin)

simpylc/simulations/car/control.py

@@ -0,0 +1,32 @@
+- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- - - - - - - * - - - * - - - - - - - - - - - - - - - - - - - - 
+- - - - - - - - - - - - - - * - - - - - - - - - - - * - - - - -
+- - - * - - - * - - * - - - - - - - * - - - * - - - - - - - * -
+- - - - - * - - - - - - - * - - - - - - @ - - - - * - - - - - -
+- - - - - - - - - - - - - - - - - * - - - - * - - - - - * - - -
+- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- * - * - - - - - - - - - - - - - - - - - - - - - - - - - - * -
+- - - - - - - - - - - - - - - - - - - - - - - - - - - - * - - -
+- - - - - - - - - * - - - * - - - - - - - - - - - - - - - - - -
+- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- - * - * - - * - - - - * - - - - * - - - - - - - - - - * - * -
+- - - - - - - - - * - - - - - * - - - - - - - - - - - - - - - -
+- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- * - * - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- - - - - - - * - * - - - - - - - * - * - - - - - - - * - * - -
+- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- * - * - - - - * - * - - - - - - - - - - - - - - - - - - - - -
+- - - - - - - - - - - - - - - - - * - * - - - - - - * - * - - -
+- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- - - - - - - - - * - * - - - - - - - - - - - - - - - - - - - -
+* - * - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- - - - - - - - - - - - - - - - - * - * - - - - - - * - * - - -
+- - - - - - - - - * - * - - - - - - - - - - - - - - - - - - - -
+- - - * - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+- * - - - - - - - - - - - - - - - - - - * - - - - - - - - - - -
+- - - - * - - - - * - - - - - - - - * - - - - - - - - * - * - -
+- - - - - - * - * - - * - - - - - - - - - * - - - - - - - - - -
+- - - * - - - - - - - - - - - - - - - - - - - * - - * - - - - -
+- - - - - - * - - * - - - - - - - - - - * - - - - - - - * - - -
+- - - - - - - - - - - - - - - - - - - - - - - - * - - - - - - -

simpylc/simulations/car/default.track

@@ -0,0 +1,36 @@
+class KeyboardPilot:
+    def __init__ (self):
+        while True:
+            self.readInputs ()
+            self.sweep ()
+            self.writeOutputs ()
+            
+    def readInputs (self):
+        key = self.getKey ()
+        
+        self.upKey = key == 'KEY_UP'
+        self.downKey = key == 'KEY_DOWN'
+        self.leftKey = key == 'KEY_LEFT'
+        self.rightKey = key == 'KEY_RIGHT'
+
+        self.targetVelocityStep = self.world.control.targetVelocityStep
+        self.steeringAngleStep = self.world.control.steeringAngleStep
+
+    def sweep (self):
+        if self.upKey:
+            self.targetVelocityStep += 1
+            print ('Target velocity step: ', self.targetVelocityStep)
+        elif self.downKey:
+            self.targetVelocityStep -= 1
+            print ('Target velocity step: ', self.targetVelocityStep)
+        elif self.leftKey:
+            self.steeringAngleStep += 1
+            print ('Steering angle step: ', self.steeringAngleStep)
+        elif self.rightKey:
+            self.steeringAngleStep -= 1
+            print ('Steering angle step: ', self.steeringAngleStep)
+        
+    def writeOutputs (self):
+        self.world.control.targetVelocityStep.set (self.targetVelocityStep)
+        self.world.control.steeringAngleStep.set (self.steeringAngleStep)
+        

simpylc/simulations/car/keyboard_pilot.py

@@ -0,0 +1,41 @@
+#! /usr/bin/python
+
+# ====== Legal notices
+#
+# Copyright (C) 2013  - 2018 GEATEC engineering
+#
+# This program is free software.
+# You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicence.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY, without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+# See the QQuickLicence for details.
+#
+# The QQuickLicense can be accessed at: http://www.qquick.org/license.html
+#
+# __________________________________________________________________________
+#
+#
+#  THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !!
+#
+# __________________________________________________________________________
+#
+# It is meant for training purposes only.
+#
+# Removing this header ends your licence.
+#
+
+import os
+import sys as ss
+
+ss.path.append (os.path.abspath ('../../..')) # If you want to store your simulations somewhere else, put SimPyLC in your PYTHONPATH environment variable
+
+import simpylc as sp
+import keyboard_pilot as lp
+import control as ct
+import physics as ps
+import visualisation as vs
+import timing as tm
+
+sp.World (lp.KeyboardPilot, ct.Control, ps.Physics, vs.Visualisation) # , tm.Timing)

simpylc/simulations/car/keyboard_world.py

@@ -0,0 +1,50 @@
+'''
+Inputs:
+
+- A lidar distance array with a resolution of 1 degree
+- An x, y position and course angle of the car
+
+Outpus:
+
+- Acceleration stepper output
+
+'''
+
+import time as tm
+
+import simpylc as sp
+
+class LidarPilot:
+    noObstacle = (sp.finity, 0)
+
+    def __init__ (self):
+        self.lidar = self.world.visualisation.lidar
+        
+        while True:
+            self.readInputs ()
+            self.sweep ()
+            self.writeOutputs ()
+            tm.sleep (0.02)
+            
+            
+    def readInputs (self):
+        self.nearestObstacles = [self.noObstacle, self.noObstacle]
+        
+        for lidarAngle in range (-self.lidar.halfApertureAngle, self.lidar.halfApertureAngle):
+            lidarDistance = self.lidar.distances [lidarAngle]
+            if lidarDistance < self.nearestObstacles [0][0]:
+                self.nearestObstacles [0] = (lidarDistance, lidarAngle)
+            elif lidarDistance < self.nearestObstacles [1][0]:
+                self.nearestObstacles [1] = (lidarDistance, lidarAngle)
+                                                                                                                                
+        self.targetObstacle = sp.tsDiv (sp.tAdd (*self.nearestObstacles), 2)
+        
+        self.velocity = self.world.physics.velocity
+        self.steeringAngle = self.world.physics.steeringAngle
+
+    def sweep (self):
+        self.steeringAngle = self.targetObstacle [1]
+
+    def writeOutputs (self):
+        self.world.physics.steeringAngle.set (self.steeringAngle)
+        

simpylc/simulations/car/lidar_pilot.py

@@ -32,10 +32,10 @@
 ss.path.append (os.path.abspath ('../../..')) # If you want to store your simulations somewhere else, put SimPyLC in your PYTHONPATH environment variable
 
 import simpylc as sp
-import pilot as pl
+import lidar_pilot as lp
 import control as ct
 import physics as ps
 import visualisation as vs
 import timing as tm
 
-sp.World (pl.Pilot, ct.Control, ps.Physics, vs.Visualisation) # , tm.Timing)
+sp.World (lp.LidarPilot, ps.Physics, vs.Visualisation) # , tm.Timing)

simpylc/simulations/car/world.py renamed to simpylc/simulations/car/lidar_world.py

@@ -36,6 +36,8 @@ def __init__ (self):
 
         self.group ('wheels', True)
 
+        self.acceleration = sp.Register (2)
+        self.targetVelocity= sp.Register ()
         self.velocity = sp.Register ()
         self.midWheelAngularVelocity = sp.Register ()
         self.midWheelAngle = sp.Register (30)
@@ -60,15 +62,12 @@ def __init__ (self):
         self.slipping = sp.Marker ()
         self.radialVelocity = sp.Register ()
 
-    def input (self):   
-        self.velocity = sp.world.control.velocity
-        self.steeringAngle = sp.world.control.steeringAngle
-        
     def sweep (self):
         self.page ('traction')  
         self.group ('wheels', True)
 
-        self.midWheelAngularVelocity = self.velocity / pm.displacementPerWheelAngle
+        self.velocity.set (self.velocity + self.acceleration * sp.world.period, self.velocity < self.targetVelocity, self.velocity - self.acceleration * sp.world.period)
+        self.midWheelAngularVelocity.set (self.velocity / pm.displacementPerWheelAngle)
         self.midWheelAngle.set (self.midWheelAngle + self.midWheelAngularVelocity * sp.world.period)
         self.tangentialVelocity.set (self.midWheelAngularVelocity * pm.displacementPerWheelAngle)