Blame view
software/servo/servo2to1/servo2to1_core.py
8.74 KB
14b996da3
First commit
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 |
# -*- coding: utf-8 -*-
"""
author Benoit Dubois
copyright FEMTO Engineering, 2021
licence GPL v3+
brief Generic (core) controller composed of 2 inputs 2 servo and 1 output:
details
-------
INPUT 1 >---|PID 1|---|
------- |
|-----> OUTPUT
------- |
INPUT 2 >---|PID 2|---|
-------
"""
import pyb
import ucollections
import uselect
from servo_serial_com import ServoSerialCom
MNEMO_LIST = ["kp", "ki", "sp", "osp", "imax", "omax", "oscale", "fs"]
# Avoid use of timer 2, 3, 5 and 6
# see http://docs.micropython.org/en/latest/pyboard/library/pyb.Timer.html
TIMER_MONITOR_ID = 9
TIMER_MONI_FREQ = 0.35
PIN_KP1 = 'PE0'
PIN_KI1 = 'PE2'
PIN_KP2 = 'PE4'
PIN_KI2 = 'PE6'
#----------------------------------------------------------
class Servo2to1(object):
"""A class defining a servo with two inputs and one output. You can select
the input to be used by the servo. Each input used a different controller,
so you can adjust different control parameters.
Note:
- servo can be disabled by selecting input '0',
- input '0' is selected at initialization,
- if input is changed, integrator of selected controller is reseted.
Note on implementation:
- inputs must have a read() method,
- controllers must have a compute() and reset() methods,
- output must have a write() method.
"""
def __init__(self, in1, in2, ctrl1, ctrl2, out, timer, freq):
self.in1 = in1
self.in2 = in2
self.ctrl1 = ctrl1
self.ctrl2 = ctrl2
self.out = out
self.timer = timer
self.cfreq = freq
self.freq1 = freq
self.freq2 = freq
self.ctrl = 1
self.yn = 0 # Needed for monitoring
self.un = 0 # Needed for monitoring
def compute(self):
if self.ctrl == 1:
self.yn = self._value1()
self.un = self._compute1(self.yn)
elif self.ctrl == 2:
self.yn = self._value2()
self.un = self._compute2(self.yn)
else: # Servo disabled
self.un = 0
self.yn = 0
return
self._set_value(self.un)
@property
def ctrl(self):
return self._ctrl
@ctrl.setter
def ctrl(self, value):
if value == 1:
self._reset1()
elif value == 2:
self._reset2()
self._ctrl = value
@property
def cfreq(self):
return self.timer.freq()
@cfreq.setter
def cfreq(self, value):
self.timer.freq(value)
def _value1(self):
return self.in1.read()
def _value2(self):
return self.in2.read()
def _compute1(self, value):
return self.ctrl1.compute(value)
def _compute2(self, value):
return self.ctrl2.compute(value)
def _reset1(self):
self.ctrl1.reset()
def _reset2(self):
self.ctrl2.reset()
def _set_value(self, value):
self.out.write(value)
#----------------------------------------------------------
class Servo2to1Core(object):
def __init__(self, servo):
self.task = 0
self.switches, self.irqs = self.build_switches()
self.servo = servo
self.scom = ServoSerialCom()
self.init_servo_parameters()
self.log_stack = ucollections.deque((), 5)
# servo.timer is used to sync servo update.
self.servo.timer.callback(lambda f: self.callback_servo())
# Init servo and timer_servo
self.handle_servo_state()
def callback_servo(self):
"""Executed when timer_servo is triggered.
"""
self.task = 1
def callback_kx(self, line):
"""Executed when a changement on a Kx switches is detected.
"""
self.task = 2
def apply_cmd(self, cmd):
if len(cmd) == 1: # Request
pass
else:
if cmd[0] == "kp":
if cmd[1] == '0':
self.servo.ctrl1.kp = cmd[2]
elif cmd[1] == '1':
self.servo.ctrl2.kp = cmd[2]
elif cmd[0] == "ki":
if cmd[1] == '0':
self.servo.ctrl1.ki = cmd[2]
elif cmd[1] == '1':
self.servo.ctrl2.ki = cmd[2]
elif cmd[0] == "sp":
if cmd[1] == '0':
self.servo.ctrl1.sp = cmd[2]
elif cmd[1] == '1':
self.servo.ctrl2.sp = cmd[2]
elif cmd[0] == "osp":
if cmd[1] == '0':
self.servo.ctrl1.osp = cmd[2]
elif cmd[1] == '1':
self.servo.ctrl2.osp = cmd[2]
elif cmd[0] == "imax":
if cmd[1] == '0':
self.servo.ctrl1.imax = cmd[2]
elif cmd[1] == '1':
self.servo.ctrl2.imax = cmd[2]
elif cmd[0] == "omax":
if cmd[1] == '0':
self.servo.ctrl1.omax = cmd[2]
elif cmd[1] == '1':
self.servo.ctrl2.omax = cmd[2]
elif cmd[0] == "oscale":
if cmd[1] == '0':
self.servo.ctrl1.oscale = cmd[2]
elif cmd[1] == '1':
self.servo.ctrl2.oscale = cmd[2]
elif cmd[0] == "fs":
if cmd[1] == '0':
self.servo.freq1 = cmd[2]
elif cmd[1] == '1':
self.servo.freq2 = cmd[2]
def handle_servo_state(self):
"""Handle controller to be used with respect to switche value.
Note: Controller 1 have priority over controller 2.
"""
if self.switches['p2'].value() == 1:
self.servo.ctrl = 2
self.servo.timer.freq(self.servo.freq2)
# Avoid spike during switch to controller 2
self.servo.ctrl2.osp = self.servo.ctrl1.un
if self.switches['i2'].value() == 1:
self.servo.ctrl2.enable_int(True)
else:
self.servo.ctrl2.enable_int(False)
elif self.switches['p1'].value() == 1:
self.servo.ctrl = 1
self.servo.timer.freq(self.servo.freq1)
if self.switches['i1'].value() == 1:
self.servo.ctrl1.enable_int(True)
else:
self.servo.ctrl1.enable_int(False)
else:
self.servo.ctrl = 0
def init_servo_parameters(self):
self.servo.ctrl1.kp = 0
self.servo.ctrl1.ki = 0
self.servo.ctrl1.sp = 0
self.servo.ctrl2.kp = 0
self.servo.ctrl2.ki = 0
self.servo.ctrl2.sp = 0
def build_switches(self):
switches = {'p1': pyb.Pin(PIN_KP1),
'i1': pyb.Pin(PIN_KI1),
'p2': pyb.Pin(PIN_KP2),
'i2': pyb.Pin(PIN_KI2)}
irqs = {'p1': pyb.ExtInt(switches['p1'],
pyb.ExtInt.IRQ_RISING_FALLING,
pyb.Pin.PULL_UP,
self.callback_kx),
'i1': pyb.ExtInt(switches['i1'],
pyb.ExtInt.IRQ_RISING_FALLING,
pyb.Pin.PULL_UP,
self.callback_kx),
'p2': pyb.ExtInt(switches['p2'],
pyb.ExtInt.IRQ_RISING_FALLING,
pyb.Pin.PULL_UP,
self.callback_kx),
'i2': pyb.ExtInt(switches['i2'],
pyb.ExtInt.IRQ_RISING_FALLING,
pyb.Pin.PULL_UP,
self.callback_kx)}
return switches, irqs
def run(self):
while True:
if self.task == 2: # Handle events on switches
self.handle_servo_state()
self.task = 0
elif self.task == 1: # Servo computation process
self.servo.compute()
self.log_stack.append("{:d}\t{:d}\t{:d}\t{:d}
".format(
self.servo.yn,
self.servo.un,
self.servo.ctrl1.integrator,
self.servo.ctrl2.integrator))
self.task = 0
else: # => task = 0, Handle R/W on serial interface
events = self.scom.poller.poll(250)
if self.scom.vcom.any():
msg = self.scom.read_message()
if msg is not None:
cmd = self.scom.decode_message(msg)
if cmd is not None:
self.apply_cmd(cmd)
try:
message = self.log_stack.popleft()
except IndexError:
pass
else:
self.scom.write_message_simple(message)
|