Park the wifi stuff - so I can remove it.

2022-06-22 22:27:01 +01:00
parent 13801c8551
commit 115e9e03ef
12 changed files with 454 additions and 14 deletions
@@ -0,0 +1,38 @@
 <!DOCTYPE html>
 <div id="graph"><svg></svg></div>
 <script type="module">
  import * as d3_module from "https://cdn.jsdelivr.net/npm/d3@7";
  import * as Plot from "https://cdn.skypack.dev/@observablehq/plot@0.4";
  var current_dataset = [];
  while (true) {
    // fetch new data
    await d3.json("/data").then(function(data) {
      // then append to list
      current_dataset.push(data);
      // Map to a 10s (n S) sliding window
      // ie find the most recent value, then reduce/filter anything more than 10s earlier than that.
      const most_recent_time = current_dataset[current_dataset.length - 1].time;
      const window_start = Math.max(0, most_recent_time - 10);
      // filter the list
      current_dataset = current_dataset.filter(
        value => value.time >= window_start
      );
      // generate marks
      var graph = Plot.plot({
        x: { grid: true},
        y: { grid: true},
        marks: [
          Plot.line(current_dataset, {x: "time", y: "last_value", stroke: "red"}),
          Plot.line(current_dataset, {x: "time", y: "pid_output", stroke: "blue"}),
        ]
      });
      // render graph
      var graphNode = document.getElementById("graph");
      graphNode.replaceChild(graph, graphNode.firstChild);
    });
  }
  // }
 </script>
@@ -0,0 +1,100 @@
 import time
 import json
 from adafruit_esp32spi import adafruit_esp32spi_wsgiserver
 from adafruit_wsgi.wsgi_app import WSGIApp
 import robot
 import robot_wifi
 import pid
 class FollowObject:
    def __init__(self):
        self.max_speed = 0.9
        self.follow_pid = pid.PID(0.1, 0.1, 0.015, 15)
        self.wifi = None
        self.server = None
        self.last_time = time.monotonic()
        self.left_dist = 0
        self.pid_output = 0
    def setup_robot(self):
        robot.left_distance.distance_mode = 1
    def setup_wifi(self, app):
        print("Setting up wifi.")
        self.wifi, esp = robot_wifi.connect_to_wifi()
        self.server = adafruit_esp32spi_wsgiserver.WSGIServer(80, application=app)
        adafruit_esp32spi_wsgiserver.set_interface(esp)
        print("Starting server")
        self.server.start()
        ip_int = ".".join(str(int(n)) for n in esp.ip_address)
        print(f"IP Address is {ip_int}")
    def index(self, request):
        return (
            200,
            [("Content-Type", "application/json")],
            [
                json.dumps(
                    {
                        "last_value": self.follow_pid.last_value,
                        "pid_output": self.pid_output,
                        "time": self.last_time,
                    }
                )
            ],
        )
    def movement_update(self):
        # do we have data
        if robot.left_distance.data_ready:
            self.left_dist = robot.left_distance.distance
            # calculate time delta
            new_time = time.monotonic()
            time_delta = new_time - self.last_time
            self.last_time = new_time
            # get speeds from pid
            self.pid_output = self.follow_pid.update(self.left_dist, time_delta)
            speed = self.pid_output * self.max_speed
            # make movements
            robot.set_left(speed)
            robot.set_right(speed)
            # reset and loop
            robot.left_distance.clear_interrupt()
    def main_loop(self):
        robot.left_distance.start_ranging()
        while True:
            try:
                self.movement_update()
                self.server.update_poll()
            except RuntimeError as e:
                print(f"Server poll error: {type(e)}, {e}")
                robot.stop()
                print(f"Resetting ESP...")
                self.wifi.reset()
                print("Reset complete.")
    def start(self):
        app = WSGIApp()
        app.route("/")(self.index)
        print("Starting")
        try:
            self.setup_robot()
            self.setup_wifi(app)
            self.main_loop()
        finally:
            robot.stop()
            robot.left_distance.clear_interrupt()
            robot.left_distance.stop_ranging()
 FollowObject().start()
@@ -0,0 +1,42 @@
 from black import err
 class PID:
    def __init__(self, proportional_k, integral_k, differential_k, set_point):
        self.proportional_k = proportional_k
        self.integral_k = integral_k
        self.differential_k = differential_k
        self.set_point = set_point
        self.error_sum = 0
        self.last_value = 0
        self.min_output = -1
        self.max_output = 1
        self.dead_zone = 0.3
    def update(self, measurement, time_delta):
        error_value = measurement - self.set_point
        proportional = error_value * self.proportional_k
        # calculate integral
        self.error_sum += error_value * time_delta
        # clamp it
        self.error_sum = min(self.max_output, self.error_sum)
        self.error_sum = max(self.min_output, self.error_sum)
        integral = self.error_sum * self.integral_k
        differentiated_error = (error_value - self.last_value) / time_delta
        differential = differentiated_error * self.differential_k
        self.last_value = error_value
        output = proportional + integral + differential
        # clamp output
        if abs(output) < self.dead_zone:
            output = 0
        else:
            output = min(self.max_output, output)
            output = max(self.min_output, output)
        return output
