def init_controller():
"""
Initialize the set of controllers. At moment only PID Controller are
implemented.
If you want to use other controllers, just construct a class
that inherits from the abstract Class controller.controller. Then you are
forced to use the supported interface.
The default gainz (P, I and D) are hardcoded at the beginning of
*server.py*, but can easily be changed via the user interface of the
client.
Return:
(list of controller.PIDController)
"""
tsamplingPID = TSAMPLING
maxoutPID = MAX_CTROUT
controller = []
sets = [{'name': '0', 'P': PID[0], 'I': PID[1], 'D': PID[2]},
{'name': '1', 'P': PID[0], 'I': PID[1], 'D': PID[2]},
{'name': '2', 'P': PID[0], 'I': PID[1], 'D': PID[2]},
{'name': '3', 'P': PID[0], 'I': PID[1], 'D': PID[2]},
{'name': '4', 'P': PID[0], 'I': PID[1], 'D': PID[2]},
{'name': '5', 'P': PID[0], 'I': PID[1], 'D': PID[2]},
{'name': '6', 'P': PID[0], 'I': PID[1], 'D': PID[2]},
{'name': '7', 'P': PID[0], 'I': PID[1], 'D': PID[2]}]
for elem in sets:
controller.append(
ctrlib.PidController([elem['P'], elem['I'], elem['D']],
tsamplingPID, maxoutPID))
return controller
def init_channels():
"""
Initialize the software representation of the hardware, i.e.
Sensors, Proportional Valves, and Discrete Valves
"""
rootLogger.info("Initialize IMUs ...")
# mplx address for IMU is 0x71
imu_set = [imu for imu in IMUset]
imu_used_ = [CHANNELset[name]['IMUs'] for name in CHANNELset]
while [None] in imu_used_:
imu_used_.remove([None])
imu_used = list(np.unique([imu for subl in imu_used_ for imu in subl]))
for imu in imu_used:
if imu not in imu_set and imu is not None:
raise KeyError(
'IMU with name "{}"'.format(imu) + ' is used for angle' +
'calculation, but is not in the set of connected IMUs')
try:
IMU = {}
for name in IMUset:
IMU[name] = sensors.MPU_9150(name=name, mplx_id=IMUset[name]['id'])
except IOError: # not connected
IMU = False
rootLogger.info("IMU detected?: {}".format(not (not (IMU))))
rootLogger.info("Initialize Channels ...")
PSens, PValve, DValve, Controller, Imu_controller = {}, {}, {}, {}, {}
for name in CHANNELset:
PSens[name] = sensors.DPressureSens(
name=name,
mplx_id=CHANNELset[name]['PSensid'],
maxpressure=MAX_PRESSURE)
PValve[name] = actuators.Valve(name=name,
pwm_pin=CHANNELset[name]['pin'])
Controller[name] = ctrlib.PidController(CHANNELset[name]['ctr'],
TSAMPLING, MAX_CTROUT)
if CHANNELset[name]['IMUs'][-1]:
Imu_controller[name] = ctrlib.PidController(
CHANNELset[name]['ctrIMU'], TSAMPLING, MAX_CTROUT)
for name in DiscreteCHANNELset:
DValve[name] = actuators.DiscreteValve(
name=name, pin=DiscreteCHANNELset[name]['pin'])
return PSens, PValve, DValve, IMU, Controller, Imu_controller
def setUp(self):
gain_p = 1
gain_i = 1
gain_d = 1
maxoutput = 100
tsampling = .015
self.pid_controller =\
controller.PidController([gain_p, gain_i, gain_d],
tsampling, maxoutput)
if Gss.isctime():
Gss = Gss.sample(ts, method='bilinear')
def set_stateG():
return [[0]]
stateG = set_stateG()
# Controller
Kp = 1.3
Ti = .55
Td = .2
gam = 0.1
Ctr = ctrlib.PidController([Kp, Ti, Td], ts, 1)
Ctr_unlim = ctrlib.PidController([Kp, Ti, Td], ts, 100)
Ctr_lim = ctrlib.PidController_WindUp([Kp, Ti, Td], ts, 1)
Ctr_sympy = ctrlib.PidController_SymPy([Kp, Ti, Td], ts, 1)
# Ti = Kp/Ki
# Td = Kd/Kp
a = [gam * Ti * Td, Ti, 0]
b = [(1 + gam) * Kp * Ti * Td, Kp * Ti + gam * Kp * Td, Kp]
C = control.tf(b, a)
Css = control.tf2ss(C)
if Css.isctime():
Css = Css.sample(ts, method='bilinear')