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

No longer using this example

Browse Source
This commit is contained in:
Danny Staple 2022-11-10 21:50:14 +00:00
parent 5d25b86732
commit 80076b24bf
11 changed files with 0 additions and 575 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

128
ch-13/2-fusing-sensors/all_sensors_test/all_sensors_test.py
View File

@ -1,128 +0,0 @@
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 AllSensorsApp:
def __init__(self):
self.wifi = None
self.server = None
self.intended_speed = 0.9
self.speed_to_encoder_factor = 1/5000
self.left_speed_pid = pid.PID(0, -0.7, 0, self.intended_speed)
self.right_speed_pid = pid.PID(0, -0.7, 0, self.intended_speed)
self.start_time = time.monotonic()
self.last_time = self.start_time
self.left_dist = 0
self.right_dist = 0
self.left_pid_output = 0
self.right_pid_output = 0
self.left_speed = 0
self.right_speed = 0
# def setup_robot(self):
# robot.left_distance.distance_mode = 1
# robot.right_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 data(self, request):
imu_data = robot.imu.euler
return (
200,
[("Content-Type", "application/json")],
[
json.dumps(
{
"left_speed_pid": self.left_pid_output,
"right_speed_pid": self.right_pid_output,
"imu_z": imu_data[2],
"left_speed": self.left_speed,
"right_speed": self.right_speed,
"left_distance": self.left_dist,
"right_distance": self.right_dist,
"time": self.last_time - self.start_time,
}
)
],
)
def index(self, request):
# serve the live graph
with open("graphing.html") as fd:
return 200, [("Content-Type", "text/html")], [fd.read()]
def movement_update(self):
# calculate time delta
new_time = time.monotonic()
time_delta = new_time - self.last_time
self.last_time = new_time
self.left_speed = robot.left_encoder.get_speed(time_delta) * self.speed_to_encoder_factor
self.right_speed = robot.right_encoder.get_speed(time_delta) * self.speed_to_encoder_factor
self.left_pid_output = self.left_speed_pid.update(self.left_speed, time_delta)
self.right_pid_output = self.right_speed_pid.update(self.right_speed, time_delta)
# robot.set_left(self.left_pid_output)
# robot.set_right(self.right_pid_output)
# # do we have data
# if robot.left_distance.data_ready:
# self.left_dist = robot.left_distance.distance
# self.right_dist = robot.right_distance.distance
# # reset and loop
# robot.left_distance.clear_interrupt()
# robot.right_distance.clear_interrupt()
def main_loop(self):
# robot.left_distance.start_ranging()
# robot.right_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)
app.route("/data")(self.data)
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()
# robot.right_distance.clear_interrupt()
# robot.right_distance.stop_ranging()
AllSensorsApp().start()

1
ch-13/2-fusing-sensors/all_sensors_test/code.py
View File

@ -1 +0,0 @@
import all_sensors_test

64
ch-13/2-fusing-sensors/all_sensors_test/graphing.html
View File

@ -1,64 +0,0 @@
<!DOCTYPE html>
<b>Dist</b>
<div id="dist"><svg></svg></div>
<b>Speed</b>
<div id="speed"><svg></svg></div>
<b>Imu</b>
<div id="imu"><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
);
// render dist graph
var dist_graph = Plot.plot({
x: { grid: true},
y: { grid: true},
marks: [
Plot.line(current_dataset, {x: "time", y: "left_distance", stroke: "red"}),
Plot.line(current_dataset, {x: "time", y: "right_distance", stroke: "blue"}),
]
});
var graphNode = document.getElementById("dist");
graphNode.replaceChild(dist_graph, graphNode.firstChild);
// render speed graph
var speed_graph = Plot.plot({
x: { grid: true},
y: { grid: true},
marks: [
Plot.line(current_dataset, {x: "time", y: "left_speed", stroke: "red"}),
Plot.line(current_dataset, {x: "time", y: "right_speed", stroke: "blue"}),
Plot.line(current_dataset, {x: "time", y: "left_speed_pid", stroke: "orange"}),
Plot.line(current_dataset, {x: "time", y: "right_speed_pid", stroke: "green"}),
]
});
var graphNode = document.getElementById("speed");
graphNode.replaceChild(speed_graph, graphNode.firstChild);
// render imu graph
var speed_graph = Plot.plot({
x: { grid: true},
y: { grid: true},
marks: [
Plot.line(current_dataset, {x: "time", y: "imu_z", stroke: "red"}),
]
});
var graphNode = document.getElementById("imu");
graphNode.replaceChild(speed_graph, graphNode.firstChild);
});
}
// }
</script>

39
ch-13/2-fusing-sensors/all_sensors_test/pid.py
View File

@ -1,39 +0,0 @@
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

84
ch-13/2-fusing-sensors/all_sensors_test/pio_encoder.py
View File

