ecki/Robotics-at-Home-with-Raspberry-Pi-Pico
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Park the wifi stuff - so I can remove it.

Browse Source
This commit is contained in:
Danny Staple 2022-06-22 22:27:01 +01:00
parent 13801c8551
commit 115e9e03ef
12 changed files with 454 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
ch-10/2-graphing-d3/graphing.html Normal file
View File

@ -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>

100
ch-10/2-graphing-d3/graphing_follow.py Normal file
View File

@ -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()

42
ch-10/2-graphing-d3/pid.py Normal file
View File

@ -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

77
ch-10/2-graphing-d3/pio_encoder.py Normal file
View File

@ -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]

52
ch-10/2-graphing-d3/robot.py Executable file
View File

@ -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)

25
ch-10/2-graphing-d3/robot_wifi.py Normal file
View File

@ -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

26
ch-11/pc-live-graph/live_graph.py
View File

@ -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 _graph_source(self):
while True:
try:
with requests.get(url, timeout=1, stream=True) as response:
print(f"status: {response.status_code}")
yield from response.iter_lines()
except requests.exceptions.RequestException:
pass
def make_frame(self, frame):
try:
response = requests.get(url, timeout=1)
except requests.exceptions.RequestException:
print("Waiting...")
return
print(f"Content: {response.content}")
print(f"status: {response.status_code}")
item = response.json()
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:

14
ch-9/3-0-counting-server/counting.html Normal file
View File

@ -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>

23
ch-9/3-0-counting-server/counting.py Normal file
View File

@ -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)

63
ch-9/3-0-counting-server/robot_wifi.py Normal file
View File

@ -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

4
ch-9/3-sensors/robot.py
View File

@ -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(i2c1)
left_distance = adafruit_vl53l1x.VL53L1X(i2c0)
right_distance = adafruit_vl53l1x.VL53L1X(i2c1)
def stop():

4
ch-9/4-teleop/robot.py
View File

@ -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(i2c1)
left_distance = adafruit_vl53l1x.VL53L1X(i2c0)
right_distance = adafruit_vl53l1x.VL53L1X(i2c1)
def stop():