Blame view
software/servo/doc/save_servo/STM32_controller.py
2.66 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 |
import pyb, micropython, ujson
micropython.alloc_emergency_exception_buf(100)
from VNH5019 import VNH5019
from PID import PID
class controller(object):
def __init__(self, config_filename = 'pid_config.json'):
self.mesTemp = pyb.ADC(pyb.Pin('PC4'))
self.mesPh = pyb.ADC(pyb.Pin('PC5'))
self.vnh = VNH5019()
self.config_filename = config_filename
self.configure()
def cb(self, tim):
self.y01 = self.mesPh.read()
self.u1 = min(4000, max(-4000, self.myPID1.compute(self.y01)))
self.vnh.setPWM1(self.u1)
self.y02 = self.mesTemp.read()
self.u2 = min(4000, max(-4000, self.myPID2.compute(self.y02)))
self.vnh.setPWM2(self.u2)
def start(self):
print('Start')
self.timer.callback(self.cb)
#self.wait_for_restart()
def stop(self):
self.timer.callback(None)
#self.debug.callback(None)
self.vnh.setPWM1(0)
self.vnh.setPWM2(0)
print('Stop')
def wait_for_restart(self):
while pyb.Switch()() == False:
pyb.delay(20)
self.restart()
def restart(self):
self.stop()
pyb.delay(1000)
self.configure()
self.start()
def configure(self):
self.load_config()
#self.myPID1 = PID(self.config['kp1'], self.config['ki1'], self.config['sp1'])
self.myPID1 = PID(2000, 5, 1125)
self.myPID2 = PID(self.config['kp2'], self.config['ki2'], self.config['sp2'])
self.myPID2.stop()
self.u1 = 0
self.y01 = 0
self.u2 = 0
self.y02 = 0
self.timer = pyb.Timer(1, freq = 1000)
#self.debug = pyb.Timer(3, freq=10)
#self.debug.callback(lambda t:print(self.config['sp1'], self.y01, self.u1, self.config['sp2'], self.y02, self.u2))
def disp(self, tim = None):
print(self.config['sp1'], self.y01, self.u1, self.config['sp2'], self.y02, self.u2)
def logON(self, f = 1):
self.debug = pyb.Timer(3, freq = int(f))
self.debug.callback(self.disp)
def logOFF(self):
self.debug.callback(None)
def load_config(self):
try:
with open(self.config_filename, 'r') as f:
self.config = ujson.loads(f.read())
except:
config = {'kp1': 0, 'ki1': 0, 'sp1': 1450, 'kp2': 0, 'ki2': 0, 'sp2': 1500}
with open(self.config_filename, 'w') as f:
config_json = ujson.dumps(config)
f.write("%s" % config_json)
finally:
with open(self.config_filename, 'r') as f:
self.config = ujson.loads(f.read())
if __name__=='__main__':
myLock = controller()
myLock.start()
|