226 lines
6.7 KiB
Python
226 lines
6.7 KiB
Python
'''
|
|
Created on Tue Jan 9 2018
|
|
|
|
@author: jan
|
|
'''
|
|
|
|
import time
|
|
import globalvars
|
|
import queue
|
|
import zmq
|
|
|
|
class PID:
|
|
def __init__(self, dev_num_adc, channel_num_adc, dev_num_dac, channel_num_dac, pid_num, kp=None, ki=None, kd=None, setpoint=None):
|
|
if kp is None:
|
|
self.kp = 1
|
|
else:
|
|
self.kp = float(kp)
|
|
|
|
if ki is None:
|
|
self.ki = 1
|
|
else:
|
|
self.ki = float(ki)
|
|
|
|
if kd is None:
|
|
self.kd = 1
|
|
else:
|
|
self.kd = float(kd)
|
|
|
|
if setpoint is None:
|
|
self.setpoint = 0.0
|
|
else:
|
|
self.setpoint = float(setpoint)
|
|
|
|
self.dev_num_adc = dev_num_adc
|
|
self.channel_num_adc = channel_num_adc
|
|
self.dev_num_dac = dev_num_dac
|
|
self.channel_num_dac = channel_num_dac
|
|
self.pid_num = pid_num
|
|
|
|
self.ITerm = 0
|
|
self.lastInput = self.setpoint
|
|
self.outMin = 0
|
|
self.outMax = 5
|
|
self.error = 0
|
|
|
|
self.start = time.time()
|
|
self.stop = 0
|
|
|
|
# Order of initialisation shouldn't matter, because zmq handel everything
|
|
self.context = zmq.Context(1)
|
|
self.SPIclient = self.context.socket(zmq.REQ)
|
|
self.SPIclient.connect("tcp://localhost:5555")
|
|
|
|
def computePID(self):
|
|
#Get ADC value
|
|
#self.out_queue.put(['ADC', self.dev_num, self.channel_num, self.pid_num])
|
|
#Input = self.in_queue.get()
|
|
msg = ['ADC', self.dev_num_adc, self.channel_num_adc, self.pid_num]
|
|
self.SPIclient.send('\t'.join(str(x) for x in msg).encode())
|
|
Input = float(self.SPIclient.recv().decode())
|
|
|
|
#Get elapsed time for right PID constants
|
|
self.stop = time.time()
|
|
elapsed_time = self.stop - self.start
|
|
self.start = time.time()
|
|
|
|
#print(type(self.ki))
|
|
Ki = float(self.ki) * elapsed_time
|
|
Kd = float(self.kd) / elapsed_time
|
|
|
|
#Calculate working variables
|
|
self.error = float(self.setpoint) - Input
|
|
dInput = Input - self.lastInput
|
|
self.ITerm = self.ITerm + (Ki * self.error)
|
|
# Check min-max for I-Term
|
|
if self.ITerm > self.outMax:
|
|
self.ITerm = self.outMax
|
|
elif self.ITerm < self.outMin:
|
|
self.ITerm = self.outMin
|
|
|
|
# Calculate PID Output
|
|
self.Output = float(self.kp) * self.error + self.ITerm - Kd * dInput
|
|
# Check min-max for output
|
|
if self.Output > self.outMax:
|
|
self.Output = self.outMax
|
|
elif self.Output < self.outMin:
|
|
self.Output = self.outMin
|
|
|
|
# Save variable for next run
|
|
self.lastInput = Input
|
|
|
|
# Set output value
|
|
#self.out_queue.put(['DAC', self.dev_num, self.channel_num, self.pid_num, self.Output])
|
|
msg = ['DAC', self.dev_num_dac, self.channel_num_dac, self.pid_num, self.Output]
|
|
self.SPIclient.send('\t'.join(str(x) for x in msg).encode())
|
|
ans = self.SPIclient.recv().decode()
|
|
#if self.in_queue.get() != 'ACK':
|
|
if ans != 'ACK':
|
|
print('Error setting DAC-value')
|
|
|
|
def setSetpoint(self, setpoint):
|
|
#self.ITerm = 0
|
|
self.setpoint = setpoint
|
|
|
|
def setKp(self,P):
|
|
self.kp=P
|
|
|
|
def setKi(self,I):
|
|
self.ki=I
|
|
|
|
def setKd(self,D):
|
|
self.kd=D
|
|
|
|
def setOutmax(self,outMax):
|
|
self.outMax = outMax
|
|
|
|
def setOutmin(self,outMin):
|
|
self.outMin = outMin
|
|
|
|
def getSetpoint(self):
|
|
return self.setpoint
|
|
|
|
def getError(self):
|
|
return self.error
|
|
|
|
def getKp(self):
|
|
return self.kp
|
|
|
|
def getKi(self):
|
|
return self.ki
|
|
|
|
def getKd(self):
|
|
return self.kd
|
|
|
|
def getOutput(self):
|
|
return self.Output
|
|
|
|
def getTemperatur(self):
|
|
return self.lastInput
|
|
|
|
def getRunValues(self):
|
|
return [self.lastInput, self.error, self.Output]
|
|
|
|
|
|
def run(pid_obj, freq, num):
|
|
context = zmq.Context()
|
|
server = context.socket(zmq.REP)
|
|
port = 5556 + num
|
|
server.bind("tcp://*:%d" % port)
|
|
poller = zmq.Poller()
|
|
poller.register(server, zmq.POLLIN)
|
|
|
|
stop = False
|
|
|
|
while not globalvars.stopall and not stop:
|
|
cmd_recv = ''
|
|
if globalvars.pidrun[num]:
|
|
pid_obj.computePID()
|
|
#try:
|
|
# msg = in_queue.get(True,1/freq)
|
|
#except queue.Empty:
|
|
# msg = ['None']
|
|
socks = dict(poller.poll(1000))
|
|
if server in socks and socks[server] == zmq.POLLIN:
|
|
cmd_recv = server.recv_string()
|
|
else:
|
|
cmd_recv = 'None'
|
|
else:
|
|
socks = dict(poller.poll(1000))
|
|
if server in socks and socks[server] == zmq.POLLIN:
|
|
cmd_recv = server.recv_string()
|
|
|
|
msg = cmd_recv.split('\t')
|
|
ans = 'ACK'
|
|
if msg[0] == 'None':
|
|
pass
|
|
elif msg[0] == 'set_setpoint':
|
|
pid_obj.setSetpoint(msg[1])
|
|
server.send_string('ACK')
|
|
elif msg[0] == 'set_kp':
|
|
pid_obj.setKp(msg[1])
|
|
server.send_string('ACK')
|
|
elif msg[0] == 'set_ki':
|
|
pid_obj.setKi(msg[1])
|
|
server.send_string('ACK')
|
|
elif msg[0] == 'set_kd':
|
|
pid_obj.setKd(msg[1])
|
|
server.send_string('ACK')
|
|
elif msg[0] == 'set_outmax':
|
|
pid_obj.setOutmax(float(msg[1]))
|
|
server.send_string('ACK')
|
|
elif msg[0] == 'set_outmin':
|
|
pid_obj.setOutmin(float(msg[1]))
|
|
server.send_string('ACK')
|
|
elif msg[0] == 'get_setpoint':
|
|
#out_queue.put(pid_obj.getSetpoint() )
|
|
server.send_string(str(pid_obj.getSetpoint()))
|
|
elif msg[0] == 'get_error':
|
|
#out_queue.put(pid_obj.getError() )
|
|
server.send_string(str(pid_obj.getError()))
|
|
elif msg[0] == 'get_kp':
|
|
#out_queue.put(pid_obj.getKp() )
|
|
server.send_string(str(pid_obj.getKp()))
|
|
elif msg[0] == 'get_ki':
|
|
#out_queue.put(pid_obj.getKi() )
|
|
server.send_string(str(pid_obj.getKi()))
|
|
elif msg[0] == 'get_kd':
|
|
#out_queue.put(pid_obj.getKd() )
|
|
server.send_string(str(pid_obj.getKd()))
|
|
elif msg[0] == 'lock':
|
|
print('Starting PID')
|
|
server.send_string('Starting PID')
|
|
elif msg[0] == 'get_output':
|
|
#out_queue.put(pid_obj.getOutput() )
|
|
server.send_string(str(pid_obj.getOutput()))
|
|
elif msg[0] == 'get_temperatur':
|
|
#out_queue.put(pid_obj.getTemperatur() )
|
|
server.send_string(str(pid_obj.getTemperatur()))
|
|
elif msg[0] == 'get_run_values':
|
|
#out_queue.put(pid_obj.getRunValues() )
|
|
server.send_string(str(pid_obj.getRunValues()))
|
|
|
|
elif msg[0] == 'END':
|
|
server.send_string('ACK')
|
|
stop = True
|