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 ...')