@ -1,84 +0,0 @@
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])
self.previous_reading = 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]
def get_speed(self, delta_time):
new_read = self.read()
distance = new_read - self.previous_reading
self.previous_reading = new_read
return distance / delta_time

54
ch-13/2-fusing-sensors/all_sensors_test/robot.py
View File

@ -1,54 +0,0 @@
import board
import pwmio
import pio_encoder
import busio
import adafruit_vl53l1x
import adafruit_bno055
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)
# right_distance = adafruit_vl53l1x.VL53L1X(i2c0)
# left_distance = adafruit_vl53l1x.VL53L1X(i2c1)
imu = adafruit_bno055.BNO055_I2C(i2c0)
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)

31
ch-13/2-fusing-sensors/all_sensors_test/robot_wifi.py
View File

@ -1,31 +0,0 @@
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)
status_led = DigitalInOut(board.LED)
status_led.switch_to_output()
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()
status_led.value = 1
return wifi, esp

1
ch-13/2-fusing-sensors/no_imu/code.py
View File

@ -1 +0,0 @@
import all_sensors_test

42
ch-13/2-fusing-sensors/no_imu/graphing.html
View File

@ -1,42 +0,0 @@
<!DOCTYPE html>
<b>value</b>
<div id="value"><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) {
try {
const controller = new AbortController();
const timeoutId = setTimeout(() => controller.abort(), 1000);
// fetch new data
await d3.json("/data", {signal: controller.signal}).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
);
// render dist graph
var value_graph = Plot.plot({
x: { grid: true},
y: { grid: true},
marks: [
Plot.line(current_dataset, {x: "time", y: "value", stroke: "red"}),
]
});
var graphNode = document.getElementById("value");
graphNode.replaceChild(value_graph, graphNode.firstChild);
});
clearTimeout(timeoutId);
} catch (error) {
console.log(error.name === 'AbortError');
}
}
</script>

93
ch-13/2-fusing-sensors/no_imu/no_sensors.py
View File

@ -1,93 +0,0 @@
import time
import json
import traceback
from adafruit_esp32spi import adafruit_esp32spi_wsgiserver
from adafruit_wsgi.wsgi_app import WSGIApp
import robot_wifi
import random
class RandomWalkSensors:
def __init__(self):
self.wifi = None
self.server = None
self.last_value = 0
self.start_time = time.monotonic()
self.last_time = self.start_time
# self.app = None
def setup_server(self, esp, app):
self.server = adafruit_esp32spi_wsgiserver.WSGIServer(80, application=app)
adafruit_esp32spi_wsgiserver.set_interface(esp)
print("Starting server")
self.server.start()
def reconnect(self):
print(f"{time.monotonic()} Resetting esp... ")
self.wifi.reset()
print(f"{time.monotonic()} Reconnecting wifi... ")
self.wifi.connect()
print(f"{time.monotonic()} starting server...")
self.server.start()
print(f"{time.monotonic()} started server...")
def data(self, request):
return (
200,
[("Content-Type", "application/json")],
[
json.dumps(
{
"value": self.last_value,
"time": self.last_time - self.start_time,
}
)
],
)
def index(self, request):
# serve the live graph
with open("graphing.html") as fd:
return 200, [("Content-Type", "text/html")], [fd.read()]
def update(self):
# calculate time delta
new_time = time.monotonic()
time_delta = new_time - self.last_time
self.last_time = new_time
self.last_value += random.randint(-100, 100) * 0.01 * time_delta
if self.last_value > 1:
self.last_value = 1
if self.last_value < -1:
self.last_value = -1
def main_loop(self):
while True:
try:
self.update()
self.server.update_poll()
except RuntimeError as e:
traceback.print_exception(BaseException, e, e.__traceback__)
if not "Failed to send" in str(e):
self.reconnect()
def start(self):
app = WSGIApp()
app.route("/")(self.index)
app.route("/data")(self.data)
print("Starting")
self.wifi, esp, spi = robot_wifi.connect_to_wifi()
try:
self.setup_server(esp, app)
self.main_loop()
finally:
spi.unlock()
spi.deinit()
RandomWalkSensors().start()

38
ch-13/2-fusing-sensors/no_imu/robot_wifi.py
View File

@ -1,38 +0,0 @@
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)
status_led = DigitalInOut(board.LED)
status_led.switch_to_output()
print("Setting up wifi.")
spi = busio.SPI(board.GP14, MOSI=board.GP11, MISO=board.GP12)
print("SPI Configure")
esp = adafruit_esp32spi.ESP_SPIcontrol(spi, esp32_cs, esp32_ready, esp32_reset,
baudrate=300*1000
)
esp.ready_timeout = 1
print("Firmware vers.", esp.firmware_version)
wifi = adafruit_esp32spi_wifimanager.ESPSPI_WiFiManager(esp, secrets)
wifi.connect()
ip_int = ".".join(str(int(n)) for n in esp.ip_address)
print(f"IP Address is {ip_int}")
status_led.value = 1
return wifi, esp, spi