feat: use redis to communicate movement

2020-08-27 16:37:54 +02:00
parent 32e2e0dfae
commit 24a36e156f
5 changed files with 147 additions and 72 deletions
--- a/1
+++ b/1
@@ -13,6 +13,7 @@ Pillow = ">=7.0.0"
 flask-socketio = ">=4.2.1"
 eventlet = ">=0.25.1"
 redis = ">=3.5.3"
+smbus2 = ">=0.3.0"

 [requires]
 python_version = "3.8"
--- a/Pipfile.lock
+++ b/Pipfile.lock
@@ -1,7 +1,7 @@
 {
    "_meta": {
        "hash": {
-            "sha256": "6fd6898ae25b69f66680a0dc9bd46f466117c4853a67496aa7896301d8e573a9"
+            "sha256": "c33401093f7e3413ccb6280cda747aff9f89f36374dbffe122be6be5b4e7a2d7"
        },
        "pipfile-spec": 6,
        "requires": {
@@ -34,11 +34,11 @@
        },
        "eventlet": {
            "hashes": [
-                "sha256:4c8ab42c51bff55204fef43cff32616558bedbc7538d876bb6a96ce820c7f9ed",
-                "sha256:955f2cf538829bfcb7b3aa885ace40e8ae5965dcd5b876c384d0c5869702db1d"
+                "sha256:4f4a43366b4cbd4a3f2f231816e5c3dae8ab316df9b7da11f0525e2800559f33",
+                "sha256:faa384fdd8e5797ab1f16979576fc93ce89a7e1e5622d345caec1bf767a274fd"
            ],
            "index": "pypi",
-            "version": "==0.25.2"
+            "version": "==0.26.1"
        },
        "flask": {
            "hashes": [
@@ -142,60 +142,62 @@
        },
        "numpy": {
            "hashes": [
-                "sha256:13af0184177469192d80db9bd02619f6fa8b922f9f327e077d6f2a6acb1ce1c0",
-                "sha256:26a45798ca2a4e168d00de75d4a524abf5907949231512f372b217ede3429e98",
-                "sha256:26f509450db547e4dfa3ec739419b31edad646d21fb8d0ed0734188b35ff6b27",
-                "sha256:30a59fb41bb6b8c465ab50d60a1b298d1cd7b85274e71f38af5a75d6c475d2d2",
-                "sha256:33c623ef9ca5e19e05991f127c1be5aeb1ab5cdf30cb1c5cf3960752e58b599b",
-                "sha256:356f96c9fbec59974a592452ab6a036cd6f180822a60b529a975c9467fcd5f23",
-                "sha256:3c40c827d36c6d1c3cf413694d7dc843d50997ebffbc7c87d888a203ed6403a7",
-                "sha256:4d054f013a1983551254e2379385e359884e5af105e3efe00418977d02f634a7",
-                "sha256:63d971bb211ad3ca37b2adecdd5365f40f3b741a455beecba70fd0dde8b2a4cb",
-                "sha256:658624a11f6e1c252b2cd170d94bf28c8f9410acab9f2fd4369e11e1cd4e1aaf",
-                "sha256:76766cc80d6128750075378d3bb7812cf146415bd29b588616f72c943c00d598",
-                "sha256:7b57f26e5e6ee2f14f960db46bd58ffdca25ca06dd997729b1b179fddd35f5a3",
-                "sha256:7b852817800eb02e109ae4a9cef2beda8dd50d98b76b6cfb7b5c0099d27b52d4",
-                "sha256:8cde829f14bd38f6da7b2954be0f2837043e8b8d7a9110ec5e318ae6bf706610",
-                "sha256:a2e3a39f43f0ce95204beb8fe0831199542ccab1e0c6e486a0b4947256215632",
-                "sha256:a86c962e211f37edd61d6e11bb4df7eddc4a519a38a856e20a6498c319efa6b0",
-                "sha256:a8705c5073fe3fcc297fb8e0b31aa794e05af6a329e81b7ca4ffecab7f2b95ef",
-                "sha256:b6aaeadf1e4866ca0fdf7bb4eed25e521ae21a7947c59f78154b24fc7abbe1dd",
-                "sha256:be62aeff8f2f054eff7725f502f6228298891fd648dc2630e03e44bf63e8cee0",
-                "sha256:c2edbb783c841e36ca0fa159f0ae97a88ce8137fb3a6cd82eae77349ba4b607b",
-                "sha256:cbe326f6d364375a8e5a8ccb7e9cd73f4b2f6dc3b2ed205633a0db8243e2a96a",
-                "sha256:d34fbb98ad0d6b563b95de852a284074514331e6b9da0a9fc894fb1cdae7a79e",
-                "sha256:d97a86937cf9970453c3b62abb55a6475f173347b4cde7f8dcdb48c8e1b9952d",
-                "sha256:dd53d7c4a69e766e4900f29db5872f5824a06827d594427cf1a4aa542818b796",
-                "sha256:df1889701e2dfd8ba4dc9b1a010f0a60950077fb5242bb92c8b5c7f1a6f2668a",
-                "sha256:fa1fe75b4a9e18b66ae7f0b122543c42debcf800aaafa0212aaff3ad273c2596"
+                "sha256:082f8d4dd69b6b688f64f509b91d482362124986d98dc7dc5f5e9f9b9c3bb983",
+                "sha256:1bc0145999e8cb8aed9d4e65dd8b139adf1919e521177f198529687dbf613065",
+                "sha256:309cbcfaa103fc9a33ec16d2d62569d541b79f828c382556ff072442226d1968",
+                "sha256:3673c8b2b29077f1b7b3a848794f8e11f401ba0b71c49fbd26fb40b71788b132",
+                "sha256:480fdd4dbda4dd6b638d3863da3be82873bba6d32d1fc12ea1b8486ac7b8d129",
+                "sha256:56ef7f56470c24bb67fb43dae442e946a6ce172f97c69f8d067ff8550cf782ff",
+                "sha256:5a936fd51049541d86ccdeef2833cc89a18e4d3808fe58a8abeb802665c5af93",
+                "sha256:5b6885c12784a27e957294b60f97e8b5b4174c7504665333c5e94fbf41ae5d6a",
+                "sha256:667c07063940e934287993366ad5f56766bc009017b4a0fe91dbd07960d0aba7",
+                "sha256:7ed448ff4eaffeb01094959b19cbaf998ecdee9ef9932381420d514e446601cd",
+                "sha256:8343bf67c72e09cfabfab55ad4a43ce3f6bf6e6ced7acf70f45ded9ebb425055",
+                "sha256:92feb989b47f83ebef246adabc7ff3b9a59ac30601c3f6819f8913458610bdcc",
+                "sha256:935c27ae2760c21cd7354402546f6be21d3d0c806fffe967f745d5f2de5005a7",
+                "sha256:aaf42a04b472d12515debc621c31cf16c215e332242e7a9f56403d814c744624",
+                "sha256:b12e639378c741add21fbffd16ba5ad25c0a1a17cf2b6fe4288feeb65144f35b",
+                "sha256:b1cca51512299841bf69add3b75361779962f9cee7d9ee3bb446d5982e925b69",
+                "sha256:b8456987b637232602ceb4d663cb34106f7eb780e247d51a260b84760fd8f491",
+                "sha256:b9792b0ac0130b277536ab8944e7b754c69560dac0415dd4b2dbd16b902c8954",
+                "sha256:c9591886fc9cbe5532d5df85cb8e0cc3b44ba8ce4367bd4cf1b93dc19713da72",
+                "sha256:cf1347450c0b7644ea142712619533553f02ef23f92f781312f6a3553d031fc7",
+                "sha256:de8b4a9b56255797cbddb93281ed92acbc510fb7b15df3f01bd28f46ebc4edae",
+                "sha256:e1b1dc0372f530f26a03578ac75d5e51b3868b9b76cd2facba4c9ee0eb252ab1",
+                "sha256:e45f8e981a0ab47103181773cc0a54e650b2aef8c7b6cd07405d0fa8d869444a",
+                "sha256:e4f6d3c53911a9d103d8ec9518190e52a8b945bab021745af4939cfc7c0d4a9e",
+                "sha256:ed8a311493cf5480a2ebc597d1e177231984c818a86875126cfd004241a73c3e",
+                "sha256:ef71a1d4fd4858596ae80ad1ec76404ad29701f8ca7cdcebc50300178db14dfc"
            ],
            "markers": "python_version >= '3.6'",
-            "version": "==1.19.0"
+            "version": "==1.19.1"
        },
        "opencv-python": {
            "hashes": [
-                "sha256:156e2954d5b38b676e8a24d66703cf15f252e24ec49db7e842a8b5eed46074ba",
-                "sha256:1bf486680a16d739f7852a62865b72eb7692df584694815774ba97b471b8bc3f",
-                "sha256:1ea08f22246ccd33174d59edfa3f13930bf2c28096568242090bd9d8770fb8a8",
-                "sha256:210ab40c8c9dadc7dc9ed7beebe2e0a2415a744f8d6857762a80c8e0fcc477c8",
-                "sha256:2ec6502bfac01b27ac06daf7bc9f7a4f482a6a0d8e1b30e15c411d478454a19f",
-                "sha256:2fe704e35808cf6b17b793e89fd00e9ef7779f85f274666a4e092671aedd09c0",
-                "sha256:4b93b5f8df187e4dba9fb25c46fa8cf342c257de144f7c86d75c06416566a199",
-                "sha256:55e1d7a2d11c40ea5b53aabe5c4122038803c7d492505c8f93af077aa7fe2ce1",
-                "sha256:677f61332436e22f83a1e4e6f6863a760734fbc8029ba6a8ef0af4554cde6f93",
-                "sha256:76ddc6daf8607eda1d866395dcf98526ef96f3e616d8c37ccc7629f9aaf6d4d4",
-                "sha256:c4f1e9d963c8f370284afa87fcf521cc8439a610a500bf8ede27fd64dd9050bd",
-                "sha256:c93b1198c85175a9fa9a9839c4da55c7ab9c5f57256f2e4211cd6c91d7d422e8",
-                "sha256:d765c44827778cbe6bc8f272cd61514e8509b93fd24dd3324cd4abddf2026b11",
-                "sha256:eb709245e56f6693d297f8818ff8e6c017fa80fdb5a923c64be623a678c7150e",
-                "sha256:ef4ac758a4e2caee80ef9c86b83a279d6f132c9e7ae77957cf74013928814e05",
-                "sha256:f67c1d92ff96c6c106f786b7ef9b9ab448fa03ef28cb7bb6f0f7b857b65bc158",
-                "sha256:f6fa2834d85c78865ca6e3de563916086cb8c83c3f2ef80924fcd07005f05df9",
-                "sha256:fa1a6d149a1a5e0bc54c737a59fe38d75384a092ae5e35f9b876fbb621f755c6",
-                "sha256:fd457deedcf153dd6805a2b4d891ac2a0969566d3755fbf48a3ffb53978c9ed1"
+                "sha256:0039506845d7076e6871c0075227881a84de69799d70ed37c8704d203b740911",
+                "sha256:02f7e31c710a7c82229fc4ad98e7e4cf265d19ab52b4451cbe7e33a840fe6595",
+                "sha256:093c1bfa6da24a9d4dde2d54a22b9acfb46f5cb2c50d7387356cf897f0db0ab9",
+                "sha256:17663f0469b2944b7d4051d4b1c425235d153777f17310c6990370bbb4d12695",
+                "sha256:177f14625ea164f38b5b6f5c2b316f8ff8163e996cc0432de90f475956a9069a",
+                "sha256:324a2c680caae9edbd843a355a2e03792cbd23faf6c24c20dd594fa9aac80765",
+                "sha256:34d0d2c9a80c02d55f83a67c29fc4145a9dcf1fe3ddef0535d0b0d9c7b89b8d2",
+                "sha256:505bd984aae24c489910bbd168e515580d62bc1dbdd5ee36f2c2d42803c4b795",
+                "sha256:608dae0444065669fc26fa6bf1653072e40735b33dfa514c74a6165563a99e97",
+                "sha256:78a0796ec15d1b41f5a87c41f339356eb04858749c8845936be532cb3436f898",
+                "sha256:80a51a797f71ee4a401d281749bb096370007202204bbcd1ecfc9ead58bd3b0b",
+                "sha256:a35b3a3540623090ba5fdad7ed97d0d75ca80ee55f5d7c1cecddda723665c0f8",
+                "sha256:a6e1d065a45ec1bf466f47bdf767e0505b244c9470140cf8bab1dd8835f0d3ee",
+                "sha256:b3ae62990faebefbc3cbc5430f7b6de57bafdcf297134113a9c6d6ccfce4438f",
+                "sha256:bcb24c4f82fa79f049db4bfd0da1d18a315da66a55aa3d4cde81d1ec18f0a7ff",
+                "sha256:cb00bbd41268f5fa0fa327ca30f7621a8ece983e0d8ae472e2ffe7ab1617606f",
+                "sha256:ccd92a126d253c7bd65b36184fe097a0eea77da4d72d427e1630633bc586233e",
+                "sha256:d19cbbcdc05caf7b41e28898f05c076c94b07647b4556c8327663a40acd4e3bd",
+                "sha256:f5b82cd49b560e004608ca53ce625e5167b41f0fdc610758d6989083e26b5a03",
+                "sha256:fae421571a7709ae0baa9bfd08177165bc1d56d7c79c806d12627d58a6faf2d1",
+                "sha256:fec63240ea3179a2b4176a3256a99682129d75450a15bf2807904600ec64b45a"
            ],
            "index": "pypi",
-            "version": "==4.3.0.36"
+            "version": "==4.4.0.42"
        },
        "pillow": {
            "hashes": [
@@ -216,11 +218,13 @@
                "sha256:94cf49723928eb6070a892cb39d6c156f7b5a2db4e8971cb958f7b6b104fb4c4",
                "sha256:97f9e7953a77d5a70f49b9a48da7776dc51e9b738151b22dacf101641594a626",
                "sha256:9ad7f865eebde135d526bb3163d0b23ffff365cf87e767c649550964ad72785d",
+                "sha256:9c87ef410a58dd54b92424ffd7e28fd2ec65d2f7fc02b76f5e9b2067e355ebf6",
                "sha256:a060cf8aa332052df2158e5a119303965be92c3da6f2d93b6878f0ebca80b2f6",
                "sha256:c79f9c5fb846285f943aafeafda3358992d64f0ef58566e23484132ecd8d7d63",
                "sha256:c92302a33138409e8f1ad16731568c55c9053eee71bb05b6b744067e1b62380f",
                "sha256:d08b23fdb388c0715990cbc06866db554e1822c4bdcf6d4166cf30ac82df8c41",
                "sha256:d350f0f2c2421e65fbc62690f26b59b0bcda1b614beb318c81e38647e0f673a1",
+                "sha256:e901964262a56d9ea3c2693df68bc9860b8bdda2b04768821e4c44ae797de117",
                "sha256:ec29604081f10f16a7aea809ad42e27764188fc258b02259a03a8ff7ded3808d",
                "sha256:edf31f1150778abd4322444c393ab9c7bd2af271dd4dafb4208fb613b1f3cdc9",
                "sha256:f7e30c27477dffc3e85c2463b3e649f751789e0f6c8456099eea7ddd53be4a8a",
@@ -231,10 +235,10 @@
        },
        "python-engineio": {
            "hashes": [
-                "sha256:133bdb5fb89f43a53f8612fb1ddbb3a453318713dea18a9ecf5346ed0c0f793c",
-                "sha256:41353c2539493e9e30e0e75e53f9cbb670f09a5ebcf82fe738081a9ba28fe55c"
+                "sha256:36b33c6aa702d9b6a7f527eec6387a2da1a9a24484ec2f086d76576413cef04b",
+                "sha256:cfded18156862f94544a9f8ef37f56727df731c8552d7023f5afee8369be2db6"
            ],
-            "version": "==3.13.1"
+            "version": "==3.13.2"
        },
        "python-socketio": {
            "hashes": [
@@ -259,6 +263,13 @@
            "markers": "python_version >= '2.7' and python_version not in '3.0, 3.1, 3.2, 3.3'",
            "version": "==1.15.0"
        },
+        "smbus2": {
+            "hashes": [
+                "sha256:210e66eebe4d0b1fe836b3ec2751841942e1c4918c0b429b20a0e20a222228b4"
+            ],
+            "index": "pypi",
+            "version": "==0.3.0"
+        },
        "werkzeug": {
            "hashes": [
                "sha256:2de2a5db0baeae7b2d2664949077c2ac63fbd16d98da0ff71837f7d1dea3fd43",
--- a/control/pca9685/pca9685.py
+++ b/control/pca9685/pca9685.py
@@ -11,9 +11,28 @@
 **********************************************************************
 '''

-import smbus
+from smbus2 import SMBus
 import time
 import math
+import os
+
+
+class MockSMBus:
+    def __init__(self, bus_number):
+        self.bus_number = bus_number
+        self.data = {}
+
+    def write_byte_data(self, addr, res, value):
+        print(f"SMBus write {addr}[{res}] {value}")
+        if addr not in self.data:
+            self.data[addr] = {}
+        self.data[addr][res] = value if value else 0
+
+    def read_byte_data(self, addr, res):
+        print(f"SMBus read {addr}[{res}]")
+        if addr not in self.data:
+            self.data[addr] = {}
+        return self.data[addr][res] if res in self.data[addr] else 0


 def map_range(x, in_min, in_max, out_min, out_max):
@@ -53,7 +72,10 @@ class PWM:
            self.bus_number = 1
        else:
            self.bus_number = bus_number
-        self.bus = smbus.SMBus(self.bus_number)
+        if os.environ['PCA9685_MOCK_SMBUS'] == 'true':
+            self.bus = MockSMBus(self.bus_number)
+        else:
+            self.bus = SMBus(self.bus_number)
        self._frequency = 60

    def _debug_(self, message):
@@ -109,7 +131,7 @@ class PWM:
        prescale = math.floor(prescale_value + 0.5)
        self._debug_('Final pre-scale: %d' % prescale)

-        old_mode = self._read_byte_data(self._MODE1);
+        old_mode = self._read_byte_data(self._MODE1)
        new_mode = (old_mode & 0x7F) | 0x10
        self._write_byte_data(self._MODE1, new_mode)
        self._write_byte_data(self._PRESCALE, int(math.floor(prescale)))
--- a/control/walle.py
+++ b/control/walle.py
@@ -3,7 +3,7 @@ import collections
 from .pca9685 import ServoController, map_range
 from .dc_motor_controller import DcMotor, setup_gpio

-ServoMinMax = collections.namedtuple('ServoMinMax', ['minval', 'maxval'])
+ServoMinMax = collections.namedtuple('ServoMinMax', ['minval', 'maxval', 'restval', 'maxstep'])

 try:
    from RPi import GPIO
@@ -25,13 +25,13 @@ SERVO_NECK_TOP = 10
 SERVO_NECK_BOTTOM = 9

 SERVO_MIN_MAX = {
-    SERVO_ARM_L: ServoMinMax(50, 130),
-    SERVO_ARM_R: ServoMinMax(50, 130),
-    SERVO_EYE_L: ServoMinMax(40, 100),
-    SERVO_EYE_R: ServoMinMax(80, 120),
-    SERVO_NECK_ROTATE: ServoMinMax(60, 120),  # 60 - 90 - 120
-    SERVO_NECK_TOP: ServoMinMax(30, 180),
-    SERVO_NECK_BOTTOM: ServoMinMax(10, 180),
+    SERVO_ARM_L: ServoMinMax(50, 130, 50, 5),
+    SERVO_ARM_R: ServoMinMax(50, 130, 130, 5),
+    SERVO_EYE_L: ServoMinMax(40, 100, 40, 5),
+    SERVO_EYE_R: ServoMinMax(80, 120, 120, 5),
+    SERVO_NECK_ROTATE: ServoMinMax(60, 120, 90, 5),  # 60 - 90 - 120
+    SERVO_NECK_TOP: ServoMinMax(30, 180, 30, 5),
+    SERVO_NECK_BOTTOM: ServoMinMax(10, 180, 10, 5),
 }


@@ -42,14 +42,37 @@ class WallE:
        self.motor_b = DcMotor(PIN_MOTOR_B_ENABLE, PIN_MOTOR_B_REVERSE)
        self.servo_controller = ServoController()
        self.servo_controller.setup()
+        self.servo_positions = {}
+        self.servo_targets = {}
+
+    def setup(self):
+        for channel, min_max in SERVO_MIN_MAX.items():
+            self.servo_controller.write(channel, min_max.restval)
+            self.servo_positions[channel] = min_max.restval
+
+    def tick(self):
+        for channel, target in self.servo_targets.items():
+            current_value = self.servo_positions[channel]
+            if target == current_value:
+                del self.servo_targets[channel]
+                continue
+            try:
+                servo_min_max = SERVO_MIN_MAX[channel]
+            except IndexError:
+                continue
+            delta = abs(current_value - target)
+            step_size = min(delta, servo_min_max.maxstep)
+            new_val = current_value + step_size if target > current_value else current_value - step_size
+            self.servo_controller.write(channel, new_val)
+            self.servo_positions[channel] = new_val

    def set_servo(self, channel, value):
        try:
-            min_val, max_val = SERVO_MIN_MAX[channel]
+            servo_min_max = SERVO_MIN_MAX[channel]
        except IndexError:
            return None
-        value = min(max_val, max(min_val, value))
-        self.servo_controller.write(channel, value)
+        value = min(servo_min_max.maxval, max(servo_min_max.minval, value))
+        self.servo_targets[channel] = value

    def set_arm_l(self, val):
        self.set_servo(SERVO_ARM_L, val)
--- a/worker.py
+++ b/worker.py
@@ -1,9 +1,10 @@
 import sched
 from threading import Thread
+import json

 from redis import Redis

-# from control.walle import WallE
+from control.walle import WallE
 from control.camera import Camera

 INTERVAL_TICK = 0.05  # 20/s
@@ -21,16 +22,33 @@ class EventListener(Thread):
        for item in self.pubsub.listen():
            channel = item['channel'].decode()
            data = item['data']
-            print(channel)
-            print(data)
+            if not isinstance(data, bytes):
+                print(f"Channel {channel} got message that is not bytes {data}")
+                continue
+            try:
+                self.handle_message(channel, data)
+            except ValueError:
+                pass  # todo
+
+    def handle_message(self, channel: str, data: bytes):
+        data_dict = json.loads(data.decode())
+        if channel == 'move':
+            walle.set_movement(data_dict['angle'], data_dict['force'])
+        elif channel == 'look':
+            walle.set_eye_velocity(data_dict['angle'], data_dict['force'])
+        else:
+            print(f'Unknown channel {channel}')


 scheduler = sched.scheduler()
 camera = Camera()
+walle = WallE()
+walle.setup()


 def walle_tick():
-    pass
+    scheduler.enter(INTERVAL_TICK, 1, camera_image)
+    walle.tick()


 def camera_image():