def _process(self, cargo):
""" Bring the muscle to the disered Reference and if reached go
back to HOLD """
# print 'PThread', self.id, 'process to new ref:', self.ref
self.ref_old = self.ref
self.status[self.id] = 'PROCESS'
sys_out = self.sensor.get_value()
while not isclose(sys_out, self.ref, tol=self.tol) and \
not self.stopped() and self.ref == self.ref_old:
sys_out = self.sensor.get_value()
self.rec[self.sensor.name] = sys_out
ctr_out = self.controller.output(self.ref, sys_out)
sys_in = ctrlib.sys_input(ctr_out)
self.valve.set_pwm(sys_in)
self.rec_r['r{}'.format(self.valve.name)] = self.ref
self.rec_u['u{}'.format(self.valve.name)] = ctr_out
time.sleep(self.sampling_time)
if self.stopped():
new_state = 'EXIT'
elif self.ref != self.ref_old:
new_state = 'PROCESS'
else:
new_state = 'HOLD'
# print 'PThread', self.id, 'goes to:', new_state
return (new_state, cargo)
def user_reference(cargo):
"""
Set the references for each valves to the data recieved by the comm_tread
"""
print("Arriving in USER_REFERENCE State: ")
cargo.actual_state = 'USER_REFERENCE'
while cargo.state == 'USER_REFERENCE':
try:
# read
for sensor in cargo.sens:
cargo.rec[sensor.name] = sensor.get_value()
# write
for valve, controller in zip(cargo.valve, cargo.controller):
ref = cargo.ref_task[valve.name]
sys_out = cargo.rec[valve.name]
ctr_out = controller.output(ref, sys_out)
valve.set_pwm(ctrlib.sys_input(ctr_out))
cargo.rec_r['r{}'.format(valve.name)] = ref
cargo.rec_u['u{}'.format(valve.name)] = ctr_out
for dvalve in cargo.dvalve:
state = cargo.dvalve_task[dvalve.name]
dvalve.set_state(state)
# meta
time.sleep(cargo.sampling_time)
except:
new_state = 'ERROR'
cargo.errmsg = sys.exc_info()
else:
new_state = cargo.state
return (new_state, cargo)
def set_ref(cargo):
for valve, controller in zip(cargo.valve, cargo.controller):
ref = cargo.ref_task[valve.name]
sys_out = cargo.rec[valve.name]
ctr_out = controller.output(ref, sys_out)
valve.set_pwm(ctrlib.sys_input(ctr_out))
cargo.rec_r['r{}'.format(valve.name)] = ref
cargo.rec_u['u{}'.format(valve.name)] = ctr_out
return cargo
def imu_set_ref(cargo):
''' Positions of IMUs:
< ^ >
0 ----- 1 ----- 2
|
|
|
3 ------4 ------5
< v >
In IMUcalc.calc_angle(acc0, acc1, rot_angle), "acc0" is turned by rot_angle
'''
imu_idx = {
'0': [0, 1, -90],
'1': [1, 2, -90],
'2': [1, 4, 180],
'3': [4, 1, 180],
'4': [4, 3, -90],
'5': [5, 4, -90]
}
# s = ''
imu_rec = cargo.rec_IMU
for valve, controller in zip(cargo.valve, cargo.imu_ctr):
ref = cargo.ref_task[valve.name] * 90.
idx0, idx1, rot_angle = imu_idx[valve.name]
acc0 = imu_rec[str(idx0)]
acc1 = imu_rec[str(idx1)]
sys_out = IMUcalc.calc_angle(acc0, acc1, rot_angle)
ctr_out = controller.output(ref, sys_out)
pressure = cargo.rec[valve.name]
pressure_bound = pressure_check(pressure, 1.5 * cargo.maxpressure,
1 * cargo.maxpressure)
ctr_out_ = cutoff(ctr_out + pressure_bound)
# ss = 'Channel[{}]\nangle: \t {}\tref: \t {}\tprss: \t {}\tpbound:\t {}\tdelta: \t {}\n'.format(
# valve.name, round(sys_out*100)/100., ref, round(pressure*100)/100.,
# round(pressure_bound*100)/100., round(delta*100)/100.)
# s = s + ss
# for torso, set pwm to 0 if other ref is higher:
if valve.name in ['2', '3']:
other = '2' if valve.name == '3' else '3'
other_ref = cargo.ref_task[other] * 90
if ref == 0 and ref == other_ref:
if pressure > .5:
ctr_out_ = -MAX_CTROUT
elif ref < other_ref or (ref == other_ref and ref > 0):
ctr_out_ = -MAX_CTROUT
valve.set_pwm(ctrlib.sys_input(ctr_out_))
cargo.rec_r['r{}'.format(valve.name)] = ref
cargo.rec_u['u{}'.format(valve.name)] = ctr_out
# s = s + '\n\n'
# print(s)
return cargo
def pressure_reference():
rootLogger.info("Arriving in PRESSURE_REFERENCE State: ")
llc_ref.alpha = {idx: None for idx in range(n_pc)}
while llc_ref.state == 'PRESSURE_REFERENCE':
# write
for name in PValve:
pressure_ref = llc_ref.pressure[name]
pwm = ctrlib.sys_input(pressure_ref)
PValve[name].set_pwm(pwm)
llc_rec.u[name] = pwm
set_dvalve()
# meta
time.sleep(self.sampling_time)
return llc_ref.state
def process_pattern(self, pattern):
""" Play the given pattern only once.
Args:
pattern(list): A list of lists of references
Example:
WCommander.process_pattern([[ref11, ref12, ..., ref1N, tmin1],
[ref21, ref22, ..., ref2N, tmin2],
...
[refM1, refM2, ..., refMN, tminM]])
"""
n_valves = len(self.cargo.valve)
n_dvalves = len(pattern[0]) - 1 - n_valves
for pos in pattern:
# read the refs
local_min_process_time = pos[-1]
ppos = pos[:n_valves]
dpos = pos[-n_dvalves - 1:-1]
# set d valves
for dvalve in self.cargo.dvalve:
state = dpos[int(dvalve.name)]
dvalve.set_state(state)
# hold the thing for local_min_process_time
tstart = time.time()
while time.time() - tstart < local_min_process_time:
# read
for sensor in self.cargo.sens:
self.cargo.rec[sensor.name] = sensor.get_value()
# write
for valve, controller in zip(self.cargo.valve,
self.cargo.controller):
ref = ppos[int(valve.name)]
sys_out = self.cargo.rec[valve.name]
ctr_out = controller.output(ref, sys_out)
valve.set_pwm(ctrlib.sys_input(ctr_out))
self.cargo.rec_r['r{}'.format(valve.name)] = ref
self.cargo.rec_u['u{}'.format(valve.name)] = ctr_out
# meta
time.sleep(self.cargo.sampling_time)
def angle_reference(shared_memory):
rootLogger.info("Arriving in ANGLE_REFERENCE State. ")
shared_memory.actual_state_of_mainthread = 'ANGLE_REFERENCE'
init_output()
while shared_memory.task_state_of_mainthread == 'ANGLE_REFERENCE':
shared_memory = read_sens(shared_memory)
if IMU:
set_dvalve()
shared_memory = read_imu(shared_memory)
imu_rec = shared_memory.rec_IMU
for name in ImuCtr:
ref = shared_memory.ref_task[name] * 90.
idx0, idx1 = CHANNELset[name]['IMUs']
rot_angle = CHANNELset[name]['IMUrot']
acc0 = imu_rec[idx0]
acc1 = imu_rec[idx1]
sys_out = IMUcalc.calc_angle(acc0, acc1, rot_angle)
ctr_out = ImuCtr[name].output(ref, sys_out)
pressure = shared_memory.rec[name]
pressure_bound = pressure_check(pressure,
1.5 * MAX_PRESSURE,
1 * MAX_PRESSURE)
ctr_out_ = cutoff(ctr_out + pressure_bound)
# for torso, set pwm to 0 if other ref is higher:
if name in [2, 3]:
other = 2 if name == 3 else 3
other_ref = shared_memory.ref_task[other] * 90
if ref == 0 and ref == other_ref:
if pressure > .5:
ctr_out_ = -MAX_CTROUT
elif ref < other_ref or (ref == other_ref and ref > 0):
ctr_out_ = -MAX_CTROUT
pwm = ctrlib.sys_input(ctr_out_)
PValve[name].set_pwm(pwm)
shared_memory.rec_u[name] = pwm
shared_memory.rec_angle[name] = round(sys_out, 2)
time.sleep(shared_memory.sampling_time)
new_state = shared_memory.task_state_of_mainthread
return (new_state, shared_memory)
def _hold(self, cargo):
""" Hold the reference as long as WalkingCommander set a new one"""
# print 'PThread', self.id, 'hold ref:', self.ref
self.status[self.id] = 'HOLD'
while self.ref == self.ref_old and not self.stopped():
sys_out = self.sensor.get_value()
self.rec[self.sensor.name] = sys_out
ctr_out = self.controller.output(self.ref, sys_out)
sys_in = ctrlib.sys_input(ctr_out)
self.valve.set_pwm(sys_in)
self.rec_r['r{}'.format(self.valve.name)] = self.ref
self.rec_u['u{}'.format(self.valve.name)] = ctr_out
time.sleep(self.sampling_time)
if self.stopped():
new_state = 'EXIT'
else:
new_state = 'PROCESS'
# print 'PThread', self.id, 'goes to:', new_state
return (new_state, cargo)
def pressure_reference(shared_memory):
"""
Set the references for each valves to the data recieved by the UI_tread
"""
rootLogger.info("Arriving in PRESSURE_REFERENCE State: ")
shared_memory.actual_state_of_mainthread = 'PRESSURE_REFERENCE'
while shared_memory.task_state_of_mainthread == 'PRESSURE_REFERENCE':
# read
shared_memory = read_sens(shared_memory)
# write
for name in PValve:
ref = shared_memory.ref_task[name]
sys_out = shared_memory.rec[name]
ctr_out = Ctr[name].output(ref, sys_out)
pwm = ctrlib.sys_input(ctr_out)
PValve[name].set_pwm(pwm)
shared_memory.rec_u[name] = pwm
set_dvalve()
# meta
time.sleep(shared_memory.sampling_time)
new_state = shared_memory.task_state_of_mainthread
return (new_state, shared_memory)
def angle_reference():
rootLogger.info("Arriving in ANGLE_REFERENCE State. ")
init_output()
while llc_ref.state == 'ANGLE_REFERENCE':
if IMU:
set_dvalve()
read_imu()
imu_rec = self.IMU
for name in ImuCtr:
ref = llc_ref.alpha[name]
idx0, idx1 = CHANNELset[name]['IMUs']
rot_angle = CHANNELset[name]['IMUrot']
acc0 = imu_rec[idx0]
acc1 = imu_rec[idx1]
sys_out = IMUcalc.calc_angle(acc0, acc1, rot_angle)
pressure_ref = ImuCtr[name].output(ref, sys_out)
# for torso, set pressure to 0 if other ref is higher:
if name in [2, 3]:
other = 2 if name == 3 else 3
other_ref = llc_ref.pressure[other]
if ref == 0 and other_ref == 0:
if pressure_ref > .2:
pressure_ref = 0
elif (ref < other_ref
or (ref == other_ref and ref > 0)):
pressure_ref = 0
pwm = ctrlib.sys_input(pressure_ref)
PValve[name].set_pwm(pwm)
llc_ref.pressure[name] = pressure_ref
llc_rec.u[name] = pwm
llc_rec.aIMU[name] = round(sys_out, 2)
time.sleep(self.sampling_time)
return llc_ref.state