@@ -0,0 +1,77 @@
 import rp2pio
 import adafruit_pioasm
 import array
 program = """
 ; use the osr for count
 ; input pins c1 c2
    set y, 0            ; clear y
    mov osr, y          ; and clear osr
 read:
    ; x will be the old value
    ; y the new values
    mov x, y            ; store old Y in x
    in null, 32         ; Clear ISR - using y
    in pins, 2          ; read two pins into y
    mov y, isr
    jmp x!=y, different ; Jump if its different
    jmp read            ; otherwise loop back to read
 different:
    ; x has old value, y has new.
    ; extract the upper bit of X.
    in x, 31             ; get bit 31 - old p1 (remember which direction it came in)
    in null, 31         ; keep only 1 bit
    mov x, isr          ; put this back in x
    jmp !x, c1_old_zero
 c1_old_not_zero:
    jmp pin, count_up
    jmp count_down
 c1_old_zero:
    jmp pin, count_down
    ; fall through
 count_up:
    ; for a clockwise move - we'll add 1 by inverting
    mov x, ~ osr        ; store inverted OSR on x
    jmp x--, fake       ; use jump to take off 1
 fake:
    mov x, ~ x          ; invert back
    jmp send
 count_down:
    ; for a clockwise move, just take one off
    mov x, osr          ; store osr in x
    jmp x--, send       ; dec and send
 send:
    ; send x.
    mov isr, x          ; send it
    push noblock        ; put ISR into input FIFO
    mov osr, x          ; put X back in OSR
    jmp read            ; loop back
 """
 assembled = adafruit_pioasm.assemble(program)
 class QuadratureEncoder:
    def __init__(self, first_pin, second_pin, reversed=False):
        """Encoder with 2 pins. Must use sequential pins on the board"""
        self.sm = rp2pio.StateMachine(
            assembled,
            frequency=0,
            first_in_pin=first_pin,
            jmp_pin=second_pin,
            in_pin_count=2,
        )
        self.reversed = reversed
        self._buffer = array.array("i", [0])
    def read(self):
        while self.sm.in_waiting:
            self.sm.readinto(self._buffer)
        if self.reversed:
            return -self._buffer[0]
        else:
            return self._buffer[0]
@@ -0,0 +1,52 @@
 import board
 import pwmio
 import pio_encoder
 import busio
 import adafruit_vl53l1x
 motor_A1 = pwmio.PWMOut(board.GP17)
 motor_A2 = pwmio.PWMOut(board.GP16)
 motor_B1 = pwmio.PWMOut(board.GP18)
 motor_B2 = pwmio.PWMOut(board.GP19)
 right_motor = motor_A1, motor_A2
 left_motor = motor_B1, motor_B2
 right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21, reversed=True)
 left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27)
 i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1)
 i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3)
 left_distance = adafruit_vl53l1x.VL53L1X(i2c0)
 right_distance = adafruit_vl53l1x.VL53L1X(i2c1)
 def stop():
    motor_A1.duty_cycle = 0
    motor_A2.duty_cycle = 0
    motor_B1.duty_cycle = 0
    motor_B2.duty_cycle = 0
 def set_speed(motor, speed):
    # Swap motor pins if we reverse the speed
    if speed < 0:
        direction = motor[1], motor[0]
        speed = -speed
    else:
        direction = motor
    speed = min(speed, 1)  # limit to 1.0
    max_speed = 2 ** 16 - 1
    direction[0].duty_cycle = int(max_speed * speed)
    direction[1].duty_cycle = 0
 def set_left(speed):
    set_speed(left_motor, speed)
 def set_right(speed):
    set_speed(right_motor, speed)
@@ -0,0 +1,25 @@
 import board
 import busio
 from digitalio import DigitalInOut
 from adafruit_esp32spi import adafruit_esp32spi
 from adafruit_esp32spi import adafruit_esp32spi_wifimanager
 try:
    from secrets import secrets
 except ImportError:
    print("WiFi secrets are kept in secrets.py, please add them there!")
    raise
 def connect_to_wifi():
    esp32_cs = DigitalInOut(board.GP10)
    esp32_ready = DigitalInOut(board.GP9)
    esp32_reset = DigitalInOut(board.GP8)
    spi = busio.SPI(board.GP14, MOSI=board.GP11, MISO=board.GP12)
    esp = adafruit_esp32spi.ESP_SPIcontrol(spi, esp32_cs, esp32_ready, esp32_reset)
    esp.reset()
    wifi = adafruit_esp32spi_wifimanager.ESPSPI_WiFiManager(esp, secrets)
    wifi.connect()
    return wifi, esp
@@ -1,5 +1,6 @@
 """ Turn JSON data stream into graphs"""
 import requests
 import json
 import matplotlib.pyplot as plt
 from matplotlib.animation import FuncAnimation
