Car simulations joined

Author: geatec

simpylc/.~lock.simpylc_howto.odt#

@@ -26,6 +26,7 @@
 
 import time as tm
 import traceback as tb
+import math as mt
 
 import simpylc as sp
 
@@ -77,7 +78,7 @@ def sweep (self):   # Control algorithm to be tested
         self.targetObstacleAngle = (self.nearestObstacleAngle + self.nextObstacleAngle) / 2
 
         self.steeringAngle = self.targetObstacleAngle
-        self.targetVelocity = (sp.abs (90 - self.steeringAngle) / 80) if self.driveEnabled else 0
+        self.targetVelocity = ((90 - abs (self.steeringAngle)) / 60) if self.driveEnabled else 0
 
     def output (self):  # Output to simulator
         sp.world.physics.steeringAngle.set (self.steeringAngle)

simpylc/simulations/car/control.py renamed to simpylc/simulations/car/alternatives/control.py

@@ -0,0 +1,78 @@
+# ====== Legal notices
+#
+# Copyright (C) 2013 - 2020 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.geatec.com/qqLicence.html
+#
+# __________________________________________________________________________
+#
+#
+#  THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !!
+#
+# __________________________________________________________________________
+#
+# It is meant for training purposes only.
+#
+# Removing this header ends your licence.
+#
+
+import time as tm
+import traceback as tb
+import math as mt
+
+import timer as tr
+import pid_controller as pc
+
+class LidarPilotBase:
+    def __init__ (self):
+        self.driveEnabled = False
+        self.steeringAngle = 0
+        
+        self.timer = tr.Timer ()
+        self.steeringPidController = pc.PidController (1.05, 0, -0.03)
+        
+        while True:
+            self.timer.tick ()
+            self.input ()
+            self.sweep ()
+            self.output ()
+            tm.sleep (0.02)
+        
+    def sweep (self):   # Control algorithm to be tested
+        self.nearestObstacleDistance = self.finity
+        self.nearestObstacleAngle = 0
+        
+        self.nextObstacleDistance = self.finity
+        self.nextObstacleAngle = 0
+
+        for lidarAngle in range (-self.lidarHalfApertureAngle, self.lidarHalfApertureAngle):
+            lidarDistance = self.lidarDistances [lidarAngle]
+            
+            if lidarDistance < self.nearestObstacleDistance:
+                self.nextObstacleDistance =  self.nearestObstacleDistance
+                self.nextObstacleAngle = self.nearestObstacleAngle
+                
+                self.nearestObstacleDistance = lidarDistance 
+                self.nearestObstacleAngle = lidarAngle
+
+            elif lidarDistance < self.nextObstacleDistance:
+                self.nextObstacleDistance = lidarDistance
+                self.nextObstacleAngle = lidarAngle
+           
+        self.targetObstacleDistance = (self.nearestObstacleDistance + self.nextObstacleDistance) / 2
+        self.targetObstacleAngle = (self.nearestObstacleAngle + self.nextObstacleAngle) / 2
+        
+        self.steeringAngle = self.steeringPidController.getY (self.timer.deltaTime, self.targetObstacleAngle, 0)
+        # print (f'{self.targetObstacleAngle:5.3f} {self.steeringAngle:5.3f}', '\r')
+        # self.steeringAngle = self.targetObstacleAngle
+        self.targetVelocity = ((90 - abs (self.steeringAngle)) / 60) if self.driveEnabled else 0
+
+        

simpylc/simulations/car/keyboard_pilot.py renamed to simpylc/simulations/car/alternatives/keyboard_pilot.py

@@ -0,0 +1,39 @@
+# ====== Legal notices
+#
+# Copyright (C) 2013 - 2020 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.geatec.com/qqLicence.html
+#
+# __________________________________________________________________________
+#
+#
+#  THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !!
+#
+# __________________________________________________________________________
+#
+# It is meant for training purposes only.
+#
+# Removing this header ends your licence.
+#
+
+import lidar_pilot_base as lb
+
+class LidarPilotRealIo (lb.LidarPilotBase):
+
+            def __init__ (self):
+        self.finity = 1_000_000_000        
+        super () .__init__ ()
+        
+    def input (self):   # Input from hardware
+        pass
+    
+    def output (self):  # Output to hardware
+        pass

simpylc/simulations/car/lidar_pilot.py renamed to simpylc/simulations/car/alternatives/lidar_pilot.py

