def update(self): if self._anim is not None: self._emotion = self._anim() if self._anim.has_ended(): self._anim = None phi = cmath.phase(self._emotion) r = abs(self._emotion) # Create a list of dummy values. # None indicates that the servo at that index will not be updated. servo_pos_us = [None for i in range(16)] # Buffer to store all DOF values #self.dof_values = {} # (1) Calculate DOF positions using phi/r # (2) This step also applies overlay functions to the DOFs if self.isEmotion: for dofname, dof in self.dofs.iteritems(): self.dof_values[dofname] = dof.calc(phi, r) # (3) Update all servos for servo in self.servos: if servo.pin < 0 or servo.pin > 15: continue # Skip invalid pins if servo.dofname in self.dof_values: servo_pos_us[servo.pin] = servo.dof_to_us(self.dof_values[servo.dofname]) # TODO: send values to hardware with Hardware.lock: Hardware.servo_set_all(servo_pos_us)
def stop(opsoroapp): with Hardware.lock: Hardware.servo_disable() global circumplex_t if circumplex_t is not None: circumplex_t.stop()
def __init__(self, unit_id=0): # Handle camera configuration. self.unit_id = unit_id self.camera_port = BASE_PORT + unit_id self.video_url = 'http://localhost:{}/video_feed'.format(self.\ camera_port) self.image_url = 'http://localhost:{}/current_image'.format(self.\ camera_port) self.binary_image_url = 'http://localhost:{}/current_binary_image'.\ format(self.camera_port) # Launch camera image server. self.boot_image_server() # Register motor controls. # TODO self.x = 0 self.y = 0 self.z = 0 self.dx = 1 self.dy = 1 self.dz = 1 # Create the Hardware. self.hardware = Hardware() # Clean up on exit. atexit.register(self.close)
def __init__(self): logger.info("Starting amplifier control service") self.validVolCmd = ['Up', 'Down', 'Mute'] self.toggleinputs = [ 'Himbeer314', 'CD', 'Bladdnspiela', 'Portable', 'Hilfssherriff' ] self.oled = AmpiOled() self.hyp = Hypctrl(self.oled) #Starte handler für SIGTERM (kill -15), also normales beenden #Handler wird aufgerufen, wenn das Programm beendet wird, z.B. durch systemctl signal.signal(signal.SIGTERM, self.signal_term_handler) self.hw = Hardware(self.oled, self.hyp) time.sleep(1.1) #Short break to make sure the display is cleaned self.hw.setSource("Aus") #Set initial source to Aus self.mc_restart_cnt = 0 self.t_stop = threading.Event() self.clearTimer() self.udpServer() self.tcpServer() #tcpServer_t = Thread(target=tcpServer, args=(1, t_stop)) #tcpServer_t.start() logger.info("Amplifier control service running")
def startcap(electrodes): global running global numelectrodes Hardware.cap_init(electrodes=electrodes, gpios=0, autoconfig=True) numelectrodes = electrodes running = True
def stop(opsoroapp): with Hardware.lock: Hardware.servo_disable() # Remove all overlay functions for dofname, dof in Expression.dofs.iteritems(): if overlay_fn in dof.overlays: dof.overlays.remove(overlay_fn)
def stop(opsoroapp): with Hardware.lock: Hardware.servo_disable() global socialscriptloop_t if socialscriptloop_t is not None: socialscriptloop_t.stop() if socialscript_t is not None: socialscript_t.stop()
class App: def __init__(self): self.now = 0 self.demo_now = 0 self.event_queue = [] self.hw = Hardware() self.hw.enter_button.when_pressed = self.enter_button_pressed self.hw.up_button.when_pressed = self.up_button_pressed self.hw.down_button.when_pressed = self.down_button_pressed self.hw.exit_button.when_pressed = self.exit_button_pressed self.dmd = self.hw.dmd self.modes = { 'clock': clock.Mode(self.dmd), 'menu': menu.Mode(self.dmd), 'clock-select': menu.ClockSelectMode(self.dmd), 'status': status.Mode(self.dmd), } self.mode = 'clock' self.dmd.brightness = 0.5 self.dmd.rotation = 180 self.modes[self.mode].start() self.dmd.show() def enter_button_pressed(self): self.event_queue.insert(0, 'enter') def up_button_pressed(self): self.event_queue.insert(0, 'up') def down_button_pressed(self): self.event_queue.insert(0, 'down') def exit_button_pressed(self): self.event_queue.insert(0, 'exit') def loop(self): next_mode = self.modes[self.mode].service(self.event_queue) if next_mode is not None: next_mode_name = next_mode['mode'] self.modes[self.mode].stop() kwargs = dict(next_mode) del kwargs['mode'] self.modes[next_mode_name].start(**kwargs) self.mode = next_mode_name self.dmd.show() self.hw.service() def run(self): interval = 1 / 60 while True: self.now = time.time() self.loop() remaining = interval - (time.time() - self.now) if remaining > 0: time.sleep(remaining)
def action_when_go_to_REAL_TIME_GRAPH(self, frame: GraphFrame): import platform if platform.system() != 'Windows': from hardware import Hardware Hardware.getInstance().activateSelectorVmax() Hardware.getInstance().deactivateSelectorMachine() self.animationForGraphFrameFunction = frame.startAnimation() frame.setDuration(ChromaAnalyse.getInstance().getDuration()) RootView.getInstance().unbind( "<<" + self.controller.convertBrocheToBrocheName(Broche.BUTTON_OK) + ">>") return frame
class StackData: def __init__(self): self.hwData = Hardware() self.flData = Flavor() self.imgData = Image() def config(self, configuration, fileName): if (configuration == "hardware"): self.hwData.config(fileName) elif (configuration == "flavor"): self.flData.config(fileName) elif (configuration == "image"): self.imgData.config(fileName) def show(self, configuration): if (configuration == "hardware"): self.hwData.show() elif (configuration == "flavor"): self.flData.show() elif (configuration == "images"): self.imgData.show() def showAll(self): if self.hwData.getHardwareDataSize() != 0: self.hwData.show() else: print('\n No hardware data found\n') if self.flData.getFlavorsDataSize() != 0: self.flData.show() else: print('\n No flavors data found\n') if self.imgData.getImageDataSize() != 0: self.imgData.show() else: print('\n No images data found\n') logging.info('SUCCESS') def adminShowAvailableHardware(self): self.hwData.showAvailableHardware() def canHost(self, hardwareName, flavorName): hardware = self.hwData.getHardwareInfo(hardwareName) flavor = self.flData.getFlavorInfo(flavorName) if hardware == None or flavor == None: print("\n Please verify hardware name and flavor name") logging.error('FAILURE') return False if hardware.mem >= flavor.mem and hardware.numDisks >= flavor.numDisks and hardware.numVCpus >= flavor.numVCpus: return True else: return False
def main(): hardware = Hardware() update_interval = 0.1 thread = threading.Thread(target=update, args=(hardware, update_interval), daemon=True) thread.start() user_input_timeout = 1 status_timeout = 10 max_ping = 200 quick_checks = networkstatus.MultipleChecks([ networkstatus.PingDefaultGatewayCheck(max_ping=max_ping, timeout=status_timeout), networkstatus.PingCheck('google.com', max_ping=max_ping, timeout=status_timeout), networkstatus.DnsCheck('google.com', timeout=status_timeout), networkstatus.PingPrinterCheck(max_ping=max_ping, timeout=status_timeout) ]) extended_checks = networkstatus.MultipleChecks([ networkstatus.SpeedCheck( min_down=20*1000*1000, # 20 mpbs (limited by Raspberry Pi 3b wifi capabilities) min_up=5*1000*1000, # 5 mpbs max_ping=max_ping, timeout=status_timeout) ]) checks = networkstatus.NormalAndExtendedChecks(quick_checks, extended_checks, [hardware]) # Start by running the extended test last_normal_test = 0 last_extended_test = 0 normal_test_interval = 60 extended_test_interval = 60 * 60 while True: current_time = time.time() if (current_time >= last_normal_test + normal_test_interval or current_time >= last_extended_test + extended_test_interval): last_normal_test = current_time time_str = datetime.datetime.now(datetime.timezone.utc).strftime("%Y-%m-%dT%H:%M:%SZ") if current_time >= last_extended_test + extended_test_interval: print('# time(utc),{}'.format(checks.column_names())) last_extended_test = current_time result_str = checks.extended_check() else: result_str = checks.normal_check() print('{},{}'.format(time_str, result_str), flush=True) user_input = hardware.get_user_input(user_input_timeout) if user_input == hardware.UserInput.NORMAL_TEST: last_normal_test = 0 elif user_input == hardware.UserInput.EXTENDED_TEST: last_normal_test = 0 last_extended_test = 0
def __init__(self, feed): # load gui thisdir = os.path.dirname(__file__) gladefile = os.path.join(thisdir, "glade/main.glade") self.builder = gtk.Builder() self.builder.add_from_file(gladefile) self.datarate = DataRateAnalizer() # create and connect components self.hardware = Hardware(feed) self.pressure_plot = DataPlot() self.vario = Vario() #self.filter = AlphaBeta(1.2923, 0.86411); #self.filter = UnpredictingKalman(0.004, 0.5) self.hardware.listen("pressure", self.vario.on_pressure) self.hardware.listen("altitude", self.pressure_plot.on_raw_data) #self.distribution_plot = PressureDistributionPlot() #self.hardware.listen("pressure", self.distribution_plot.on_raw_data) #self.vario.listen("altitude", lambda k, v: self.filter.accept(v)) self.hardware.listen("filtered", self.pressure_plot.on_filtered_data) self.hardware.listen("temp", self.on_temperature) self.hardware.listen("pressure", self.on_raw_pressure) self.hardware.listen("altitude", self.on_altitude) self.hardware.listen("filtered", self.on_filtered) self.hardware.listen("velocity", self.on_vario)
def run(self, robot_config_values, room_size, rects, start_pos, start_angle, square_size, cartographer_grid): self.is_running = True self.commands_queue = Queue() # Clear map and set sizes for views self.map_window.start(room_size) self.map_scene.draw_map(room_size, rects, start_pos, start_angle) self.scanner_window.start(room_size) self.discovered_map_window.start(room_size, square_size) # Create some objects for the RobotController to use, as we have all the data here hardware = Hardware(start_pos, start_angle, room_size, rects, robot_config_values, self.map_change_pos, self.map_change_rotation) cartographer = Cartographer(room_size, square_size, self.cartographer_grid_values_callback, cartographer_grid) self.controller = RobotController(room_size, hardware, cartographer, self.commands_queue, self.scanner_draw_points, self.scanner_draw_lines, self.scanner_draw_robot, self.scanner_clear) self.controller.start()
def init(): assert ROOMID global room global raspberry global bookings room = Database(ROOMID) raspberry = Hardware(LEDS, PINS) bookings = room.Bookings
def create_app(): state = SharedState() hardware = Hardware(state) state.hardware_listener = hardware return (WebApp([(r'/()', IndexEndpoint), (r'/static/(.*)', StaticEndpoint), (r'/spots', SpotEndpoint), (r'/config', ConfigEndpoint, { 'state': state })]), hardware)
def action_when_quit_REAL_TIME_GRAPH(self): import platform if platform.system() != 'Windows': from hardware import Hardware Hardware.getInstance().deactivateSelectorVmax() #TODO: verify if usb key is inserted ChromaAnalyse.getInstance().saveDataToUsbKey() RootView.getInstance().getFrame().saveImageOfGraphWithName( ChromaAnalyse.getInstance().getKeyPath() + ChromaAnalyse.getInstance().getNameOfFile()) self.animationForGraphFrameFunction = None popup = Popup() #bind to quit popup RootView.getInstance().bind( "<<" + self.controller.convertBrocheToBrocheName(Broche.BUTTON_OK) + ">>", popup.quit) RootView.getInstance().bind( "<<" + self.controller.convertBrocheToBrocheName(Broche.BUTTON_STOP) + ">>", popup.quit) #display popup title = "Matériel d'enregistrement" message = "Les données ont bien été enregistrées sur la clé USB\n:)" popup.popupInformation(title=title, message=message) popup.destroy() #unbind stop an ok to quit RootView.getInstance().unbind( "<<" + self.controller.convertBrocheToBrocheName(Broche.BUTTON_OK) + ">>") RootView.getInstance().unbind( "<<" + self.controller.convertBrocheToBrocheName(Broche.BUTTON_STOP) + ">>") #bind ok,stop to next an previous page RootView.getInstance().bind( "<<" + self.controller.convertBrocheToBrocheName(Broche.BUTTON_OK) + ">>", self.controller.goToNextPage) RootView.getInstance().bind( "<<" + self.controller.convertBrocheToBrocheName(Broche.BUTTON_STOP) + ">>", self.controller.goToPreviousPage) ChromaAnalyse.getInstance().reset()
def start(): print('Starting...') global hardware, main hardware = Hardware(conf['setup']) main = Main(hardware) main.reset() main.load(conf['events']) main.run()
def __init__ (self): print "Now do bin" #### BINN ## subscribe vision srv_name = 'bin_srv' rospy.wait_for_service(srv_name) print 'service starts' self.detect_binn = rospy.ServiceProxy(srv_name, Bin_Srv) #### BINN self.aicontrol = AIControl() self.hw = Hardware()
def __init__(self): if MOCK_HARDWARE: from hardware_mock import HardwareMock self._hardware = HardwareMock() else: from hardware import Hardware self._hardware = Hardware() if MOCK_HEARTBEAT: self._heartbeat = HeartbeatMock() else: self._heartbeat = Heartbeat() if MOCK_DATA: self._sensor_service = SensorServiceMock() self._registration_service = RegistrationServiceMock() else: self._sensor_service = SensorService() self._registration_service = RegistrationService()
def __init__(self): self.now = 0 self.demo_now = 0 self.event_queue = [] self.hw = Hardware() self.hw.enter_button.when_pressed = self.enter_button_pressed self.hw.up_button.when_pressed = self.up_button_pressed self.hw.down_button.when_pressed = self.down_button_pressed self.hw.exit_button.when_pressed = self.exit_button_pressed self.dmd = self.hw.dmd self.modes = { 'clock': clock.Mode(self.dmd), 'menu': menu.Mode(self.dmd), 'clock-select': menu.ClockSelectMode(self.dmd), 'status': status.Mode(self.dmd), } self.mode = 'clock' self.dmd.brightness = 0.5 self.dmd.rotation = 180 self.modes[self.mode].start() self.dmd.show()
def __init__ (self): # rospy.init_node('sett_srv', anonymous=True) print "Now do Set Course" #### SETT ## subscribe vision sett_srv = 'setcourse_srv' rospy.wait_for_service(sett_srv) print 'service starts top srv' self.detect_sett = rospy.ServiceProxy(sett_srv, SetCourse_Srv) #### SETT self.aicontrol = AIControl() self.hw = Hardware()
def __init__(self, fullscreen=False): self.bg_color = (0, 0, 0) self.clock = pygame.time.Clock() if fullscreen: self.screen = pygame.display.set_mode((800, 600), pygame.FULLSCREEN) else: self.screen = pygame.display.set_mode((800, 600)) self.modes = [HeartMode(assets_path), TemperatureMode(assets_path), WeatherMode(assets_path), FortuneMode(assets_path), LinesMode(assets_path), ClockMode(assets_path)] self.current_mode = 0 self.running = True self.standby = False self.hw = Hardware() self.led_level = 0
def __init__(self): # Initialize an ADS1299 object self.SETTINGS = Settings() self.SETTINGS.update(ADS1299.DEFAULT_SETTINGS) try: # Lazy import local settings when __init__ is called. # If local settings exist, overwrite this object settings. from .settings import LOCAL_SETTINGS self.SETTINGS.update(LOCAL_SETTINGS) except ImportError: pass # Initialize SPI for ADS1299 self.spi = mraa.Spi(self.SETTINGS.SPI_CHANNEL) self.spi.frequency(self.SETTINGS.SPI_FREQUENCY) self.spi.mode(self.SETTINGS.SPI_MODE) # Initialize hardware control for ADS1299 self.hardware = Hardware() self.hardware.power = self.init_gpio(self.SETTINGS.PIN_POWER, mraa.DIR_OUT_LOW, mraa.MODE_STRONG) self.hardware.reset = self.init_gpio(self.SETTINGS.PIN_RESET, mraa.DIR_OUT_HIGH, mraa.MODE_STRONG) self.hardware.start = self.init_gpio(self.SETTINGS.PIN_START, mraa.DIR_OUT_LOW, mraa.MODE_STRONG) self.hardware.ready = self.init_gpio(self.SETTINGS.PIN_READY, mraa.DIR_IN, mraa.MODE_HIZ) self.hardware.chip_select = self.init_gpio( self.SETTINGS.PIN_CHIP_SELECT, mraa.DIR_OUT_HIGH, mraa.MODE_STRONG) self.hardware.chip_select_2 = self.init_gpio( self.SETTINGS.PIN_CHIP_SELECT_2, mraa.DIR_OUT_HIGH, mraa.MODE_STRONG) self.hardware.power.write(1) # power up # wait for internal reference waking up (150ms) # wait for internal clock waking up (20us) # wait for external cloxk waking up (1.5ms) sleep(160 * 10**(-3)) self.hardware.reset.write(0) # reset ADS1299 sleep(1 * 10**(-3)) # wait for reset register (18 CLK) self.hardware.reset.write(1) # finish reseting ADS1299 # Connect Register controller for ADS1299 self.register = Register(self.spi, self.hardware.chip_select)
def main(): hardware = Hardware() with open(TIMESTAMPS_FILE, 'a', buffering=1) as f: try: # Use external trigger only hardware.send_message(messages.TriggerConditionMessage(0b1000000)) while True: msg = hardware.read_message() if type(msg) == messages.MeasuredDataMessage: logging.info("%s %s ns", msg.timestamp, msg.count_ticks_PPS) f.write("%s %s ns\n" % (msg.timestamp, msg.count_ticks_PPS)) except KeyboardInterrupt: print "Interrupted by user." finally: hardware.close()
def TouchLoop(): global running global clientconn while not touch_t.stopped(): if running: data = {} with Hardware.lock: ret = Hardware.cap_get_filtered_data() for i in range(numelectrodes): data[i] = ret[i] if clientconn: clientconn.send_data("updateelectrodes", {"electrodedata": data}) touch_t.sleep(0.1)
def __init__(self, house_sections_seed_file, io_parts_seed_file): self.hardware = Hardware(io_parts_seed_file) house_sections_seeds = json.load( codecs.open(house_sections_seed_file, "r", "utf-8")) self.house_sections = {} for section_name, section in house_sections_seeds.items(): priority = section["priority"] message_name = section["message_name"] self.house_sections[section_name] = HouseSection( self.hardware, section_name, message_name, priority) for name, condition in section["conditions"].items(): message_name = condition["message_name"] priority = condition["priority"] io_part_statuses = condition["io_part_statuses"] self.house_sections[section_name].add_condition( name, message_name, priority, io_part_statuses) self.update_section_current_conditions()
def start(opsoroapp): # Turn servo power off, init servos, update expression with Hardware.lock: Hardware.servo_disable() Hardware.servo_init() Hardware.servo_neutral() with Expression.lock: Expression.set_emotion(valence=0.0, arousal=0.0) Expression.update() # Start update thread global circumplex_t circumplex_t = StoppableThread(target=CircumplexLoop) circumplex_t.start()
def __init__ (self): print "Now do bouy" print 'eiei' #### BOUY ## subscribe vision bouy_srv = 'find_obj' ### P'Ink service rospy.wait_for_service(bouy_srv) print 'service starts bouy srv' self.detect_bouy = rospy.ServiceProxy(bouy_srv, vision_srv) #### BOUY #### PATH ## subscribe vision bot_srv = 'vision2' rospy.wait_for_service(bot_srv) print 'service starts bot srv' self.detect_path = rospy.ServiceProxy(bot_srv, vision_srv) #### PATH self.aicontrol = AIControl() self.hw = Hardware()
def start(opsoroapp): global dof_positions dof_positions = {} # Apply overlay function for servo in Expression.servos: if servo.pin < 0 or servo.pin > 15: continue # Skip invalid pins dof_positions[servo.dofname] = 0.0 if servo.dofname in Expression.dofs: Expression.dofs[servo.dofname].overlays.append(overlay_fn) # Turn servo power off, init servos, update expression with Hardware.lock: Hardware.servo_disable() Hardware.servo_init() Hardware.servo_neutral() with Expression.lock: Expression.update()
def start(opsoroapp): # Apply overlay function for servo in Expression.servos: if servo.pin < 0 or servo.pin > 15: continue # Skip invalid pins dof_positions[servo.dofname] = 0.0 # Turn servo power off, init servos, update expression with Hardware.lock: Hardware.servo_disable() Hardware.servo_init() Hardware.servo_neutral() with Expression.lock: Expression.set_emotion(valence=0.0, arousal=0.0) Expression.update() # Start update thread global socialscriptloop_t socialscriptloop_t = StoppableThread(target=SocialscriptLoop) socialscriptloop_t.start();
class MainLoop(object): def __init__(self, fullscreen=False): self.bg_color = (0, 0, 0) self.clock = pygame.time.Clock() if fullscreen: self.screen = pygame.display.set_mode((800, 600), pygame.FULLSCREEN) else: self.screen = pygame.display.set_mode((800, 600)) self.modes = [HeartMode(assets_path), TemperatureMode(assets_path), WeatherMode(assets_path), FortuneMode(assets_path), LinesMode(assets_path), ClockMode(assets_path)] self.current_mode = 0 self.running = True self.standby = False self.hw = Hardware() self.led_level = 0 def lirc2pygame(self, keys): k = keys[0] if k == "KEY_POWER": return pygame.event.Event(pygame.KEYDOWN, key=pygame.K_s) if k == "KEY_RECORD": return pygame.event.Event(pygame.KEYDOWN, key=pygame.K_l) # These keys are already handled by pygame! # if k == "KEY_LEFT": # return pygame.event.Event(pygame.KEYDOWN, key=pygame.K_LEFT) # if k == "KEY_RIGHT": # return pygame.event.Event(pygame.KEYDOWN, key=pygame.K_RIGHT) # if k == "KEY_UP": # return pygame.event.Event(pygame.KEYDOWN, key=pygame.K_UP) # if k == "KEY_DOWN": # return pygame.event.Event(pygame.KEYDOWN, key=pygame.K_DOWN) return None def process_event(self, event): if event.type == pygame.QUIT: self.running = False elif event.type == pygame.KEYDOWN: if event.key == pygame.K_ESCAPE: self.running = False if event.key == pygame.K_s: self.standby = not self.standby self.hw.enable_screen(not self.standby) if event.key == pygame.K_l: if self.led_level > 0: self.led_level = 0 self.hw.enable_led(False) else: self.led_level = 10 self.hw.enable_led(True) self.hw.dim_led(self.led_level * 10) elif event.key == pygame.K_RIGHT: if not self.standby: self.current_mode = (self.current_mode + 1) % len(self.modes) elif event.key == pygame.K_LEFT: if not self.standby: self.current_mode = (self.current_mode - 1) % len(self.modes) elif event.key == pygame.K_UP: self.hw.enable_led(True) self.led_level = min(10, self.led_level + 1) self.hw.dim_led(self.led_level * 10) elif event.key == pygame.K_DOWN: self.led_level = max(0, self.led_level - 1) if self.led_level == 0: self.hw.enable_led(False) else: self.hw.dim_led(self.led_level * 10) elif event.key == pygame.K_0: self.led_level = 0 self.hw.enable_led(False) elif event.key == pygame.K_1: self.hw.enable_led(True) self.led_level = 1 self.hw.dim_led(self.led_level * 10) elif event.key == pygame.K_2: self.hw.enable_led(True) self.led_level = 2 self.hw.dim_led(self.led_level * 10) elif event.key == pygame.K_3: self.hw.enable_led(True) self.led_level = 3 self.hw.dim_led(self.led_level * 10) elif event.key == pygame.K_4: self.hw.enable_led(True) self.led_level = 4 self.hw.dim_led(self.led_level * 10) elif event.key == pygame.K_5: self.hw.enable_led(True) self.led_level = 5 self.hw.dim_led(self.led_level * 10) elif event.key == pygame.K_6: self.hw.enable_led(True) self.led_level = 6 self.hw.dim_led(self.led_level * 10) elif event.key == pygame.K_7: self.hw.enable_led(True) self.led_level = 7 self.hw.dim_led(self.led_level * 10) elif event.key == pygame.K_8: self.hw.enable_led(True) self.led_level = 8 self.hw.dim_led(self.led_level * 10) elif event.key == pygame.K_9: self.hw.enable_led(True) self.led_level = 9 self.hw.dim_led(self.led_level * 10) def main_loop(self, lirc_socket=None): # Hide mouse cursor pygame.mouse.set_visible(False) clear = self.screen.copy() clear.fill((0, 0, 0)) self.hw.enable_screen(True) while self.running: mode = self.modes[self.current_mode] for event in pygame.event.get(): self.process_event(event) mode.process_event(event) if lirc_socket: keys = lirc.nextcode() if keys: event = self.lirc2pygame(keys) if event: self.process_event(event) mode.process_event(event) if self.standby: self.clock.tick(5) else: self.screen.blit(clear, (0, 0)) mode.loop(self.screen) pygame.display.flip() self.clock.tick(mode.preferred_fps()) pygame.display.set_caption("fps: %.2f" % self.clock.get_fps())
class BouyMission (object): def __init__ (self): print "Now do bouy" print 'eiei' #### BOUY ## subscribe vision bouy_srv = 'find_obj' ### P'Ink service rospy.wait_for_service(bouy_srv) print 'service starts bouy srv' self.detect_bouy = rospy.ServiceProxy(bouy_srv, vision_srv) #### BOUY #### PATH ## subscribe vision bot_srv = 'vision2' rospy.wait_for_service(bot_srv) print 'service starts bot srv' self.detect_path = rospy.ServiceProxy(bot_srv, vision_srv) #### PATH self.aicontrol = AIControl() self.hw = Hardware() def run (self, q): self.aicontrol.drive_z (const.BOUY_DETECTING_DEPTH) if q == 'A': mul = 1 else: mul = -1 red_bouy = self.detect_bouy(String('bouy'), String('red')) red_bouy = red_bouy.data if not red_bouy.appear: green_bouy = self.detect_bouy(String('bouy'), String('green')) green_bouy = green_bouy.data if green_bouy.appear: move = 0 while red_bouy.appear == False and move != 5: self.aicontrol.drive_y (1*mul) red_bouy = self.detect_bouy(String('bouy'),String('red')) red_bouy = red_bouy.data rospy.sleep(2) move += 1 if red_bouy.appear: print 'found red bouy' else: self.aicontrol.drive_y (-5*mul) ############## self.aicontrol.stop (2) bouy_color = ['red', 'green', 'yellow'] for i in xrange(1): print 'will hit ' + bouy_color[i] count = 20 self.aicontrol.drive_z (const.BOUY_DETECTING_DEPTH) ############# CHANGE ME !!!! print 'drive z const complete' while not rospy.is_shutdown() and not self.aicontrol.is_fail(count) : now_pose = self.aicontrol.get_pose() bouy_data = self.detect_bouy(String('bouy'),String(bouy_color[i])) bouy_data = bouy_data.data print bouy_data if bouy_data.appear: vx = (1/bouy_data.area)*500 vy = bouy_data.x vz = bouy_data.y if bouy_data.area > 900 : ### near ### print 'near' bc = 0.05 sr = 0.2 else : ### far ### print 'far' bc = 0.1 sr = 0.4 if self.aicontrol.is_center([bouy_data.x,bouy_data.y],-bc,bc,-bc,bc) : print bouy_data if bouy_data.area > 1000: ### CHANGE ME !!! print 'go to bouy' print 'drive_x 2 meter' self.aicontrol.drive_x (2) break else: print 'drive_x 0.5 meter so far' self.aicontrol.drive_x (1) else : self.aicontrol.drive([0,vy,vz,0,0,0]) print 'set to center' rospy.sleep (sr) else : self.aicontrol.drive_x (0.1) count -= 1 ### end while ### self.aicontrol.stop (1) print 'stop state after hit bouy' if count != 0: self.aicontrol.drive_x (-1) print 'backward' self.aicontrol.drive_y (mul) print 'slide to hit green bouy' self.aicontrol.turn_yaw_relative (90) self.aicontrol.drive_y (-3) self.aicontrol.drive_z (const.PATH_DETECTING_DEPTH) self.aicontrol.turn_yaw_relative (-90) else: self.aicontrol.drive_z (const.PATH_DETECTING_DEPTH) # print 'backward' # print 'go to set point' # self.aicontrol.drive_x (-3) # rospy.sleep(1) # print 'set point' # print 'finish ' + bouy_color[i] # if i == 0: # move = 0 # green_bouy = self.detect_bouy(String('bouy'),String('green')) # green_bouy = green_bouy.data # while green_bouy.appear == False and move != 5: # self.aicontrol.drive_y (1*-mul) # green_bouy = self.detect_bouy(String('bouy'),String('green')) # green_bouy = green_bouy.data # rospy.sleep(2) # move += 1 # if green_bouy.appear: # print 'found green bouy' # else: # self.aicontrol.drive_y (-5*-mul) # ##### # else: # move = 0 # yellow_bouy = self.detect_bouy(String('bouy'),String('yellow')) # yellow_bouy = yellow_bouy.data # while yellow_bouy.appear == False and move != 5: # self.aicontrol.drive_y (1*-mul) # yellow_bouy = self.detect_bouy(String('bouy'),String('yellow')) # yellow_bouy = yellow_bouy.data # rospy.sleep(2) # move += 1 # if green_bouy.appear: # print 'found yellow bouy' # else: # self.aicontrol.drive_y (-5*-mul) ##### ### end for ### print 'finish 2 bouy' self.aicontrol.drive_x(-1) self.aicontrol.drive_z(const.PATH_DETECTING_DEPTH) self.aicontrol.drive_x (3) if self.aicontrol.is_fail(count): return False else: return True # self.yellow_bouy() def red_bouy (self): def yellow_bouy (self): self.aicontrol.drive_z (const.BOUY_DETECTING_DEPTH) print 'do yellow bouy' count = 20 self.hw.command ('gripper', 'close') print 'open gripper' while not rospy.is_shutdown() and not self.aicontrol.is_fail(count) : bouy_data = self.detect_bouy(String('bouy'),String('yellow')) bouy_data = bouy_data.data print bouy_data if bouy_data.appear: vx = (1/bouy_data.area)*500 vy = bouy_data.x vz = bouy_data.y if bouy_data.area > 400: ### near ### print 'near' bc = 0.05 sr = 0.3 else : ### far ### print 'far' bc = 0.1 sr = 0.5 if self.aicontrol.is_center([bouy_data.x,bouy_data.y],-bc,bc,-bc,bc) : print bouy_data if bouy_data.area > 600: ### change area value print 'go to bouy' print 'drive_x 2 meter' self.aicontrol.drive_x (2) rospy.sleep (1) self.hw.command ('gripper', 'grab') rospy.sleep (1) print 'grab la na eiei' self.aicontrol.drive_z (const.BOUY_YELLOW_PULL_DEPTH) ####### CHANGE ME !!! print 'drive_z complete' print 'pull down' self.hw.command ('gripper', 'close') self.aicontrol.stop (2) print 'release' self.aicontrol.drive_x (-2) self.aicontrol.drive_z (const.PATH_DETECTING_DEPTH) self.aicontrol.drive_y (-0.8) break else: print 'drive_x 1 meter so far' self.aicontrol.drive_x (1) rospy.sleep(0.5) else : self.aicontrol.drive([0,vy,vz,0,0,0]) print 'set to center' rospy.sleep (sr) else : self.aicontrol.stop(0.2) count -= 1 ### end while ### self.aicontrol.stop (3) print 'stop state after hit bouy' self.find_path () def find_path (self): self.aicontrol.drive_z (const.PATH_DETECTING_DEPTH) path_data = self.detect_path(String('path1'),String('orange')) path_data = path_data.data print path_data count = 30 found = False while not rospy.is_shutdown() and not self.aicontrol.is_fail(count) : if path_data.appear: print 'found path' self.aicontrol.stop(2) break else: yy = 0.1 self.aicontrol.drive_x (1) for i in xrange(8): self.aicontrol.drive_y (yy) path_data = self.detect_path(String('path1'),String('orange')) path_data = path_data.data print path_data if path_data.appear: found = True break if not found: self.aicontrol.drive_y(-0.5) yy = -0.1 for i in xrange(8): self.aicontrol.drive_y (yy) path_data = self.detect_path(String('path1'),String('orange')) path_data = path_data.data print path_data if path_data.appear: found = True break count -= 1 return found ''' def slide_to_do_bouy (self): while not rospy.is_shutdown() and not self.aicontrol.is_fail(count) : now_pose = self.aicontrol.get_pose() bouy_data = self.detect_bouy(String('bouy'),String('red') # bouy_data = bouy_data.data # print bouy_data if bouy_data.appear: vx = (1/bouy_data.area)*500 vy = bouy_data.x vz = bouy_data.y if bouy_data.area > 600 : ### near ### print 'near' bc = 0.05 sr = 0.2 else : ### far ### print 'far' bc = 0.1 sr = 0.4 if self.aicontrol.is_center([bouy_data.x,bouy_data.y],-bc,bc,-bc,bc) : print bouy_data if bouy_data.area > 1200: ### CHANGE ME !!! print 'go to bouy' print 'drive_x 3 meter' self.aicontrol.drive_x (3) rospy.sleep(2) break else: print 'drive_x 0.5 meter so far' self.aicontrol.drive_x (0.5) rospy.sleep(2) else : self.aicontrol.drive([0,vy,vz,0,0,0]) print 'set to center' rospy.sleep (sr) else : self.aicontrol.stop(0.2) self.aicontrol.drive_x (0.1) count -= 1 ### end while ### print 'SLIDE TO HIT' self.aicontrol.drive_y (5) self.aicontrol.drive_z (-0.05) return ''' if __name__ == '__main__': print 'do bouy' rospy.init_node('bouy_ai', anonymous=True) self.run() # self.yellow_bouy() print 'RED AND GREEN BOUY COMPLETE !!'
class Ampi(): def __init__(self): logger.info("Starting amplifier control service") self.validVolCmd = ['Up', 'Down', 'Mute'] self.toggleinputs = [ 'Himbeer314', 'CD', 'Bladdnspiela', 'Portable', 'Hilfssherriff' ] self.oled = AmpiOled() self.hyp = Hypctrl(self.oled) #Starte handler für SIGTERM (kill -15), also normales beenden #Handler wird aufgerufen, wenn das Programm beendet wird, z.B. durch systemctl signal.signal(signal.SIGTERM, self.signal_term_handler) self.hw = Hardware(self.oled, self.hyp) time.sleep(1.1) #Short break to make sure the display is cleaned self.hw.setSource("Aus") #Set initial source to Aus self.mc_restart_cnt = 0 self.t_stop = threading.Event() self.clearTimer() self.udpServer() self.tcpServer() #tcpServer_t = Thread(target=tcpServer, args=(1, t_stop)) #tcpServer_t.start() logger.info("Amplifier control service running") def __del__(self): pass def signal_term_handler(self, signal, frame): logger.info("Got " + str(signal)) logger.info("Closing UDP Socket") self.udpSock.close() #UDP-Server abschiessen self.hw.setSource("Aus") #Preamp schlafen legen self.hw.stop() logger.info("So long sucker!") #Fein auf Wiedersehen sagen sys.exit(0) #Und raus hier def clearTimer(self): ciT = threading.Thread(target=self._clearTimer) ciT.setDaemon(True) ciT.start() def _clearTimer(self): while (not self.t_stop.is_set()): self.mc_restart_cnt = 0 # Clear mediacenter reboot counter self.t_stop.wait(3) def tcpServer(self): sT = threading.Thread(target=self._tcpServer) sT.setDaemon(True) sT.start() def _tcpServer(self): server = socketserver.TCPServer((eth_addr, tcp_port), MyTCPSocketHandler) server.serve_forever() def udpServer(self): logger.info("Starting UDP-Server at {}:{}".format(eth_addr, udp_port)) self.udpSock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.udpSock.bind((eth_addr, udp_port)) self.t_stop = threading.Event() udpT = threading.Thread(target=self._udpServer) udpT.setDaemon(True) udpT.start() def _udpServer(self): while (not self.t_stop.is_set()): try: data, addr = self.udpSock.recvfrom( 1024) # Puffer-Groesse ist 1024 Bytes. ret = self.parseCmd( data ) # Abfrage der Fernbedienung (UDP-Server), der Rest passiert per Interrupt/Event logger.info(ret) self.udpSock.sendto(str(ret).encode('utf-8'), addr) except Exception as e: logger.warning( "Uiui, beim UDP senden/empfangen hat's kracht: {}".format( str(e))) def stopKodiPlayer(self): try: kodi = Kodi("http://localhost/jsonrpc") playerid = kodi.Player.GetActivePlayers()["result"][0]["playerid"] result = kodi.Player.Stop({"playerid": playerid}) logger.info("Kodi aus!") except Exception as e: logger.warning("Beim Kodi stoppen is wos passiert: {}".format( str(e))) def selectSource(self, jcmd): logger.info("Input: {}".format(jcmd['Parameter'])) logger.info("Source set remotely to {}".format(jcmd['Parameter'])) self.hw.setSource(jcmd['Parameter']) ret = self.hw.getSource() if (ret == -1): ret = {"Antwort": "bassd net", "Input": ret} else: ret = {"Antwort": "bassd", "Input": ret} return (json.dumps(ret)) def selectOutput(self, jcmd): logger.info("Output: {}".format(jcmd['Parameter'])) ret = self.hw.selectOutput(jcmd['Parameter']) return ret def ampiZustand(self): zustand = json.dumps({ "Antwort": "Zustand", "Input": self.hw.getSource(), "Hyperion": self.hyp.getScene(), "Volume": self.hw.volume.getVolume(), "OledBlank": self.hw.oled.getBlankScreen(), "TV": self.hw.getTvPwr(), "PA2200": self.hw.getAmpPwr(), "Amp-Ausgang": self.hw.getAmpOut(), "Headphone-Ausgang": self.hw.getHeadOut() }) return (zustand) def parseCmd(self, data): data = data.decode() try: jcmd = json.loads(data) except: logger.warning("Das ist mal kein JSON, pff!") ret = json.dumps({"Antwort": "Kaa JSON Dings!"}) if (jcmd['Aktion'] == "Input"): ret = self.selectSource(jcmd) elif (jcmd['Aktion'] == "Output"): ret = self.selectOutput(jcmd) ret = json.dumps({"Antwort": jcmd, "Wert": ret}) elif (jcmd['Aktion'] == "Hyperion"): logger.info("Remote hyperion control") ret = json.dumps({ "Antwort": "Hyperion", "Szene": self.hyp.setScene() }) elif (jcmd['Aktion'] == "Volume"): if jcmd['Parameter'] in self.validVolCmd: ret = self.setVolume(jcmd['Parameter']) else: ret = json.dumps({"Antwort": "Kein echtes Volumen-Kommando"}) elif (jcmd['Aktion'] == "Switch"): if jcmd['Parameter'] == "DimOled": ret = self.hw.oled.toggleBlankScreen() logger.info("Dim remote command toggled") if (ret): ret = json.dumps({"Antwort": "Oled", "Wert": "Aus"}) else: ret = json.dumps({"Antwort": "Oled", "Wert": "An"}) elif jcmd['Parameter'] == "Power": self.stopKodiPlayer() time.sleep(0.2) self.hw.setSource("Aus") #self.hyp.setScene("Kodi") logger.info("Aus is fuer heit!") ret = json.dumps({"Antwort": "Betrieb", "Wert": "Aus"}) elif jcmd['Parameter'] == "Mediacenter": self.mc_restart_cnt += 1 ret = json.dumps({ "Antwort": "Mediacenter", "Wert": "BaldRestart" }) if self.mc_restart_cnt >= 2: os.system('sudo systemctl restart mediacenter') logger.info("Mediaceenter wird neu gestart") ret = json.dumps({ "Antwort": "Mediacenter", "Wert": "Restart" }) elif jcmd['Parameter'] == "Input": src = self.hw.getSource() try: idx = self.toggleinputs.index(src) idx += 1 if (idx >= len(self.toggleinputs)): idx = 0 except ValueError: idx = 0 self.hw.setSource(self.toggleinputs[idx]) ret = json.dumps({ "Antwort": "Input", "Wert": self.toggleinputs[idx] }) else: logger.warning("Des bassd net.") ret = json.dumps({ "Antwort": "Schalter", "Wert": "Kein gültiges Schalter-Kommando" }) elif (jcmd['Aktion'] == "Zustand"): logger.info("Wos für a Zustand?") ret = self.ampiZustand() #TODO: Alle Zustände lesen und ausgeben else: logger.warning("Invalid remote command: {}".format(data)) ret = json.dumps({ "Antwort": "Fehler", "Wert": "Kein gültiges Kommando" }) return (ret) def setVolume(self, val): if (val == "Up"): ret = self.hw.volume.incVolumePot() elif (val == "Down"): ret = self.hw.volume.decVolumePot() else: ret = self.hw.volume.toggleMute() if (ret == -1): ret = {"Antwort": "bassd net", "Input": ret} else: ret = {"Antwort": "bassd", "Input": ret} return (json.dumps(ret))
def servo_set_all(self, pos_list): pos_list = list(pos_list.values()) Hardware.servo_set_all(post_list)
try: if '-p' in argv: port = int(argv[argv.index('-p') + 1]) elif '--port' in argv: port = int(argv[argv.index('--port') + 1]) else: port = 8080 httpd = HTTPServer(('localhost', port), MyHandler) print("Starting web server on port: %d" % httpd.server_port) print("Open your web browser on http://localhost:%d" %httpd.server_port) print("Press CONTROL-C for stopping it!") netmapper = NetMapper() netmapper.start() hardware = Hardware() hardware.start() httpd.serve_forever() except KeyboardInterrupt: print(" received, shutting down the web server\n") httpd.socket.close() exit() except ValueError: print("Error: bad launch parameters") exit(1) except Exception as e: print(e)
class SettMission (object): def __init__ (self): # rospy.init_node('sett_srv', anonymous=True) print "Now do Set Course" #### SETT ## subscribe vision sett_srv = 'setcourse_srv' rospy.wait_for_service(sett_srv) print 'service starts top srv' self.detect_sett = rospy.ServiceProxy(sett_srv, SetCourse_Srv) #### SETT self.aicontrol = AIControl() self.hw = Hardware() def find_sett (self): count = 50 self.aicontrol.drive_z (const.SET_DETECTING_DEPTH) ##### CHANGE ME !! while not rospy.is_shutdown() and not self.aicontrol.is_fail(count): sett_data = self.detect_sett(String('setcourse'),String('big')) sett_data = sett_data.data print sett_data if len(sett_data.appear) != 0 and sett_data.appear[0]: print 'found' if sett_data.area[0] > 15000: ###### CHANGE ME !! print 'near' bc = 0.1 else: print 'far' bc = 0.3 if self.aicontrol.is_center([sett_data.x[0],sett_data.y[0]],-bc,bc,-bc,bc): if sett_data.area[0] > 16000: ###### CHANGE ME !! print 'should see 4 sq, find big complete !!' break else: print 'forward' self.aicontrol.drive_x (0.05) else: self.aicontrol.drive ([0,sett_data.x[0],sett_data.y[0],0,0,0]) rospy.sleep(0.5) print 'set to center' self.aicontrol.stop (1) else: self.aicontrol.stop (1) print 'not found' count -= 1 def find_sq (self, yd, zd, la): ### y_direction & z_direction & launcher count = 20 saw = False # self.aicontrol.drive_y (0.6*yd/0.05) while not rospy.is_shutdown() and not self.aicontrol.is_fail(count): sq_data = self.detect_sett(String('setcourse'),String('small')) sq_data = sq_data.data print sq_data if len(sq_data.appear) == 1: if self.aicontrol.is_center ([sq_data.x[0],sq_data.y[0]],-0.05,0.05,-0.05,0.05): if sq_data.area[0] > 6000: print 'forward to fire' self.hw.command (String(la), String('fire')) self.hw.command (String(la), String('close')) print 'fire complete' self.aicontrol.stop (10) self.aicontrol.drive_x (-2) break else: print 'forward' self.aicontrol.drive_x (0.02) else: print 'move' self.aicontrol.drive ([0,sq_data.x[0],sq_data.y[0],0,0,0]) rospy.sleep(0.5) elif len(sq_data.appear) == 2 or len(sq_data.appear) == 3: self.aicontrol.drive_x (0.2) else: state = self.aicontrol.get_pose() if -4 < state[2] < -2: ###### CHANGE ME !!! self.aicontrol.drive ([0,0,zd,0,0,0]) rospy.sleep(0.5) self.aicontrol.drive ([0.2,yd,0,0,0,0]) rospy.sleep (0.2) return def run (self): v = 0.05 eq = ['fire_left', 'fire_right'] self.find_sett () self.find_sq (-v, v, eq[0]) #### right top - + self.find_sett () self.find_sq (v, -v, eq[1])
class BouyMission (object): def __init__ (self): print "Now do bouy" print 'eiei' #### BOUY ## subscribe vision bouy_srv = 'find_obj' ### P'Ink service rospy.wait_for_service(bouy_srv) print 'service starts bouy srv' self.detect_bouy = rospy.ServiceProxy(bouy_srv, vision_srv) #### BOUY #### PATH path_srv = 'vision2' rospy.wait_for_service(path_srv) print 'service starts path srv' self.detect_path = rospy.ServiceProxy(path_srv, vision_srv) #### PATH self.aicontrol = AIControl() self.hw = Hardware() self.oldx = 0 self.oldy = 0 def check_point(self, newx, newy): if abs(self.oldx - newx) < 0.1 and abs(self.oldy - newy) < 0.1: return True else: return False def run (self): red_bouy = self.detect_bouy(String('bouy'),String('red')) red_bouy = red_bouy.data rospy.sleep(2) green_bouy = self.detect_bouy(String('bouy'),String('green')) green_bouy = green_bouy.data rospy.sleep(2) bouy_color = ['red', 'green', 'yellow'] if green_bouy.appear: move = 0 while red_bouy.appear == False and move != 10: self.aicontrol.drive_y (0.5) red_bouy = self.detect_bouy(String('bouy'),String('red')) red_bouy = red_bouy.data rospy.sleep(2) move += 1 if red_bouy.appear: print 'found red bouy' self.aicontrol.stop (2) for i in xrange(2): print 'will hit ' + bouy_color[i] count = 20 self.aicontrol.drive_z (const.BOUY_DETECTING_DEPTH) ############# CHANGE ME !!!! print 'drive z const complete' while not rospy.is_shutdown() and not self.aicontrol.is_fail(count) : now_pose = self.aicontrol.get_pose() bouy_data = self.detect_bouy(String('bouy'),String(bouy_color[i])) bouy_data = bouy_data.data print bouy_data if bouy_data.appear: vx = (1/bouy_data.area)*500 vy = bouy_data.x vz = bouy_data.y if bouy_data.area > 400 : ### near ### print 'near' bc = 0.05 sr = 0.2 else : ### far ### print 'far' bc = 0.1 sr = 0.4 if self.aicontrol.is_center([bouy_data.x,bouy_data.y],-bc,bc,-bc,bc) : print bouy_data if bouy_data.area > 600: ### CHANGE ME !!! print 'go to bouy' print 'drive_x 3 meter' self.aicontrol.drive_x (3) rospy.sleep(2) break else: print 'drive_x 0.2 meter so far' self.aicontrol.drive_x (0.2) rospy.sleep(2) else : self.aicontrol.drive([0,vy,vz,0,0,0]) print 'set to center' rospy.sleep (sr) else : self.aicontrol.stop(0.2) count -= 1 ### end while ### if self.aicontrol.is_fail(count): self.aicontrol.drive_x (2) move = 0 self.aicontrol.stop (1) print 'stop state after hit bouy' print 'backward' print 'go to set point' if i == 1: xx = -2 else: xx = -3 self.aicontrol.drive_x (xx) rospy.sleep(5) if i == 0: self.aicontrol.drive_z (const.BOUY_DETECTING_DEPTH) self.aicontrol.drive_y (-3) print 'slide right 3 meter' green_bouy = self.detect_bouy(String('bouy'),String('green')) green_bouy = green_bouy.data rospy.sleep(2) while green_bouy.appear == False and move != 20: self.aicontrol.drive_y (-0.1) rospy.sleep(0.5) green_bouy = self.detect_bouy(String('bouy'),String('green')) green_bouy = green_bouy.data rospy.sleep(1) move += 1 if not self.check_point(green_bouy.x, green_bouy.y): if self.oldx > green_bouy.x: self.aicontrol.drive ([0,bouy_data.x,bouy_data.y,0,0,0]) rospy.sleep (2) else: self.aicontrol.drive ([0,self.oldx,self.oldy,0,0,0]) rospy.sleep (2) if self.oldy > green_bouy.y: truey = self.oldy truex = self.oldx self.oldy = green_bouy.y self.oldx = green_bouy.x elif i == 1: self.aicontrol.drive_z (const.BOUY_DETECTING_DEPTH) yellow_bouy = self.detect_bouy(String('bouy'),String('yellow')) yellow_bouy = yellow_bouy.data rospy.sleep(2) self.aicontrol.drive_y(1.2) while yellow_bouy.appear == False and move != 10: self.aicontrol.stop (2) yellow_bouy = self.detect_bouy(String('bouy'),String('yellow')) yellow_bouy = yellow_bouy.data self.aicontrol.drive_y (0.1) rospy.sleep(1) move += 1 rospy.sleep(3) print 'set point' print 'finish ' + bouy_color[i] ### end for ### print 'finish 2 bouy' self.yellow_bouy () # self.find_path() def find_path (self): self.aicontrol.drive_z (const.PATH_DETECTING_DEPTH) path_data = self.detect_path(String('path1'),String('orange')) path_data = path_data.data print path_data count = 80 found = False while not rospy.is_shutdown() and not self.aicontrol.is_fail(count) : if path_data.appear: print 'found path' self.aicontrol.stop(2) break else: yy = 0.1 self.aicontrol.drive_x (0.3) for i in xrange(8): self.aicontrol.drive_y (yy) path_data = self.detect_path(String('path1'),String('orange')) path_data = path_data.data print path_data if path_data.appear: found = True break if not found: self.aicontrol.drive_y(-0.5) yy = -0.1 for i in xrange(8): self.aicontrol.drive_y (yy) path_data = self.detect_path(String('path1'),String('orange')) path_data = path_data.data print path_data if path_data.appear: found = True break count -= 1 if found: return else: self.aicontrol.drive_y (0.5) return def yellow_bouy (self): self.aicontrol.drive_z (-3) print 'do yellow bouy' count = 50 self.hw.command ('gripper', 'close') print 'open gripper' while not rospy.is_shutdown() and not self.aicontrol.is_fail(count) : bouy_data = self.detect_bouy(String('bouy'),String('yellow')) bouy_data = bouy_data.data print bouy_data if bouy_data.appear: vx = (1/bouy_data.area)*500 vy = bouy_data.x vz = bouy_data.y if bouy_data.area > 400: ### near ### print 'near' bc = 0.05 sr = 0.3 else : ### far ### print 'far' bc = 0.1 sr = 0.5 if self.aicontrol.is_center([bouy_data.x,bouy_data.y],-bc,bc,-bc,bc) : print bouy_data if bouy_data.area > 600: ### change area value print 'go to bouy' print 'drive_x 2 meter' self.aicontrol.drive_x (2) rospy.sleep (1) self.hw.command ('gripper', 'grab') rospy.sleep (1) print 'grab la na eiei' self.aicontrol.drive_z (const.BOUY_YELLOW_PULL_DEPTH) ####### CHANGE ME !!! print 'drive_z complete' print 'pull down' self.hw.command ('gripper', 'close') self.aicontrol.stop (2) print 'release' self.aicontrol.drive_x (-2) self.aicontrol.drive_z (const.PATH_DETECTING_DEPTH) self.aicontrol.drive_y (-0.8) break else: print 'drive_x 0.3 meter so far' self.aicontrol.drive_x (0.3) rospy.sleep(0.5) else : self.aicontrol.drive([0,vy,vz,0,0,0]) print 'set to center' rospy.sleep (sr) else : self.aicontrol.stop(0.2) count -= 1 ### end while ### self.aicontrol.stop (3) print 'stop state after hit bouy' self.find_path()
class Sensor: """ Defines sensor logic. It reads the distance of object continuously and calls services on the server on different events to notify server. Events are : * OBJECT_WITHIN_THRESHOLD1 : Notify server about an object which just showed up in the not-very-close distance. * OBJECT_WITHIN_THRESHOLD2 : Notify server about an object which just showed up in the close distance. It is smart enough to not to send same event over and over again. Also, if there is a very close event for a very long time, it doesn't broadcast the event to the server. """ def __init__(self): if MOCK_HARDWARE: from hardware_mock import HardwareMock self._hardware = HardwareMock() else: from hardware import Hardware self._hardware = Hardware() if MOCK_HEARTBEAT: self._heartbeat = HeartbeatMock() else: self._heartbeat = Heartbeat() if MOCK_DATA: self._sensor_service = SensorServiceMock() self._registration_service = RegistrationServiceMock() else: self._sensor_service = SensorService() self._registration_service = RegistrationService() @staticmethod def _current_time_in_millis(): # see http://stackoverflow.com/questions/5998245/get-current-time-in-milliseconds-in-python # about getting the current time in millis return int(round(time.time() * 1000)) def _start(self): log.info("Starting program...") # first of all, check all the required settings # noinspection PyBroadException try: self._sensor_info = self._registration_service.get_sensor_info_sync() except: log.exception("Unable to register! Exiting program") return if not self._sensor_info: log.critical("Unable to get sensor info. Exiting program!") # give the token to services self._sensor_service.set_token(self._sensor_info.token) self._heartbeat.set_token(self._sensor_info.token) # since server settings are all good, send a heartbeat about starting the sensor program self._heartbeat.sendSync(Constants.HEARTBEAT_STARTING) # initialize hardware. # noinspection PyBroadException try: log.info("Initializing hardware GPIO...") self._hardware.initialize_gpio() except: # do not care about clean up, since hardware does it itself log.exception("Unable to initialize hardware GPIO. Exiting program!") # send heartbeat die with message self._heartbeat.sendSync(Constants.HEARTBEAT_DIE, "Unable to initialize GPIO.", sys.exc_info()) return # send the sync heartbeat afterwards to not to mix GPIO initialization exceptions with heartbeat exceptions self._heartbeat.sendSync(Constants.HEARTBEAT_GPIO_INITIALIZED) # start heartbeat here self._heartbeat.start_heartbeat_thread() previous_broadcasted_event_type = None previous_broadcasted_event_time = 0 # start measuring while True: event_type = None try: distance = self._hardware.measure() except: # do not care about clean up, since hardware itself does it # update heartbeat status so that server knows there is something wrong with the measurement self._heartbeat.sendSync(Constants.HEARTBEAT_DIE, "Error during measure.", sys.exc_info()) # since long time if that is the case log.exception("Error during measurement!") # re-raise and eventually exit the program raise if distance < self._sensor_info.threshold1: if distance >= self._sensor_info.threshold2: # we have event type 1 event_type = Constants.EVENT_TYPE_OBJECT_WITHIN_THRESHOLD1 log.info("Object found between threshold1 and threshold2 : {} cm".format(distance)) else: # we might have event type 2. but need to check if object is too close if distance <= Constants.TOO_CLOSE_DISTANCE_THRESHOLD: # ignore log.info("Object is too close : {}".format(distance)) else: # we have event type 2 log.info("Object is withing threshold2 and it is not too close : {} cm".format(distance)) event_type = Constants.EVENT_TYPE_OBJECT_WITHIN_THRESHOLD2 else: # ignore the object since it is too far away log.info("Object is too far away : {} cm".format(distance)) pass # do not broadcast the event every time! # broadcast the event if it is new. # new means: # last broadcasted event is from a different type # OR # it has been N seconds since last broadcasted event send_broadcast = False if not event_type: send_broadcast = False else: if previous_broadcasted_event_type != event_type: send_broadcast = True else: elapsed_time_since_last_broadcast = Sensor._current_time_in_millis() - previous_broadcasted_event_time if elapsed_time_since_last_broadcast > self._sensor_info.seconds_to_ignore_same_events * 1000: send_broadcast = True else: log.info( "Not broadcasting the event since same type is broadcasted very recently : " + event_type) # noinspection PyBroadException try: if send_broadcast: log.info("Gonna broadcast event : " + event_type) self._sensor_service.broadcast_event(event_type) except: log.exception("Error broadcasting event : " + event_type) # do nothing. continue with the next measurement # sleep some time before measuring again if send_broadcast: previous_broadcasted_event_type = event_type previous_broadcasted_event_time = Sensor._current_time_in_millis() # if we do broadcast, then sleep less since some time will be gone during the REST call time.sleep(Constants.SLEEP_TIME_BEFORE_NEXT_MEASUREMENT_AFTER_BROADCAST) else: # sleep more time.sleep(Constants.SLEEP_TIME_BEFORE_NEXT_MEASUREMENT_NO_BROADCAST) def _on_exit(self): # try to clean up anyway. it is safe to do that over and over again self._hardware.clean_up() # stop heartbeat thread so that we don't send heartbeats anymore self._heartbeat.stop_heartbeat_thread() # send heartbeat die self._heartbeat.sendSync(Constants.HEARTBEAT_DIE, "Exiting program") def start_program(self): # noinspection PyBroadException try: self._start() except: # catch all unhandled exceptions # that means, program wanted to terminate log.exception("Program didn't handle the exception. Probably it wanted termination.") self._on_exit()
""" Walks through the grid map, in a very naive way """ import time import pickle from hardware import Hardware from state import State hardware = Hardware("../test/live.txt") # /dev/tty.HC-06-DevB # # hardware = Hardware(serial_port='/dev/rfcomm0', output_file='../test/live.txt') # , testfile='../test/live.txt' state = State(n_particles=150) sumdeltas = 0 current_time = 0 start = time.time() i = 0 for update in hardware.updates(): i += 1 start_time = time.time() state.update(update) stop_time = time.time() current_time += update.timedelta timedeltadelta = update.timedelta - (stop_time - start_time) * 1000 sumdeltas += timedeltadelta
class Gui(object): builder = None hardware = None vario = None datarate = None pressure_plot = None distribution_plot = None def __init__(self, feed): # load gui thisdir = os.path.dirname(__file__) gladefile = os.path.join(thisdir, "glade/main.glade") self.builder = gtk.Builder() self.builder.add_from_file(gladefile) self.datarate = DataRateAnalizer() # create and connect components self.hardware = Hardware(feed) self.pressure_plot = DataPlot() self.vario = Vario() #self.filter = AlphaBeta(1.2923, 0.86411); #self.filter = UnpredictingKalman(0.004, 0.5) self.hardware.listen("pressure", self.vario.on_pressure) self.hardware.listen("altitude", self.pressure_plot.on_raw_data) #self.distribution_plot = PressureDistributionPlot() #self.hardware.listen("pressure", self.distribution_plot.on_raw_data) #self.vario.listen("altitude", lambda k, v: self.filter.accept(v)) self.hardware.listen("filtered", self.pressure_plot.on_filtered_data) self.hardware.listen("temp", self.on_temperature) self.hardware.listen("pressure", self.on_raw_pressure) self.hardware.listen("altitude", self.on_altitude) self.hardware.listen("filtered", self.on_filtered) self.hardware.listen("velocity", self.on_vario) def run(self): window = self.builder.get_object('main_window') self.builder.connect_signals(self) figures = self.builder.get_object('figures') figures.add(self.pressure_plot.get_canvas()) #figures.add(self.distribution_plot.get_canvas()) #lbl = gtk.Label("Hello") #figures.add(lbl) self.hardware.open() window.show_all() gtk.idle_add(self.on_idle) gtk.main() def on_temperature(self, key, value): self.set_label('temperature', "%s °C" % (float(value) / 10)) def on_raw_pressure(self, key, value): raw_pressure = int(value) self.set_label('raw_pressure', self.format_pressure(raw_pressure)) def on_filtered(self, key, altitude): #self.set_label('filtered_pressure', self.format_pressure(pressure)) #alt = self.vario.pressure_to_alt(pressure) self.set_label("altitude", "%.2f m" % altitude) def on_altitude(self, key, altitude): self.set_label('raw_altitude', "%.2fm" % altitude) self.datarate.tick() if self.datarate.rate: lbl = self.builder.get_object('datarate') lbl.set_text("%.2f Hz" % (self.datarate.rate)) def on_vario(self, key, vario): self.set_label("vario", "%s%.1f m/s" % (vario >= 0 and '↑' or '↓', abs(vario))) def on_idle(self): self.hardware.read() #print "Idle" return True def on_main_window_destroy(self, widget, data=None): gtk.main_quit() def format_pressure(self, pressure): return "%.2f hPa" % (float(pressure) / 100.0) def quit(self, widget): sys.exit(0) def set_label(self, id, value): lbl = self.builder.get_object(id) lbl.set_text(value)
plt.show() return I_total # Parameters # Velocity Mode max_coil_pos = 0.0012 # 5mm T = 1.5 # Period of the sin. actuation voltage in s runningTime = 1 # Velocity Mode measuring time in s dt = 0.001 p_gain_vel = 900 i_gain_vel = 700 d_gain_vel = 10 hw = Hardware() hw.switchRelay(False) #calibrate() ##### Calibration hw.setOutput(0) input("Move the balance in a levelled position and press enter!") setpoint = hw.readFotodiode() # level position ##### Velocity Mode t = 0 t_start = time.time() # Fotodiode calibration values foto_slope = 4.9042 foto_yoffset = -0.0209
class BinnMission (object): def __init__ (self): print "Now do bin" #### BINN ## subscribe vision srv_name = 'bin_srv' rospy.wait_for_service(srv_name) print 'service starts' self.detect_binn = rospy.ServiceProxy(srv_name, Bin_Srv) #### BINN self.aicontrol = AIControl() self.hw = Hardware() def getdata (self): binn_data = self.detect_binn(String('bin'),String('white')) binn_data = binn_data.data return binn_data def run (self, cover): # if cover = 1, uncover = 0 print 'Go to bin' count = 50 self.aicontrol.drive_z (-1) while not rospy.is_shutdown() and not self.aicontrol.is_fail(count): print 'in while' binn_data = self.getdata() print binn_data i = -1 if len(binn_data.appear) == 2: if not binn_data.cover[0]: i = 0 else: i = 1 elif len(binn_data.appear) == 1 and not binn_data.cover[0]: i = 0 else: # self.aicontrol.stop(1) print 'not found' count -= 1 if i != -1: if self.aicontrol.is_center ([binn_data.x[i],binn_data.y[i]],-0.05,0.05,-0.05,0.05) and -3 < binn_data.angle[i] < 3: print 'Center' print binn_data.x[i] print binn_data.y[i] self.aicontrol.drive_z (const.BIN_DETECTING_DEPTH) ##### DEPTH !!! rospy.sleep (1) break elif self.aicontrol.is_center ([binn_data.x[i],binn_data.y[i]],-0.05,0.05,-0.05,0.05): self.aicontrol.turn_yaw_relative (binn_data.angle[i]) rospy.sleep (1) else : print 'Not Center' vx = binn_data.x[i] vy = binn_data.y[i] self.aicontrol.drive ([vx,vy,0,0,0,0]) rospy.sleep(1) rospy.sleep(0.25) ''' if cover == 1: # self.hw.command ('gripper', 'grab') ### grab self.aicontrol.drive_z (-2) ### up -> open binn ### self.aicontrol.drive ([0,1,0,0,0,0]) ### move -> drop cover ### rospy.sleep(0.1) # self.hw.command ('gripper', 'leave') ### leave cover alone self.aicontrol.drive ([0,-1,0,0,0,0]) ### move back to above bin ### rospy.sleep(0.1) self.aicontrol.drive_z (-2.8) ''' ## drop x2 times self.aicontrol.drive_z (const.BIN_DROP_DEPTH) self.aicontrol.stop (2) self.hw.command('drop_left', 'drop') self.aicontrol.stop (2) print 'drop laew eiei' rospy.sleep(1) self.aicontrol.drive_y (0.3) rospy.sleep(1) self.hw.command('drop_right', 'drop') rospy.sleep(1) print 'drop marker yet' print 'bin complete' return