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
