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

Chapter 11 and 13 - wifi code (pre bluetooth)

Browse Source
This commit is contained in:
Danny Staple
2022-08-01 09:53:21 +01:00
parent 7eff69d680
commit 6b4660eb23
17 changed files with 623 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ch-13/2-fusing-sensors/all_sensors_test/all_sensors_test.py
+42 -35
View File
@@ -9,22 +9,28 @@ import robot_wifi
import pid
class FollowObject:
class AllSensorsApp:
def __init__(self):
self.max_speed = 0.7
self.follow_pid = pid.PID(0.1, 0.1, 0.015, 15)
self.wifi = None
self.server = None
self.last_time = time.monotonic()
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.pid_output = 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
# 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.")
@@ -46,14 +52,14 @@ class FollowObject:
[
json.dumps(
{
"last_value": self.follow_pid.last_value,
"pid_output": self.pid_output,
"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,
"time": self.last_time - self.start_time,
}
)
],
@@ -65,33 +71,32 @@ class FollowObject:
return 200, [("Content-Type", "text/html")], [fd.read()]
def movement_update(self):
# do we have data
if robot.left_distance.data_ready:
self.left_dist = robot.left_distance.distance
self.right_dist = robot.right_distance.distance
# calculate time delta
new_time = time.monotonic()
time_delta = new_time - self.last_time
self.last_time = new_time
# 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
# get speeds from pid
self.pid_output = self.follow_pid.update(self.left_dist, time_delta)
speed = self.pid_output * self.max_speed
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)
self.left_speed = robot.left_encoder.get_speed(time_delta)
self.right_speed = robot.right_encoder.get_speed(time_delta)
# # do we have data
# if robot.left_distance.data_ready:
# self.left_dist = robot.left_distance.distance
# self.right_dist = robot.right_distance.distance
# make movements
robot.set_left(speed)
robot.set_right(speed)
# reset and loop
robot.left_distance.clear_interrupt()
robot.right_distance.clear_interrupt()
# # reset and loop
# robot.left_distance.clear_interrupt()
# robot.right_distance.clear_interrupt()
def main_loop(self):
robot.left_distance.start_ranging()
# robot.left_distance.start_ranging()
# robot.right_distance.start_ranging()
while True:
try:
self.movement_update()
@@ -109,13 +114,15 @@ class FollowObject:
app.route("/data")(self.data)
print("Starting")
try:
self.setup_robot()
# self.setup_robot()
self.setup_wifi(app)
self.main_loop()
finally:
robot.stop()
robot.left_distance.clear_interrupt()
robot.left_distance.stop_ranging()
# robot.left_distance.clear_interrupt()
# robot.left_distance.stop_ranging()
# robot.right_distance.clear_interrupt()
# robot.right_distance.stop_ranging()
FollowObject().start()
AllSensorsApp().start()
ch-13/2-fusing-sensors/all_sensors_test/graphing.html
+2
View File
@@ -42,6 +42,8 @@
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");
ch-13/2-fusing-sensors/all_sensors_test/robot.py
+3 -3
View File
@@ -18,10 +18,10 @@ right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21, reversed=T
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)
# i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3)
right_distance = adafruit_vl53l1x.VL53L1X(i2c0)
left_distance = adafruit_vl53l1x.VL53L1X(i2c1)
# right_distance = adafruit_vl53l1x.VL53L1X(i2c0)
# left_distance = adafruit_vl53l1x.VL53L1X(i2c1)
imu = adafruit_bno055.BNO055_I2C(i2c0)
ch-13/2-fusing-sensors/all_sensors_test/robot_wifi.py
+6
View File
@@ -16,10 +16,16 @@ def connect_to_wifi():
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
ch-13/2-fusing-sensors/no_imu/graphing.html
-1
View File
@@ -39,5 +39,4 @@
console.log(error.name === 'AbortError');
}
}
// }
</script>
ch-13/2-fusing-sensors/no_imu/no_sensors.py
+15 -12
View File
@@ -19,21 +19,20 @@ class RandomWalkSensors:
self.last_time = self.start_time
# self.app = None
def setup_wifi(self, app):
# self.app = app
print("Setting up wifi.")
self.wifi, esp = robot_wifi.connect_to_wifi()
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()
ip_int = ".".join(str(int(n)) for n in esp.ip_address)
print(f"IP Address is {ip_int}")
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):
@@ -74,17 +73,21 @@ class RandomWalkSensors:
self.server.update_poll()
except RuntimeError as e:
traceback.print_exception(BaseException, e, e.__traceback__)
self.wifi.reset()
self.reconnect()
print("Reset complete.")
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.setup_wifi(app)
self.main_loop()
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()
ch-13/2-fusing-sensors/no_imu/robot_wifi.py
+12 -5
View File
@@ -18,14 +18,21 @@ def connect_to_wifi():
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)
esp = adafruit_esp32spi.ESP_SPIcontrol(spi, esp32_cs, esp32_ready, esp32_reset)
esp.reset()
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
return wifi, esp, spi