@@ -0,0 +1,50 @@
+# ====== Legal notices
+#
+# Copyright (C) 2013 - 2020 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.geatec.com/qqLicence.html
+#
+# __________________________________________________________________________
+#
+#
+#  THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !!
+#
+# __________________________________________________________________________
+#
+# It is meant for training purposes only.
+#
+# Removing this header ends your licence.
+#
+
+import simpylc as sp
+import lidar_pilot_base as lb
+
+class LidarPilotSimulatedIo (lb.LidarPilotBase):
+    def __init__ (self):
+        print ('Use up arrow to start, down arrow to stop')
+        self.finity = sp.finity
+        super () .__init__ ()
+        
+    def input (self):   # Input from simulator
+        key = sp.getKey ()
+        
+        if key == 'KEY_UP':
+            self.driveEnabled = True
+        elif key == 'KEY_DOWN':
+            self.driveEnabled = False
+        
+        self.lidarDistances = sp.world.visualisation.lidar.distances
+        self.lidarHalfApertureAngle = sp.world.visualisation.lidar.halfApertureAngle
+        
+    def output (self):  # Output to simulator
+        sp.world.physics.steeringAngle.set (self.steeringAngle)
+        sp.world.physics.targetVelocity.set (self.targetVelocity)
+        

simpylc/simulations/car/lidar_pilot_sp.py renamed to simpylc/simulations/car/alternatives/lidar_pilot_sp.py

@@ -0,0 +1,85 @@
+# ====== Legal notices
+#
+# Copyright (C) 2013  - 2020 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.
+#
+
+'''
+Requirements:
+=============
+
+Given a timedependent reference signal and a timedependent feedback sensor input,
+the PID controller should minimize the difference between them.
+To this end it's output will be the sum of:
+- A term proportional to this difference
+- A term proportional the time integral of this difference
+- A term proportional to the time derivative of this difference
+The weight of this terms should be freely choosable when creating a PID controller.
+The PID controller will function in a control loop with known, but varying cycle time.
+
+Testspec:
+=========
+
+A number known time dependent signals will be fed to the PID controller.
+They will be shown graphically together with the output of the controller,
+allowing visual validation.
+
+Design:
+=======
+
+The PID controller will be coded as a class,
+receiving the p, i and d factors at construction time.
+Construction happens prior to entering the control loop.
+In the control loop, the output signal will be computed by a method of the controller,
+receiving:
+- The most recent cycle time
+- The reference signal
+- The feedback signal from the sensor
+'''
+
+class PidController:
+    def __init__ (self, p, i, d):
+        self.p = p
+        self.i = i
+        self.d = d
+        self.yI = 0
+        self.xErrorOld = 0
+
+    def getY (self, deltaT, xSetpoint, xActual):
+        
+        # Deviation between setpoint and actual signal
+        xError = xSetpoint - xActual
+        
+        # Proportional term
+        yP = self.p * xError
+        
+        # Integrating term
+        self.yI += self.i * xError * deltaT
+        
+        # Differentiating term
+        yD = self.d * (xError - self.xErrorOld) / deltaT
+        self.xErrorOld = xError
+        
+        # Summation
+        return yP + self.yI + yD
+    

simpylc/simulations/car/timing.py renamed to simpylc/simulations/car/alternatives/timing.py

@@ -0,0 +1,37 @@
+# ====== Legal notices
+#
+# Copyright (C) 2013  - 2020 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 time as tm
+
+class Timer:
+    def __init__ (self):
+        self.time = tm.time ()
+       
+    def tick (self):
+        self._oldTime = self.time
+        self.time = tm.time ()
+        self.deltaTime = self.time - self._oldTime
+        

simpylc/simulations/car/lidar_pilot_base.py

@@ -33,20 +33,12 @@
 
 import simpylc as sp
 
-import control as ct
-import keyboard_pilot as kp
-import lidar_pilot as lp
-import lidar_pilot_sp as ls
+import lidar_pilot_simulated_io as ls
 import physics as ps
 import visualisation as vs
-import timing as tm
 
 sp.World (
-    # ct.Control,
-    # kp.KeyboardPilot,
-    lp.LidarPilot,
-    # ls.LidarPilotSp,
+    ls.LidarPilotSimulatedIo,
     ps.Physics,
-    vs.Visualisation,
-    # tm.Timing
+    vs.Visualisation
 )