def run(self):
""" run HUI """
rootLogger.info('Running HIGHLEVEL CTR Thread ...')
automat = state_machine.StateMachine()
automat.add_state('MODE1', mode1)
automat.add_state('MODE2', mode2)
automat.add_state('MODE3', mode3)
automat.add_state('EXIT', exit_cleaner)
automat.add_state('QUIT', None, end_state=True)
automat.set_start('MODE1')
try:
automat.run()
except Exception as err:
rootLogger.exception('\n exception HIGHLEVEL CTR Thread \n')
rootLogger.error(err, exc_info=True)
rootLogger.info('HIGHLEVEL CTR Thread is done ...')
def __init__(self, valve, sensor, controller, ident, status, rec, rec_r,
rec_u, sampling_time):
""" This class provide the autonomous ability to act for every muscle
of the soft robot.
You can set a reference and the Thread will bring the muscle to it
and stay there until you set another one.
Furthermore this Thread has a kind of stop_button. Everybody can push
it and cancel the Thread this way.
Thread class with a stop() method. The thread itself has to check
regularly for the stopped() condition.
Args:
valve(Actuators.ProportionalValve): valve this Thread
should control
sensor(Sensor.PressureSensor): sensor which belongs to valve
controller(Controller.Controller): plug in what you like
ident(int): ID of valve in context of WalkingCommander class
status(list): list of status for the WalkingCommander himself
rec(dict): The Recorder of the server
"""
super(Thread_ProportinalValve, self).__init__()
self.valve = valve
self.sensor = sensor
self.controller = controller
self.rec = rec
self.rec_r = rec_r
self.rec_u = rec_u
self.sampling_time = sampling_time
self._stop_event = threading.Event()
self.status = status
self.id = ident
self.status[self.id] = 'initialized'
self.ref = 0.0
self.ref_old = 0.0
self.tol = TOL
self.automat = state_machine.StateMachine()
self.automat.add_state('HOLD', self._hold)
self.automat.add_state('PROCESS', self._process)
self.automat.add_state('EXIT', None, end_state=True)
self.automat.set_start('HOLD')
def run(self):
""" run HUI """
self.rootLogger.info('Running HUI Thread ...')
""" ---------------- ----- ------- ----------------------------- """
""" ---------------- HELP FUNCTIONS ------------------------- """
""" ---------------- ----- ------- ----------------------------- """
def change_state_in_main_thread(state):
self.shared_memory.task_state_of_mainthread = state
# while not self.shared_memory.actual_state_of_mainthread == state:
# time.sleep(self.shared_memory.sampling_time)
def mode_changed():
new_state = None
if GPIO.event_detected(MODE1):
new_state = MODE[1]['ui_state']
elif GPIO.event_detected(MODE2):
new_state = MODE[2]['ui_state']
elif GPIO.event_detected(MODE3):
new_state = MODE[3]['ui_state']
elif self.ui_state == 'EXIT':
new_state = 'EXIT'
change = False
if new_state and time.time()-self.lastchange > 1:
if (all_potis_zero() and new_state is not self.ui_state
or new_state == 'EXIT'):
change = True
self.ui_state = new_state
self.rootLogger.info("UI goes to: {}".format(new_state))
reset_events()
self.lastchange = time.time()
else:
flicker_leds()
return change
def fun1():
if GPIO.event_detected(FUN1):
if time.time()-self.lastfun1 > 1:
state = self.fun1
self.fun1 = not state
self.rootLogger.info('Fun1 turned {}'.format(not state))
self.lastfun1 = time.time()
return self.fun1
def fun2():
if GPIO.event_detected(FUN2):
if time.time()-self.lastfun2 > 1:
state = self.fun2
self.fun2 = not state
self.rootLogger.info('Fun2 turned {}'.format(not state))
self.lastfun2 = time.time()
return self.fun2
def is_userpattern():
if all_potis_zero():
if self.user_pattern:
self.user_pattern = False
self.shared_memory.pattern = \
self.shared_memory.ptrndic[
self.shared_memory.ptrndic['selected']]
self.rootLogger.info('user_pattern was turned False')
else:
if not self.user_pattern:
self.user_pattern = True
self.rootLogger.info('user_pattern was turned True')
return self.user_pattern
def reset_events():
for btn in BTNS:
GPIO.event_detected(btn)
self.fun1 = False
self.fun2 = False
def set_leds():
my_state = self.ui_state
for pin, state in [(MODE1LED, MODE[1]['ui_state']),
(MODE2LED, MODE[2]['ui_state']),
(MODE3LED, MODE[3]['ui_state'])]:
GPIO.output(pin, GPIO.HIGH if my_state == state else GPIO.LOW)
for pin, state in [(FUN1LED, self.fun1),
(FUN2LED, self.fun2)]:
GPIO.output(pin, GPIO.HIGH if state else GPIO.LOW)
def select_pattern():
patterns = [
name for name in sorted(iter(self.shared_memory.ptrndic.keys()))]
patterns.remove('selected')
current_selection = self.shared_memory.ptrndic['selected']
select = None
idx = patterns.index(current_selection)
self.lcd.color = [100, 0, 0]
self.lcd.message = patterns[idx]
while not mode_changed() and select is None:
if self.lcd.up_button:
idx = idx - 1 if idx > 0 else len(patterns) - 1
self.lcd.clear()
self.lcd.message = patterns[idx]
elif self.lcd.down_button:
idx = idx + 1 if idx < len(patterns) - 1 else 0
self.lcd.clear()
self.lcd.message = patterns[idx]
elif self.lcd.select_button or fun1():
self.lcd.clear()
self.lcd.message = "SELECTED"
select = patterns[idx]
self.shared_memory.ptrndic['selected'] = patterns[idx]
time.sleep(.2)
self.lcd.clear()
self.lcd.color = [0, 0, 0]
if select:
return self.shared_memory.ptrndic[
self.shared_memory.ptrndic['selected']]
else:
return None
""" ---------------- ----- ------- ----------------------------- """
""" ---------------- STATE HANDLERS ------------------------- """
""" ---------------- ----- ------- ----------------------------- """
def exit_cleaner():
change_state_in_main_thread('EXIT')
return ('QUIT')
def pause_state():
while not mode_changed():
time.sleep(UI_TSAMPLING)
set_leds()
return (self.ui_state)
def pwm_feed_through():
change_state_in_main_thread(MODE[1]['main_state'])
while not mode_changed():
cref = read_potis()
for idx in self.shared_memory.pwm_task:
self.shared_memory.pwm_task[idx] = cref[idx]*100
dref = read_switches()
for idx in self.shared_memory.dvalve_task:
self.shared_memory.dvalve_task[idx] = dref[idx]
time.sleep(UI_TSAMPLING)
set_leds()
return (self.ui_state)
def user_reference():
while not mode_changed():
change_state_in_main_thread(MODE[2]['main_state'][fun2()])
refzero = fun1()
cref = (read_potis() if not refzero else
{idx: 0.0 for idx in self.shared_memory.ref_task})
for idx in self.shared_memory.ref_task:
self.shared_memory.ref_task[idx] = cref[idx]
dref = read_switches()
for idx in self.shared_memory.dvalve_task:
self.shared_memory.dvalve_task[idx] = dref[idx]
time.sleep(UI_TSAMPLING)
set_leds()
return (self.ui_state)
def pattern_reference():
# first select pattern
set_leds()
pattern = select_pattern()
if pattern:
self.shared_memory.pattern = pattern
# always start with ref0
self.ptrn_idx = 0
initial_cycle, initial_cycle_idx = True, 0
VIDEO = True
while not mode_changed():
change_state_in_main_thread(MODE[3]['main_state'][fun2()])
if is_userpattern():
cref = read_potis().values()
self.shared_memory.pattern = generate_pattern(*cref)
if (fun1() and self.last_process_time + self.process_time <
time.time()):
if initial_cycle: # initial cycle
pattern = get_initial_pose(self.shared_memory.pattern)
idx = initial_cycle_idx
initial_cycle_idx += 1
if initial_cycle_idx > 1:
initial_cycle = False
if VIDEO and self.camerasock:
self.camerasock.make_video('bastelspass_mit_muc')
else: # normaler style
pattern = self.shared_memory.pattern
idx = self.ptrn_idx
self.ptrn_idx = idx+1 if idx < len(pattern)-1 else 0
# generate tasks
dvtsk, pvtsk, processtime = generate_pose_ref(pattern, idx)
# send to main thread
self.shared_memory.dvalve_task = dvtsk
self.shared_memory.ref_task = pvtsk
# organisation
self.process_time = processtime
self.last_process_time = time.time()
# capture image?
if self.camerasock and not VIDEO: # not video but image
if idx % 3 == 1:
self.camerasock.make_image('test'+str(self.camidx))
self.camidx += 1
time.sleep(UI_TSAMPLING)
set_leds()
return (self.ui_state)
""" ---------------- ----- ------- ----------------------------- """
""" ---------------- RUN STATE MACHINE ------------------------- """
""" ---------------- ----- ------- ----------------------------- """
automat = state_machine.StateMachine()
automat.add_state('PAUSE', pause_state)
automat.add_state('PWM_FEED_THROUGH', pwm_feed_through)
automat.add_state('USER_REFERENCE', user_reference)
automat.add_state('PATTERN_REFERENCE', pattern_reference)
automat.add_state('EXIT', exit_cleaner)
automat.add_state('QUIT', None, end_state=True)
automat.set_start('PAUSE')
try:
automat.run()
except Exception as err:
self.rootLogger.exception('\n exception HUI Thread \n')
self.rootLogger.exception(
"Unexpected error:\n", sys.exc_info()[0])
self.rootLogger.exception(sys.exc_info()[1])
self.rootLogger.error(err, exc_info=True)
self.rootLogger.info('Exit the HUI Thread ...')
change_state_in_main_thread('EXIT')
self.rootLogger.info('HUI Thread is done ...')
def run(self):
""" run HUI """
rootLogger.info('Running HUI Thread ...')
""" ---------------- ----- ------- ----------------------------- """
""" ---------------- HELP FUNCTIONS ------------------------- """
""" ---------------- ----- ------- ----------------------------- """
def mode_changed():
new_state = None
if GPIO.event_detected(MODE1):
new_state = 'MODE1'
elif GPIO.event_detected(MODE2):
new_state = 'MODE2'
elif GPIO.event_detected(MODE3):
new_state = 'MODE3'
elif self.state == 'EXIT':
new_state = 'EXIT'
change = False
if new_state and time.time() - self.lastchange > 1:
if (all_potis_zero() and new_state is not self.state
or new_state == 'EXIT'):
change = True
self.state = new_state
rootLogger.info("UI goes to: {}".format(new_state))
reset_events()
self.lastchange = time.time()
else:
flicker_leds()
return change
def fun1():
if GPIO.event_detected(FUN1):
if time.time() - self.lastfun1 > 1:
state = self.fun1
self.fun1 = not state
rootLogger.info('Fun1 turned {}'.format(not state))
self.lastfun1 = time.time()
return self.fun1
def fun2():
if GPIO.event_detected(FUN2):
if time.time() - self.lastfun2 > 1:
state = self.fun2
self.fun2 = not state
rootLogger.info('Fun2 turned {}'.format(not state))
self.lastfun2 = time.time()
return self.fun2
def reset_events():
for btn in BTNS:
GPIO.event_detected(btn)
self.fun1 = False
self.fun2 = False
def set_leds():
my_state = self.state
for pin, state in [(MODE1LED, 'MODE1'), (MODE2LED, 'MODE2'),
(MODE3LED, 'MODE3')]:
GPIO.output(pin, GPIO.HIGH if my_state == state else GPIO.LOW)
for pin, state in [(FUN1LED, self.fun1), (FUN2LED, self.fun2)]:
GPIO.output(pin, GPIO.HIGH if state else GPIO.LOW)
""" ---------------- ----- ------- ----------------------------- """
""" ---------------- STATE HANDLERS ------------------------- """
""" ---------------- ----- ------- ----------------------------- """
def exit_cleaner():
# Clean?
return ('QUIT')
def mode1():
ui_state.mode = self.state
while not mode_changed():
ui_state.fun = [fun1(), fun2()]
ui_state.switches = read_switches()
ui_state.potis = read_potis()
time.sleep(UI_TSAMPLING)
set_leds()
return self.state
""" ---------------- ----- ------- ----------------------------- """
""" ---------------- RUN STATE MACHINE ------------------------- """
""" ---------------- ----- ------- ----------------------------- """
automat = state_machine.StateMachine()
automat.add_state('MODE1', mode1)
automat.add_state('MODE2', mode1)
automat.add_state('MODE3', mode1)
automat.add_state('EXIT', exit_cleaner)
automat.add_state('QUIT', None, end_state=True)
automat.set_start('MODE1')
try:
automat.run()
except Exception as err:
rootLogger.exception('\n exception HUI Thread \n')
rootLogger.exception("Unexpected error:\n", sys.exc_info()[0])
rootLogger.exception(sys.exc_info()[1])
rootLogger.error(err, exc_info=True)
rootLogger.info('HUI Thread is done ...')
def main():
"""
main Function of server side:
- init software repr of the hardware
- init controllers
- init the Container which contains all shared variables, i.e. Cargo
- init the server-side StateMachine
- init the server-side Communication Thread
- start the Communication Thread
- Run the State Machine
- switch between following states according to user or system given
conditions:
- PAUSE (do nothing but read sensors)
- ERROR (Print Error Message)
- REFERENCE_TRACKING (start the controller.WalkingCommander)
- USER_CONTROL (Set PWM direct from User Interface)
- USER_REFERENCE (Use controller to track user-given reference)
- EXIT (Cleaning..)
- wait for communication thread to join
- fin
"""
print('Initialize Hardware ...')
sens, valve, dvalve = init_hardware()
controller = init_controller()
print('Initialize the shared variables, i.e. cargo ...')
start_state = 'PAUSE'
cargo = Cargo(start_state,
sens=sens,
valve=valve,
dvalve=dvalve,
controller=controller)
print('Setting up the StateMachine ...')
automat = state_machine.StateMachine()
automat.add_state('PAUSE', pause_state)
automat.add_state('ERROR', error_state)
automat.add_state('REFERENCE_TRACKING', reference_tracking)
automat.add_state('USER_CONTROL', user_control)
automat.add_state('USER_REFERENCE', user_reference)
automat.add_state('EXIT', exit_cleaner)
automat.add_state('QUIT', None, end_state=True)
automat.set_start(start_state)
print('Starting Communication Thread ...')
communication_thread = comm_t.CommunicationThread(cargo)
communication_thread.start()
try:
print('Run the StateMachine ...')
automat.run(cargo)
# pylint: disable = bare-except
except:
print('\n----------caught exception! in Main Thread----------------\n')
print("Unexpected error:\n", sys.exc_info()[0])
print(sys.exc_info()[1])
traceback.print_tb(sys.exc_info()[2])
communication_thread.join()
print('All is done ...')
sys.exit(0)
def main():
"""
- Run the State Machine
- switch between following states according to user or system given
conditions:
- PAUSE (do nothing but read sensors)
- FEED_THROUGH (Set PWM direct from User Interface)
- PRESSURE_REFERENCE (Use PSensCtr to track user-given reference)
- ANGLE_REFERENCE (Use IMUCtr to track user-given reference)
- EXIT (Cleaning..)
- wait for communication thread to join
"""
class SharedMemory(object):
""" Data, which is shared between Threads """
def __init__(self):
self.task_state_of_mainthread = START_STATE
self.actual_state_of_mainthread = START_STATE
self.sampling_time = TSAMPLING
self.dvalve_task = {name: 0. for name in DiscreteCHANNELset}
self.ref_task = {name: 0. for name in CHANNELset}
self.pwm_task = {name: 20. for name in CHANNELset}
self.rec = {name: 0.0 for name in CHANNELset}
self.rec_IMU = {name: None for name in IMUset}
self.rec_u = {name: 0.0 for name in CHANNELset}
self.rec_angle = {name: None for name in CHANNELset}
self.ptrndic = {
name: ptrn.read_list_from_csv('Patterns/' + name)
for name in ptrn.get_csv_files()
}
self.ptrndic['selected'] = sorted(list(self.ptrndic.keys()))[0]
self.pattern = self.ptrndic[self.ptrndic['selected']]
rootLogger.info('Initialize Hardware ...')
PSens, PValve, DValve, IMU, Ctr, ImuCtr = init_channels()
rootLogger.info('Initialize the shared variables, i.e. cargo ...')
shared_memory = SharedMemory()
""" ---------------- Sensor Evaluation ------------------------- """
def read_sens(shared_memory):
for name in PSens:
try:
shared_memory.rec[name] = PSens[name].get_value()
except IOError as e:
if e.errno == errno.EREMOTEIO:
rootLogger.info(
'cant read pressure sensor.' +
' Continue anyway ... Fail in [{}]'.format(name))
else:
rootLogger.exception('Sensor [{}]'.format(name))
rootLogger.error(e, exc_info=True)
raise e
return shared_memory
def read_imu(shared_memory):
for name in IMU:
try:
shared_memory.rec_IMU[name] = IMU[name].get_acceleration()
except IOError as e:
if e.errno == errno.EREMOTEIO:
rootLogger.exception(
'cant read imu device.' +
'Continue anyway ...Fail in [{}]'.format(name))
else:
rootLogger.exception('Sensor [{}]'.format(name))
rootLogger.error(e, exc_info=True)
raise e
return shared_memory
""" ---------------- HELP FUNCTIONS ------------------------- """
def set_dvalve():
for name in DValve:
state = shared_memory.dvalve_task[name]
DValve[name].set_state(state)
def init_output():
for name in PValve:
PValve[name].set_pwm(20.)
def pressure_check(pressure, pressuremax, cutoff):
if pressure <= cutoff:
out = 0
elif pressure >= pressuremax:
out = -MAX_CTROUT
else:
out = -MAX_CTROUT / (pressuremax - cutoff) * (pressure - cutoff)
return out
def cutoff(x, minx=-MAX_PRESSURE, maxx=MAX_PRESSURE):
if x < minx:
out = minx
elif x > maxx:
out = maxx
else:
out = x
return out
""" ---------------- ----- ------- ------------------------- """
""" ---------------- State Handler ------------------------- """
""" ---------------- ----- ------- ------------------------- """
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 feed_through(shared_memory):
"""
Set the valves to the data recieved by the comm_tread
"""
rootLogger.info("Arriving in FEED_THROUGH State: ")
shared_memory.actual_state_of_mainthread = 'FEED_THROUGH'
while shared_memory.task_state_of_mainthread == 'FEED_THROUGH':
# read
shared_memory = read_sens(shared_memory)
# write
for name in PValve:
pwm = shared_memory.pwm_task[name]
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 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 exit_cleaner(shared_memory):
""" Clean everything up """
rootLogger.info("cleaning ...")
shared_memory.actual_state_of_mainthread = 'EXIT'
shared_memory.task_state_of_mainthread = 'EXIT'
for name in PValve:
PValve[name].set_pwm(0.)
PValve[name].cleanup()
for name in DValve:
DValve[name].cleanup()
return ('QUIT', shared_memory)
def pause_state(shared_memory):
""" do nothing. waiting for tasks """
rootLogger.info("Arriving in PAUSE State: ")
shared_memory.actual_state_of_mainthread = 'PAUSE'
init_output()
while shared_memory.task_state_of_mainthread == 'PAUSE':
shared_memory = read_sens(shared_memory)
time.sleep(shared_memory.sampling_time)
new_state = shared_memory.task_state_of_mainthread
return (new_state, shared_memory)
""" ---------------- ----- ------- ----------------------------- """
""" ---------------- RUN STATE MACHINE ------------------------- """
""" ---------------- ----- ------- ----------------------------- """
rootLogger.info('Setting up the StateMachine ...')
automat = state_machine.StateMachine()
automat.add_state('PAUSE', pause_state)
automat.add_state('ANGLE_REFERENCE', angle_reference)
automat.add_state('FEED_THROUGH', feed_through)
automat.add_state('PRESSURE_REFERENCE', pressure_reference)
automat.add_state('EXIT', exit_cleaner)
automat.add_state('QUIT', None, end_state=True)
automat.set_start(START_STATE)
rootLogger.info('Starting Communication Thread ...')
communication_thread = HUI.HUIThread(shared_memory, rootLogger)
communication_thread.setDaemon(True)
communication_thread.start()
rootLogger.info('started UI Thread as daemon?: {}'.format(
communication_thread.isDaemon()))
camerasock, imgprocsock, plotsock = init_server_connections()
communication_thread.set_camera_socket(camerasock)
if PRINTSTATE:
printer_thread = HUI.Printer(shared_memory, imgprocsock, plotsock, IMU)
printer_thread.setDaemon(True)
printer_thread.start()
rootLogger.info('Started the Printer Thread')
try:
rootLogger.info('Run the StateMachine ...')
automat.run(shared_memory)
except KeyboardInterrupt:
rootLogger.exception('keyboard interrupt detected... killing UI')
except Exception as err:
rootLogger.exception(
'\n----------caught exception! in Main Thread----------------\n')
rootLogger.exception("Unexpected error:\n", sys.exc_info()[0])
rootLogger.exception(sys.exc_info()[1])
rootLogger.error(err, exc_info=True)
rootLogger.info('\n ----------------------- killing UI --')
finally:
if PRINTSTATE:
printer_thread.kill()
if imgprocsock:
imgprocsock.close()
if camerasock:
camerasock.close()
if plotsock:
plotsock.close()
communication_thread.kill()
communication_thread.join()
if PRINTSTATE:
printer_thread.join()
rootLogger.info('All is done ...')
sys.exit(0)
def main():
"""
main Function of server side:
- init software repr of the hardware
- init controllers
- init the Container which contains all shared variables, i.e. Cargo
- init the server-side StateMachine
- init the server-side Communication Thread
- start the Communication Thread
- Run the State Machine
- switch between following states according to user or system given
conditions:
- PAUSE (do nothing but read sensors)
- ERROR (Print Error Message)
- REFERENCE_TRACKING (start the controller.WalkingCommander)
- USER_CONTROL (Set PWM direct from User Interface)
- USER_REFERENCE (Use controller to track user-given reference)
- EXIT (Cleaning..)
- wait for communication thread to join
- fin
"""
rootLogger.info('Initialize Hardware ...')
sens, valve, dvalve, IMU = init_hardware()
controller, imu_ctr = init_controller()
rootLogger.info('Initialize the shared variables, i.e. cargo ...')
start_state = START_STATE
cargo = Cargo(start_state, sens=sens, valve=valve, dvalve=dvalve,
controller=controller, IMU=IMU, imu_ctr=imu_ctr)
rootLogger.info('Setting up the StateMachine ...')
automat = state_machine.StateMachine()
automat.add_state('PAUSE', pause_state)
automat.add_state('IMU_CONTROL', imu_control)
automat.add_state('ERROR', error_state)
# automat.add_state('REFERENCE_TRACKING', reference_tracking)
automat.add_state('USER_CONTROL', user_control)
automat.add_state('USER_REFERENCE', user_reference)
automat.add_state('EXIT', exit_cleaner)
automat.add_state('QUIT', None, end_state=True)
automat.set_start(start_state)
rootLogger.info('Starting Communication Thread ...')
communication_thread = HUI.HUIThread(cargo, rootLogger)
communication_thread.setDaemon(True)
communication_thread.start()
rootLogger.info('started UI Thread as daemon?: {}'.format(
communication_thread.isDaemon()))
try:
rootLogger.info('Run the StateMachine ...')
automat.run(cargo)
# pylint: disable = bare-except
except KeyboardInterrupt:
rootLogger.exception('keyboard interrupt detected... killing UI')
communication_thread.kill()
except Exception as err:
rootLogger.exception(
'\n----------caught exception! in Main Thread----------------\n')
rootLogger.exception("Unexpected error:\n", sys.exc_info()[0])
rootLogger.exception(sys.exc_info()[1])
rootLogger.error(err, exc_info=True)
rootLogger.info('\n ----------------------- killing UI --')
communication_thread.kill()
communication_thread.join()
rootLogger.info('All is done ...')
sys.exit(0)
def run(self):
""" run HUI """
self.rootLogger.info('Running HUI Thread ...')
""" ---------------- ----- ------- ----------------------------- """
""" ---------------- HELP FUNCTIONS ------------------------- """
""" ---------------- ----- ------- ----------------------------- """
def change_state_in_main_thread(state):
self.shared_memory.task_state_of_mainthread = state
# while not self.shared_memory.actual_state_of_mainthread == state:
# time.sleep(self.shared_memory.sampling_time)
def mode_changed():
new_state = None
if GPIO.event_detected(MODE1):
new_state = MODE[1]['ui_state']
elif GPIO.event_detected(MODE2):
new_state = MODE[2]['ui_state']
elif GPIO.event_detected(MODE3):
new_state = MODE[3]['ui_state']
elif self.ui_state == 'EXIT':
new_state = 'EXIT'
change = False
if new_state and time.time()-self.lastchange > 1:
if all_potis_zero() and new_state is not self.ui_state or new_state == 'EXIT':
change = True
self.ui_state = new_state
self.rootLogger.info("UI goes to: {}".format(new_state))
reset_events()
self.lastchange = time.time()
else:
flicker_leds()
return change
def fun1():
if GPIO.event_detected(FUN1):
if time.time()-self.lastfun1 > 1:
state = self.fun1
self.fun1 = not state
self.rootLogger.info('Fun1 turned {}'.format(not state))
self.lastfun1 = time.time()
return self.fun1
def fun2():
if GPIO.event_detected(FUN2):
if time.time()-self.lastfun2 > 1:
state = self.fun2
self.fun2 = not state
self.rootLogger.info('Fun2 turned {}'.format(not state))
self.lastfun2 = time.time()
return self.fun2
def is_userpattern():
if all_potis_zero():
if self.user_pattern:
self.user_pattern = False
self.shared_memory.pattern = \
self.shared_memory.ptrndic['default']
self.rootLogger.info('user_pattern was turned False')
else:
if not self.user_pattern:
self.user_pattern = True
self.rootLogger.info('user_pattern was turned True')
return self.user_pattern
def reset_events():
for btn in BTNS:
GPIO.event_detected(btn)
self.fun1 = False
self.fun2 = False
def set_leds():
my_state = self.ui_state
for pin, state in [(MODE1LED, MODE[1]['ui_state']),
(MODE2LED, MODE[2]['ui_state']),
(MODE3LED, MODE[3]['ui_state'])]:
GPIO.output(pin, GPIO.HIGH if my_state == state else GPIO.LOW)
for pin, state in [(FUN1LED, self.fun1),
(FUN2LED, self.fun2)]:
GPIO.output(pin, GPIO.HIGH if state else GPIO.LOW)
""" ---------------- ----- ------- ----------------------------- """
""" ---------------- STATE HANDLERS ------------------------- """
""" ---------------- ----- ------- ----------------------------- """
def exit_cleaner():
change_state_in_main_thread('EXIT')
return ('QUIT')
def pause_state():
while not mode_changed():
time.sleep(UI_TSAMPLING)
set_leds()
return (self.ui_state)
def pwm_feed_through():
change_state_in_main_thread(MODE[1]['main_state'])
while not mode_changed():
cref = read_potis()
for idx in self.shared_memory.pwm_task:
self.shared_memory.pwm_task[idx] = cref[idx]*100
dref = read_switches()
for idx in self.shared_memory.dvalve_task:
self.shared_memory.dvalve_task[idx] = dref[idx]
time.sleep(UI_TSAMPLING)
set_leds()
return (self.ui_state)
def user_reference():
while not mode_changed():
change_state_in_main_thread(MODE[2]['main_state'][fun2()])
refzero = fun1()
cref = (read_potis() if not refzero else
{idx: 0.0 for idx in self.shared_memory.ref_task})
for idx in self.shared_memory.ref_task:
self.shared_memory.ref_task[idx] = cref[idx]
dref = read_switches()
for idx in self.shared_memory.dvalve_task:
self.shared_memory.dvalve_task[idx] = dref[idx]
time.sleep(UI_TSAMPLING)
set_leds()
return (self.ui_state)
def pattern_reference():
while not mode_changed():
change_state_in_main_thread(MODE[3]['main_state'][fun2()])
if is_userpattern():
cref = read_potis().values()
self.shared_memory.pattern = generate_pattern(*cref)
if (fun1() and self.last_process_time + self.process_time <
time.time()):
pattern = self.shared_memory.pattern
idx = self.ptrn_idx
idx = idx+1 if idx < len(pattern)-1 else 0
dvtsk, pvtsk, processtime = generate_pose_ref(pattern, idx)
self.shared_memory.dvalve_task = dvtsk
self.shared_memory.ref_task = pvtsk
self.ptrn_idx = idx
self.process_time = processtime
self.last_process_time = time.time()
# capture image?
if self.camerasock:
if idx % 3 == 1:
self.camerasock.make_image('test'+str(self.camidx))
self.camidx += 1
time.sleep(UI_TSAMPLING)
set_leds()
return (self.ui_state)
""" ---------------- ----- ------- ----------------------------- """
""" ---------------- RUN STATE MACHINE ------------------------- """
""" ---------------- ----- ------- ----------------------------- """
automat = state_machine.StateMachine()
automat.add_state('PAUSE', pause_state)
automat.add_state('PWM_FEED_THROUGH', pwm_feed_through)
automat.add_state('USER_REFERENCE', user_reference)
automat.add_state('PATTERN_REFERENCE', pattern_reference)
automat.add_state('EXIT', exit_cleaner)
automat.add_state('QUIT', None, end_state=True)
automat.set_start('PAUSE')
try:
automat.run()
except Exception as err:
self.rootLogger.exception('\n exception HUI Thread \n')
self.rootLogger.exception(
"Unexpected error:\n", sys.exc_info()[0])
self.rootLogger.exception(sys.exc_info()[1])
self.rootLogger.error(err, exc_info=True)
self.rootLogger.info('Exit the HUI Thread ...')
change_state_in_main_thread('EXIT')
self.rootLogger.info('HUI Thread is done ...')
def main():
"""
Main function of client:
- init connection to the server(BBB)
- init the graphical user interface(GUI)
- start GUI
- initialize a StateMachine for Communication with server
- run the Communication StateMachine
- switch between:
- read_only (ask server for measurements and store them in GUI_Rec)
- read_write (store measurements in GUIRec and write References
from GUI_Task to the server)
- change_state (Write a new State Request to the server)
- terminate when GUI is closed
- wait for GUI to join
- close the socket - connection to server
"""
TSAMPLING_GUI = .1
START_STATE = 'PAUSE'
print('Initialize connection ...')
try:
sock = client.init_BBB_connection('beaglebone')
(sens_data, rec_u, rec_r, valve_data, dvalve_data, maxpressure,
maxctrout, tsampling, PID_gains, pattern) = \
client.get_meta_data(sock)
except exception.TimeoutError:
sock = None
(sens_data, rec_u, rec_r, valve_data, dvalve_data, maxpressure,
maxctrout, tsampling, PID_gains, pattern) = \
({'1': 0, '2': 0}, {'u1': 0, 'u2': 0},
{'r1': 0, 'r2': 0}, ['1', '2', '3', '4', '5', '6'],
['1', '2', '3', '4'], .1, 1., .1,
[[.1 for i in range(3)] for j in range(6)],
[[0.1]*6+[False]*4+[3.0]]*2)
print('Initialize GUI ...')
print('with', len(valve_data), 'prop valves')
print('with', len(dvalve_data), 'discrete valves')
print('with', len(sens_data), 'pressure sensors')
gui_rec = datamanagement.GUIRecorder()
gui_task = datamanagement.GUITask(START_STATE, valve_data, dvalve_data,
maxpressure, maxctrout, tsampling,
PID_gains, pattern)
gui_rec.append(sens_data)
gui_rec.append(rec_u)
gui_rec.append(rec_r)
gui = GuiThread(gui_rec, gui_task)
gui.start()
print('Initialize Communication State Machine ...')
automat = state_machine.StateMachine()
automat.add_state('READ_ONLY', read_only)
automat.add_state('READ_WRITE', read_write)
automat.add_state('CHANGE_STATE', change_state)
automat.add_state('EXIT', None, end_state=True)
automat.set_start('READ_ONLY')
cargo = Cargo(gui_rec, gui_task, sock, TSAMPLING_GUI)
try:
print('Run the StateMachine ...')
automat.run(cargo)
finally:
print('Closing Socket ...')
sock.close()
gui.join()
print('all is done')
def run(self):
PValve, DValve, IMU, ImuCtr = init_channels()
if IMU:
self.imu_in_use = True
else:
self.imu_in_use = False
def read_imu():
for name in IMU:
try:
llc_rec.aIMU[name] = IMU[name].get_acceleration()
except IOError as e:
if e.errno == errno.EREMOTEIO:
rootLogger.exception(
'cant read imu device.' +
'Continue anyway ...Fail in [{}]'.format(name))
else:
rootLogger.exception('Sensor [{}]'.format(name))
rootLogger.error(e, exc_info=True)
raise e
def set_dvalve():
for name in DValve:
state = llc_ref.dvalve[name]
DValve[name].set_state(state)
def init_output():
for name in PValve:
PValve[name].set_pwm(0.)
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
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 exit_cleaner():
""" Clean everything up """
rootLogger.info("cleaning Output Pins ...")
for name in PValve:
PValve[name].set_pwm(0.)
PValve[name].cleanup()
for name in DValve:
DValve[name].cleanup()
return ('QUIT')
def pause_state():
""" do nothing. waiting for tasks """
rootLogger.info("Arriving in PAUSE State: ")
init_output()
while llc_ref.state == 'PAUSE':
time.sleep(self.sampling_time)
return llc_ref.state
""" ---------------- ----- ------- ----------------------------- """
""" ---------------- RUN STATE MACHINE ------------------------- """
""" ---------------- ----- ------- ----------------------------- """
automat = state_machine.StateMachine()
automat.add_state('PAUSE', pause_state)
automat.add_state('ANGLE_REFERENCE', angle_reference)
automat.add_state('PRESSURE_REFERENCE', pressure_reference)
automat.add_state('EXIT', exit_cleaner)
automat.add_state('QUIT', None, end_state=True)
automat.set_start('PAUSE')
try:
rootLogger.info('Run LowLevelCtr ...')
automat.run()
except Exception as e:
rootLogger.exception(e)
rootLogger.error(e, exc_info=True)
raise
rootLogger.info('LowLevelCtr is done ...')
def run(self):
IMU = IMU_connection_test()
self.imu_in_use = True if IMU else False
def read_imu():
for name in IMU:
try:
llc_rec.acc[name] = IMU[name].get_acceleration()
llc_rec.gyr[name] = IMU[name].get_gyro()
except IOError as e:
if (e.errno == errno.EREMOTEIO
or e.errno == errno.EWOULDBLOCK):
rootLogger.exception(
'cant read imu device.' +
'Continue anyway ...Fail in [{}]'.format(name))
else:
rootLogger.exception('Sensor [{}]'.format(name))
rootLogger.error(e, exc_info=True)
raise e
def calc_angle(self): #M "self" hinzugefügt
if IMU:
for name in CHANNELset:
idx0, idx1 = CHANNELset[name]['IMUs']
rot_angle = CHANNELset[name]['IMUrot']
acc0 = np.array(self.LP[0].filt(
llc_rec.acc[idx0])) * self.accscale
acc1 = np.array(self.LP[1].filt(
llc_rec.acc[idx1])) * self.accscale
gyr0 = np.array(self.LP[2].filt(
llc_rec.gyr[idx0])) * self.gyrscale
gyr1 = np.array(self.LP[3].filt(
llc_rec.gyr[idx1])) * self.gyrscale
domega_z = self.Diff.diff(gyr1[2] - gyr0[2])
last_alp = llc_rec.aIMU[name]
adynx, adyny = compute_utils.a_dyn(domega_z, last_alp,
self.ell)
acc1_static = [acc1[0] + adynx, acc1[1] + adyny, acc1[2]]
aIMU_filt = compute_utils.calc_angle(
acc0, acc1_static, rot_angle)
llc_rec.aIMU[name] = round(aIMU_filt, 2)
read_imu() # init recorder
calc_angle(self)
def PPIDBooster():
rootLogger.info("Arriving in PPIDBooster State. ")
booster = ctrlib.PressureBoost(version='big', tboost=.75)
while llc_ref.state == 'PPIDBOOSTER':
if IMU and is_poti():
read_imu()
calc_angle(self)
#referenz über pattern
pattern_ref(patternname='pattern_0.csv', alpha=True)
for name in CHANNELset:
aref = llc_ref.alpha[name]
pref = booster.get_reference(aref)
pwm = calibration.cut_off(int(100 * pref), 100)
PWM.set_duty_cycle(OUT[name], pwm)
llc_rec.u[name] = pwm
time.sleep(self.sampling_time)
return llc_ref.state
def PPID():
rootLogger.info("Arriving in PPID State. ")
while llc_ref.state == 'PPID':
if IMU and is_poti():
read_imu()
calc_angle(self)
#referenz über pattern
pattern_ref(patternname='pattern_0.csv', alpha=True)
for name in CHANNELset:
aref = llc_ref.alpha[name]
pref = calibration.get_pressure(aref, version='big')
pwm = calibration.cut_off(int(100 * pref), 100)
PWM.set_duty_cycle(OUT[name], pwm)
llc_rec.u[name] = pwm
time.sleep(self.sampling_time)
return llc_ref.state
def CasPID():
rootLogger.info("Arriving in CasPID State. ")
PID = [0.008, 0.020, .01]
CasCtr = ctrlib.PidController_WindUp(PID, TSAMPLING, max_output=1.)
while llc_ref.state == 'CASPID':
if IMU and is_poti():
read_imu()
calc_angle(self)
#referenz über pattern
pattern_ref(patternname='pattern_0.csv', alpha=True)
for name in CHANNELset:
aref = llc_ref.alpha[name]
pref = CasCtr.output(aref, llc_rec.aIMU[name])
pwm = pwm = calibration.cut_off(pref * 100, 100)
PWM.set_duty_cycle(OUT[name], pwm)
llc_rec.u[name] = pwm
time.sleep(self.sampling_time)
return llc_ref.state
def CasPIDClb():
rootLogger.info("Arriving in CasPIDClb State. ")
PID = [0.0204, 0.13, 0.0037]
CasCtr = ctrlib.PidController_WindUp(PID, TSAMPLING, max_output=.4)
while llc_ref.state == 'CASPIDCLB':
if IMU:
read_imu()
calc_angle(self)
#referenz über pattern
# pattern_ref(patternname='pattern_0.csv', alpha=True)
#referenz über Poti
set_alpha_ref_via_poti()
for name in CHANNELset:
aref = llc_ref.alpha[name]
clb = calibration.get_pressure(aref, version='big')
pid = CasCtr.output(aref, llc_rec.aIMU[name])
pref = clb + pid
pwm = pwm = calibration.cut_off(pref * 100, 100)
PWM.set_duty_cycle(OUT[name], pwm)
llc_rec.u[name] = pwm
time.sleep(self.sampling_time)
return llc_ref.state
def POTIREF():
rootLogger.info("Arriving in POTIREF State: ")
while llc_ref.state == 'POTIREF':
# read
if IMU:
read_imu()
calc_angle(self)
# referenz über Poti
set_pressure_ref_via_poti()
# write
for name in CHANNELset:
pref = llc_ref.pressure[name]
PWM.set_duty_cycle(OUT[name], pref * 100)
llc_rec.u[name] = pref * 100
time.sleep(self.sampling_time)
return llc_ref.state
def clb():
rootLogger.info("Arriving in CLB State: ")
while llc_ref.state == 'CLB':
# read
if IMU and is_poti():
read_imu()
calc_angle(self)
pattern_ref(patternname='clb.csv', alpha=False)
# write
for name in CHANNELset:
pref = llc_ref.pressure[name]
PWM.set_duty_cycle(OUT[name], pref * 100)
llc_rec.u[name] = pref * 100
time.sleep(self.sampling_time)
return llc_ref.state
def pause_state():
""" do nothing. waiting for tasks """
rootLogger.info("Arriving in PAUSE State: ")
while llc_ref.state == 'PAUSE':
if IMU:
read_imu()
calc_angle()
time.sleep(self.sampling_time)
return llc_ref.state
def clean():
rootLogger.info('Clean PWM ...')
PWM.cleanup()
""" ---------------- ----- ------- ----------------------------- """
""" ---------------- RUN STATE MACHINE ------------------------- """
""" ---------------- ----- ------- ----------------------------- """
automat = state_machine.StateMachine()
automat.add_state('PAUSE', pause_state)
automat.add_state('PPID', PPID)
automat.add_state('POTIREF', POTIREF)
automat.add_state('CASPID', CasPID)
automat.add_state('CASPIDCLB', CasPIDClb)
automat.add_state('PPIDBOOSTER', PPIDBooster)
automat.add_state('CLB', clb)
automat.add_state('EXIT', clean, end_state=True)
# automat.set_start('FEED_THROUGH')
automat.set_start(STARTSTATE)
try:
rootLogger.info('Run LowLevelCtr ...')
automat.run()
except Exception as e:
rootLogger.exception(e)
rootLogger.error(e, exc_info=True)
raise
rootLogger.info('LowLevelCtr is done ...')
