Pitch Control#
r –> desired
y –> actual pitch
u –> duty
%connect serial:///dev/ttyAMA1
Connected to robot-stm32 @ serial:///dev/ttyAMA1
PID controller:
%%writefile $IOT_PROJECTS/robot/code/stm32/lib/PID.py
# Based on https://github.com/br3ttb/Arduino-PID-Library
from array import array
# config vector (may be dynamically updated)
SETPOINT = const(0) # setpoint
KP = const(1) # proportional term
KI = const(2) # scaled by /fs
KD = const(3) # scaled by *fs
U_MIN = const(4) # minimum PID output (anti-windup)
U_MAX = const(5) # maximum PID output (anti-windup)
# state (used internally)
_SUM = const(0)
_Y = const(1)
class PID:
def __init__(self, config):
self.config = config
self.state = array('f', [0, 0])
def update(self, y):
"""compute & return new PID output u from plant output y"""
c = self.config
s = self.state
err = c[SETPOINT] - y
s[_SUM] += self._clip(c[KI] * err) # integrator state
u = self._clip(c[KP] * err + s[_SUM] - c[KD] * (y - s[_Y]))
s[_Y] = y # save last y (for KD term)
return u
def _clip(self, value):
c = self.config
if value > c[U_MAX]: return c[U_MAX]
if value < c[U_MIN]: return c[U_MIN]
return value
Writing /home/iot/iot49.org/docs/projects/robot/code/stm32/lib/PID.py
Pitch controller:
%%writefile $IOT_PROJECTS/robot/code/stm32/lib/pitch_control.py
from controller import *
from param import PARAM, PARAM_RESERVED
from pid import PID
P0 = const(2)
assert P0 == PARAM_RESERVED
SET_PITCH = const(P0+0)
KP = const(P0+1)
KI = const(P0+2)
KD = const(P0+3)
U_MIN = const(P0+4)
U_MAX = const(P0+5)
PARAM[U_MIN] = -100
PARAM[U_MAX] = 100
class Control(Controller):
def __init__(self, uart):
super().__init__(uart)
self.pid = PID(memoryview(PARAM)[SET_PITCH:U_MAX+1])
def update(self):
state = self.state
pitch = state[STATE_PITCH]
if abs(pitch) < 30:
duty = self.pid.update(pitch)
duty = self.fix_duty(duty)
else:
duty = 0
state[STATE_DUTY1] = state[STATE_DUTY2] = duty
@staticmethod
def fix_duty(duty):
if abs(duty) > 1:
duty = duty+8.5 if duty>0 else duty-8.5
return duty
Writing /home/iot/iot49.org/docs/projects/robot/code/stm32/lib/pitch_control.py
Run the show …
%%host
import nest_asyncio, sys, os
nest_asyncio.apply()
sys.path.append(os.path.join(os.getenv('IOT_PROJECTS'), 'robot/code/rpi'))
# fix wiring issue
from gpiozero import Button as Pin
try:
Pin(14, pull_up=False)
except:
pass
from asyncio_mqtt import Client
from struct import pack
import numpy as np
import asyncio, json, os
import stm32
from robot import *
print(f"plot @ http://{os.getenv('DNS_NAME')}.local:5006")
MQTT_BROKER = os.getenv("HOST_IP")
TOPIC_ROOT = "public/vis"
P0 = PARAM_RESERVED
SET_PITCH = const(P0+0)
KP = const(P0+1)
KI = const(P0+2)
KD = const(P0+3)
U_MIN = const(P0+4)
U_MAX = const(P0+5)
FS = 20 # controller update rate
PITCH0 = 5 # deg
class TestRemote(Remote):
async def handle(self, dt: float, code: str, value: float):
if value < 0:
print(f"PID {code} = {value} < 0 (ignored)")
return
value *= 0.001
if code == '1':
await self.robot.set(KP, value*FS)
elif code == '2':
await self.robot.set(KI, value)
elif code == '3':
await self.robot.set(KD, value*FS*FS)
class Control:
def __init__(self):
self.pitch = 90
async def main(self):
self.publish = [ "time [s]", "pitch", "duty", "dt1 [0.1ms]", "dt2 [0.1ms]" ]
async with Client(MQTT_BROKER) as client, \
Comm(self.state_listener) as robot, \
TestRemote() as remote:
remote.robot = robot
self.client = client
await client.publish(f"{TOPIC_ROOT}/new", json.dumps({
"columns": self.publish,
"rollover": 1000,
"args": { "title": f"Robot pitch" },
}))
await robot.set(PARAM_FS, FS)
await robot.set(SET_PITCH, PITCH0)
await robot.start("pitch_control")
# wait for upright position
while abs(self.pitch-PITCH0) > 3:
print(f"orient robot upright, pitch {self.pitch-PITCH0:8.2f} --> 0")
await asyncio.sleep(1)
# balancing ...
await robot.set(KP, 1)
while abs(self.pitch-PITCH0) < 60:
# print(f"balancing, pitch = {self.pitch-PITCH0:8.2f}")
await asyncio.sleep(1)
print("main DONE", self.pitch-PITCH0)
async def state_listener(self, state):
try:
t = state[STATE_K]/FS
duty = state[STATE_DUTY1]
self.pitch = state[STATE_PITCH]
dt1 = state[STATE_DT1]
dt2 = state[STATE_DT2]
await self.client.publish(
f"{TOPIC_ROOT}/bin",
pack(f'!{len(self.publish)}f', t, self.pitch, duty, dt1/100, dt2/100))
except Exception as e:
print("***** state_listener:", e)
async def handle(self, dt: float, code: str, value: float):
if value < 0:
print(f"PID {code} = {value} < 0 (ignored)")
return
value *= 0.001
if code == '1':
await self.robot.set(KP, value*FS)
elif code == '2':
await self.robot.set(KI, value)
elif code == '3':
await self.robot.set(KD, value*FS*FS)
stm32.rsync()
try:
c = Control()
asyncio.run(c.main())
except KeyboardInterrupt:
pass
print(f"kp = {PARAM[KP]}")
print(f"ki = {PARAM[KI]}")
print(f"kd = {PARAM[KD]}")
plot @ http://pi4robot.local:5006
Directories match
MCU: start Comm @ 1000000 baud
scanning for iot49-robot
connecting to iot49-robot ... connected
orient robot upright, pitch 85.00 --> 0
orient robot upright, pitch -93.78 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.74 --> 0
orient robot upright, pitch -93.74 --> 0
orient robot upright, pitch -93.74 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.75 --> 0
orient robot upright, pitch -93.59 --> 0
orient robot upright, pitch -92.07 --> 0
orient robot upright, pitch -57.22 --> 0
orient robot upright, pitch -82.29 --> 0
orient robot upright, pitch -33.39 --> 0
orient robot upright, pitch -8.73 --> 0
orient robot upright, pitch -6.09 --> 0
main DONE -63.68894958496094
kp = 1.0
ki = 0.0
kd = 0.0