@@ -12,22 +13,27 @@ class AnimatedGraph:
        self.fields = {}
        self.samples = 100
        self.reset()
        print("getting source to iterate over")
        self.source = self._graph_source()
        print("init completed")
    def reset(self):
        for field in self.fields:
            self.fields[field] = []
-    def make_frame(self, frame):
+    def _graph_source(self):
        while True:
            try:
-            response = requests.get(url, timeout=1)
+                with requests.get(url, timeout=1, stream=True) as response:
        except requests.exceptions.RequestException:
            print("Waiting...")
            return
        print(f"Content: {response.content}")
                    print(f"status: {response.status_code}")
                    yield from response.iter_lines()
            except requests.exceptions.RequestException:
                pass
-        item = response.json()
+    def make_frame(self, frame):
-
+        print("loading next item")
        item = json.loads(next(self.source))
        print("item loaded")
        if 'time' in self.fields and item["time"] < self.fields['time'][-1]:
            self.reset()
        for field in item:
@@ -0,0 +1,14 @@
 <!DOCTYPE html>
 <h1>Counting test</h1>
 <p>Counter: <span id="counter"> </span></p>
 <script type="module">
  import * as d3_module from "https://cdn.jsdelivr.net/npm/d3@7";
  var count_output = d3.select("span#counter");
  setInterval(function() {
    d3.json("count").then(function(data) {
      count_output.text(data)  
    });
  }, 300);
 </script>
@@ -0,0 +1,23 @@
 import json
 import robot_wifi
 from adafruit_wsgi.wsgi_app import WSGIApp
 app = WSGIApp()
 counter = iter(range(int(1e6)))
@app.route("/")
 def index(request):
    with open("counting.html") as fd:
        hello_html = fd.read()
    return 200, [("Content-Type", "text/html")], hello_html
@app.route("/count")
 def get_count(request):
    value = next(counter)
    print(f"Counter value is {value}")
    return 200, [("Content-Type", "application/json")], [json.dumps(value)]
 robot_wifi.start_wifi_server(app)
@@ -0,0 +1,63 @@
 import board
 import busio
 from digitalio import DigitalInOut
 from adafruit_esp32spi import adafruit_esp32spi
 from adafruit_esp32spi import adafruit_esp32spi_wifimanager
 from adafruit_esp32spi import adafruit_esp32spi_wsgiserver
 try:
    from secrets import secrets
 except ImportError:
    print("WiFi secrets are kept in secrets.py, please add them there!")
    raise
 def connect_to_wifi():
    esp32_cs = DigitalInOut(board.GP10)
    esp32_ready = DigitalInOut(board.GP9)
    esp32_reset = DigitalInOut(board.GP8)
    status_led = DigitalInOut(board.LED)
    status_led.switch_to_output()
    print("Starting spi bus...")
    spi = busio.SPI(board.GP14, MOSI=board.GP11, MISO=board.GP12)
    print("Creating ESP SPI control...")
    esp = adafruit_esp32spi.ESP_SPIcontrol(spi, esp32_cs, esp32_ready, esp32_reset)
    # print("Resetting...")
    print("Firmware vers.", esp.firmware_version)
    print("MAC addr:", [hex(i) for i in esp.MAC_address])
    # esp.reset()
    print("Starting Wifi Manager...")
    wifi = adafruit_esp32spi_wifimanager.ESPSPI_WiFiManager(esp, secrets)
    print("Connecting...")
    wifi.connect()
    status_led.value = 1
    return wifi, esp
 def start_wifi_server(app, background_task=None):
    print("Setting up wifi.")
    wifi, esp = connect_to_wifi()
    server = adafruit_esp32spi_wsgiserver.WSGIServer(80, application=app)
    adafruit_esp32spi_wsgiserver.set_interface(esp)
    print("Starting server")
    server.start()
    ip_int = ".".join(str(int(n)) for n in esp.ip_address)
    print(f"IP Address is {ip_int}")
    while True:
        try:
            server.update_poll()
            # background task
            if background_task:
                background_task()
        except ConnectionError:
            print("Connection error detected. Carrying on")
        except:
            print("Shutting down wifi on failure. resetting ESP")
            wifi.reset()
            raise
@@ -19,8 +19,8 @@ left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27)
 i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1)
 i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3)
-right_distance = adafruit_vl53l1x.VL53L1X(i2c0)
+left_distance = adafruit_vl53l1x.VL53L1X(i2c0)
-left_distance = adafruit_vl53l1x.VL53L1X(i2c1)
+right_distance = adafruit_vl53l1x.VL53L1X(i2c1)
 def stop():
@@ -19,8 +19,8 @@ left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27)
 i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1)
 i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3)
-right_distance = adafruit_vl53l1x.VL53L1X(i2c0)
+left_distance = adafruit_vl53l1x.VL53L1X(i2c0)
-left_distance = adafruit_vl53l1x.VL53L1X(i2c1)
+right_distance = adafruit_vl53l1x.VL53L1X(i2c1)
 def stop():