Perpendicular parking roughly done

Author: klima7

common/__init__.py

@@ -0,0 +1,3 @@
+from .backward_for_some_time import BackwardForSomeTime
+from .forward_for_some_time import ForwardForSomeTime
+from .wait_some_time import WaitSomeTime

common/backward_for_some_time.py

@@ -0,0 +1,17 @@
+from control import Stage
+from time import time
+
+
+class BackwardForSomeTime(Stage):
+
+    def __init__(self, duration):
+        super().__init__()
+        self.duration = duration
+        self.start_time = None
+
+    def started(self, tank, distances):
+        self.start_time = time()
+
+    def control(self, tank, distances):
+        tank.forward(-5)
+        return time() - self.start_time > self.duration

common/forward_for_some_time.py

@@ -0,0 +1,18 @@
+from control import Stage
+from time import time
+
+
+class ForwardForSomeTime(Stage):
+
+    def __init__(self, duration):
+        super().__init__()
+        self.duration = duration
+        self.start_time = None
+
+    def started(self, tank, distances):
+        self.start_time = time()
+        tank.restart_plot()
+
+    def control(self, tank, distances):
+        tank.forward(20)
+        return time() - self.start_time > self.duration

common/model.py

@@ -0,0 +1,63 @@
+import matplotlib.pyplot as plt
+import numpy as np
+import skfuzzy as fuzz
+from skfuzzy import control as ctrl
+from skfuzzy.control import Antecedent, Consequent, Rule
+
+
+class FuzzyModel:
+
+    MAX_DIST = 6
+
+    def __init__(self, max_vel, stop_dist, break_dist, break_vel, sharpness):
+        self._max_vel = max_vel
+        self._stop_dist = stop_dist
+        self._break_dist = break_dist
+        self._break_vel = break_vel
+        self._sharpness = sharpness
+
+        self._model = self._construct_model()
+
+    def get_velocity(self, distance):
+        if distance < self._stop_dist:
+            return 0
+
+        self._model.input['dist'] = distance
+        self._model.compute()
+        velocity = self._model.output['vel']
+
+        if velocity < 0.2:
+            return 0
+
+        return velocity
+
+    def _construct_model(self):
+        l_span = self._break_vel
+        h_span = self._max_vel - self._break_vel
+
+        dist = Antecedent(np.linspace(0, self.MAX_DIST, 600), 'dist')
+        vel = Consequent(np.linspace(-l_span, self._max_vel + h_span, 600), 'vel')
+
+        s = self._sharpness
+        dist['h'] = fuzz.trapmf(dist.universe, [self._break_dist - s, self._break_dist + s, self.MAX_DIST, self.MAX_DIST])
+        dist['m'] = fuzz.trapmf(dist.universe, [self._stop_dist - s, self._stop_dist + s, self._break_dist - s, self._break_dist + s])
+        dist['l'] = fuzz.trapmf(dist.universe, [0, 0, self._stop_dist - s, self._stop_dist + s])
+
+        vel['l'] = fuzz.trimf(vel.universe, [-l_span, 0, l_span])
+        vel['m'] = fuzz.trimf(vel.universe, [0, self._break_vel, self._max_vel])
+        vel['h'] = fuzz.trimf(vel.universe, [self._max_vel - h_span, self._max_vel, self._max_vel + h_span])
+
+        dist.view()
+        vel.view()
+        plt.show()
+
+        rules = [
+            Rule(dist['l'], vel['l']),
+            Rule(dist['m'], vel['m']),
+            Rule(dist['h'], vel['h']),
+        ]
+
+        ctrl_system = ctrl.ControlSystem(rules)
+        model = ctrl.ControlSystemSimulation(ctrl_system)
+
+        return model

common/wait_some_time.py

@@ -0,0 +1,17 @@
+from time import time
+from control import Stage
+
+
+class WaitSomeTime(Stage):
+
+    def __init__(self, duration):
+        super().__init__()
+        self.duration = duration
+        self.start_time = None
+
+    def started(self, tank, distances):
+        self.start_time = time()
+
+    def control(self, tank, distances):
+        tank.stop()
+        return time() - self.start_time > self.duration

control.py

@@ -13,6 +13,7 @@ def control(self):
             return True
 
         distances = self._tank.read_distances()
+        # print(f'sw: {distances.sw}; se: {distances.se}')
         current_stage = self._stages[0]
 
         # start stage
@@ -54,4 +55,4 @@ def run_simulation_with_controller(controller):
     while not finished:
         finished = controller.control()
 
-    tank.plot_distances()
+    tank.plot_distances2()

map.ttt

@@ -1,42 +1,15 @@
-from time import time
-from control import Controller, Stage
-
-
-class ForwardStage(Stage):
-
-    def __init__(self):
-        self.start = None
-        super().__init__()
-
-    def started(self, tank, distances):
-        self.start = time()
-
-    def control(self, tank, distances):
-        tank.forward(5)
-        return time() - self.start > 5
-
-
-class LeftStage(Stage):
-
-    def __init__(self):
-        self.start = None
-        super().__init__()
-
-    def started(self, tank, distances):
-        self.start = time()
-
-    def control(self, tank, distances):
-        tank.turn_left(5)
-        return time() - self.start > 5
-
-
-class PerpParkController(Controller):
-
-    def __init__(self, tank):
-        stages = [
-            ForwardStage(),
-            LeftStage(),
-            ForwardStage(),
-        ]
-        super().__init__(tank, stages)
-
+# from common import WaitSomeTime
+# from control import Controller
+#
+#
+# class PerpParkController(Controller):
+#
+#     def __init__(self, tank):
+#         stages = [
+#             WaitSomeTime(0.1),
+#             ForwardToFindLeftSpace(),
+#             BackwardBeforeTurn(),
+#             TurnLeftToPark(),
+#             # ForwardForSomeTime(3)
+#         ]
+#         super().__init__(tank, stages)

parallel/__init__.py

@@ -0,0 +1,18 @@
+from control import Controller
+from common import WaitSomeTime, BackwardForSomeTime, ForwardForSomeTime
+from .forward_to_find_left_space import ForwardToFindLeftSpace
+from .turn_left_to_park import TurnLeftToPark
+from .backward_before_turn import BackwardBeforeTurn
+
+
+class PerpParkController(Controller):
+
+    def __init__(self, tank):
+        stages = [
+            WaitSomeTime(0.1),
+            ForwardToFindLeftSpace(),
+            BackwardBeforeTurn(),
+            TurnLeftToPark(),
+            # ForwardForSomeTime(3)
+        ]
+        super().__init__(tank, stages)