class AzathothClient: def __init__(self): self.connected = False self.builder = gtk.Builder() self.builder.add_from_file('main.glade') self.mainWindow = self.builder.get_object('window_main') self.builder.connect_signals(self) # let's just add widgets as members programatically. widgets = ('btn_disconnect', 'btn_connect', 'statusbar', 'label_js_x', 'label_js_y', 'rb_js_enable', 'rb_js_disable', 'eb_js_x', 'eb_js_y', 'eb_drivestatus', 'label_drivestatus', 'label_drive_x', 'label_drive_y', 'label_raw_x', 'label_raw_y', 'label_select_cmd', 'label_select_act', 'eb_select_cmd', 'eb_select_act', 'label_estop_cmd', 'label_estop_act', 'eb_estop_cmd', 'eb_estop_act') for wid in widgets: setattr(self, wid, self.builder.get_object(wid)) self.context_id = self.statusbar.get_context_id("Azathoth") self.prev_x = 0 self.prev_y = 0 self.joystick = None self.joystick_x = 0 self.joystick_y = 0 self.joystick_enabled = False self.mainWindow.show_all() def connect(self, host, port=2024): log.msg(format="Connecting to host %(host)s on port %(port)d", host=host, port=port) self.statusbar.push(self.context_id, 'Connecting...') self.factory = ControlFactory(self) self.connection = reactor.connectTCP(host, port, self.factory) def disconnect(self): log.msg("Disconnecting") self.connection.disconnect() def setUiState(self, state): connected_controls = ('btn_disconnect', 'rb_js_enable', 'rb_js_disable', 'btn_select', 'btn_deselect', 'tb_estop', 'tb_reset', 'tb_calibrate', 'imi_calibration') if state == 'connecting': self.btn_disconnect.set_sensitive(True) self.btn_connect.set_sensitive(False) elif state == 'connected': for control in connected_controls: self.builder.get_object(control).set_sensitive(True) elif state == 'disconnected': if self.joystick_enabled: self.rb_js_enable.set_active(False) self.rb_js_disable.set_active(True) self.disableJoystick() for control in connected_controls: self.builder.get_object(control).set_sensitive(False) self.btn_connect.set_sensitive(True) def enableJoystick(self): self.joystick_enabled = True try: self.joystick = Joystick(1) except: self.joystick_enabled = False self.rb_js_enable.set_active(True) self.rb_js_disable.set_active(False) self.statusbar.push(self.context_id, 'Joystick device error') return self.js_handler = self.joystick.connect('axis', self.axis_event) self.factory.control.send_select_command(True) def disableJoystick(self): self.joystick_enabled = False self.rb_js_enable.set_active(True) self.rb_js_disable.set_active(False) if self.joystick is not None: self.joystick.disconnect(self.js_handler) self.joystick.shutdown() self.joystick = None self.factory.control.send_select_command(False) def onStartConnection(self): self.setUiState('connecting') def onConnect(self): self.statusbar.remove_all(self.context_id) self.statusbar.push(self.context_id, 'Connected') self.setUiState('connected') def onConnectionLost(self): self.setUiState('disconnected') self.statusbar.remove_all(self.context_id) self.statusbar.push(self.context_id, 'Disconnected') if self.calDlg is not None: self.calDlg.destroy() def onConnectionFailed(self, reason): self.setUiState('disconnected') self.statusbar.remove_all(self.context_id) self.statusbar.push(self.context_id, 'Connection failed!') def onUpdateAxis(self): if self.joystick_x != 0: self.eb_js_x.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#00FF00')) elif self.joystick_x == 0: self.eb_js_x.modify_bg(gtk.STATE_NORMAL, None) if self.joystick_y != 0: self.eb_js_y.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#00FF00')) elif self.joystick_y == 0: self.eb_js_y.modify_bg(gtk.STATE_NORMAL, None) self.label_js_x.set_label(str(self.joystick_x)) self.label_js_y.set_label(str(self.joystick_y)) self.factory.control.send_joystick_command(self.joystick_x, self.joystick_y) def onStatusUpdate(self, status, xpos, ypos, xval, yval): estop_act = (status & 0x01 == 0x01) estop_cmd = (status & 0x02 == 0x02) select_act = (status & 0x04 == 0x04) select_cmd = (status & 0x08 == 0x08) moving = (status & 0x10 == 0x10) if moving: self.label_drivestatus.set_label("MOVING") self.eb_drivestatus.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#00FF00')) else: self.label_drivestatus.set_label("STOPPED") self.eb_drivestatus.modify_bg(gtk.STATE_NORMAL, None) if estop_act: self.label_estop_act.set_label("RUN") self.eb_estop_act.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#00FF00')) else: self.label_estop_act.set_label("STOP") self.eb_estop_act.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#FF0000')) if estop_cmd: self.label_estop_cmd.set_label("RUN") self.eb_estop_cmd.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#00FF00')) else: self.label_estop_cmd.set_label("STOP") self.eb_estop_cmd.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#FF0000')) if select_act: self.label_select_act.set_label("ROBOT") self.eb_select_act.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#00FF00')) else: self.label_select_act.set_label("CHAIR") self.eb_select_act.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#00FFFF')) if select_cmd: self.label_select_cmd.set_label("ROBOT") self.eb_select_cmd.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#00FF00')) else: self.label_select_cmd.set_label("CHAIR") self.eb_select_cmd.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#00FFFF')) self.label_drive_x.set_label(str(xpos)) self.label_drive_y.set_label(str(ypos)) self.label_raw_x.set_label(str(xval)) self.label_raw_y.set_label(str(yval)) def on_window_main_delete_event(self, win, event): reactor.stop() def on_imi_quit_activate(self, widget): reactor.stop() def on_imi_calibration_activate(self, widget): self.calDlg = CalibrationDialog(self) def on_btn_connect_clicked(self, widget): host = self.builder.get_object('entry_host').get_text() self.connect(host) def on_btn_disconnect_clicked(self, widget): self.disconnect() def on_rb_js_enable_toggled(self, btn): if btn.get_active(): self.enableJoystick() else: self.disableJoystick() def on_btn_select_clicked(self, btn): self.factory.control.send_select_command(True) def on_btn_deselect_clicked(self, btn): self.factory.control.send_select_command(False) def on_tb_estop_clicked(self, btn): self.factory.control.send_estop_command() def on_tb_reset_clicked(self, btn): self.factory.control.send_reset_command() def on_tb_calibrate_clicked(self, btn): self.calDlg = CalibrationDialog(self) def axis_event(self, object, axis, value, init): if init == 128: # Ignore this event. One gets sent per axis when the joystick # is initialized. I should really find out why. return if axis == 0: # dividing by 256 scales the value to fit within a signed char self.prev_x = self.joystick_x self.joystick_x = value / 256 if self.joystick_x == self.prev_x: return if axis == 1: # this axis needs to be inverted self.prev_y = self.joystick_y self.joystick_y = -value / 256 if self.joystick_y == self.prev_y: return self.onUpdateAxis() def button_event(self, object, button, value, init): pass
class WheelChairController: def __init__(self): #channel numbers self.errorLightOut = 4 self.errorLightLow = 18 self.switchOut = 13 self.switchDetect = 20 #constants self.rs_stopped = 0 self.rs_forward = 1 self.rs_reverse = 2 self.ss_nothing = 0 #nothing has happened yet self.ss_start = 1 #both eyes open complete self.ss_forward1 = 2 #one eye open complete self.ss_forward2 = 3 #both eyes closed complete self.ss_reverse1 = 4 #both eyes closed complete self.redFac = .75 self.addFac = 1 - self.redFac self.maxSwitchTime = 7 #leave room for waiting and messing up self.maxReverseTime = 5 self.defaultPrevX = 0 self.defaultPrevY = -.5 #vars self.runState = self.rs_stopped self.runTime = 0 self.switchState = self.ss_nothing self.switchTime = 0 self.prevX = self.defaultPrevX self.prevY = self.defaultPrevY self.joystick = Joystick() io.setmode(io.BCM) io.setup(self.errorLightOut, io.OUT, initial=io.LOW) io.setup(self.errorLightLow, io.OUT, initial=io.LOW) io.setup(self.switchOut, io.OUT, initial=io.HIGH) io.setup(self.switchDetect, io.IN, pull_up_down=io.PUD_DOWN) time.sleep(.2) #wait for io to definitely turn on before attempting to sense if self.should_shut_down(): print("waiting for switch on") io.wait_for_edge(self.switchDetect, io.RISING) #wait for switch to turn on. self.set_led(io.HIGH) #indicate success time.sleep(.3) #wait so people know for sure that its working def sig_no_eye(self, now): #print("sig no eye") pass #probs just for debugging def sig_direction(self, x, y): #print("go ", x, ", ", y) self.prevX = self.prevX * self.redFac + x * self.addFac self.prevY = self.prevY * self.redFac + y * self.addFac if self.runState == self.rs_forward: self.joystick.forward(self.prevX, self.prevY) def sig_error_reset(self): #print("turn on error light") self.joystick.stop() self.set_led(io.HIGH) def sig_turn_off_error_light(self): self.set_led(io.LOW) def process_blink_commands(self, useGroups, now): temp = sorted(useGroups, key=lambda group: group.boxCenterX) eyes = len(useGroups) isLeft = True left = False right = False for group in temp: if group.lastOpen == now: if isLeft: left = True isLeft = False else: right = True self.joystick.showEyes(left, right) counts1 = self.count_blinks(useGroups, 1, now) #be, bd, ne, nd if now - self.switchTime > self.maxSwitchTime and not (self.switchState == self.ss_start and counts1[3] == 2): # don't timeout if during start (because that would be confusing) if eyes are mostly open still if self.switchState != self.ss_nothing: print("canceled switch due to timeout") self.switchState = self.ss_nothing if (self.runState == self.rs_reverse and now - self.runTime > self.maxReverseTime) or eyes == 0: self.stop(now) print("no eye stop") elif eyes == 2: #other eyes == 1 case is just continue forward countsHalf = self.count_blinks(useGroups, .5, now) if self.runState == self.rs_forward: if counts1[3] < 2 or countsHalf[1] == 2: #not both ~open1 or both =closed.5 self.stop(now) print("forward stop") elif self.runState == self.rs_reverse: if counts1[3] == 2 or countsHalf[1] == 2: #both ~open1 or both =closed.5 self.stop(now) print("reverse stop") elif self.runState == self.rs_stopped: if self.switchState == self.ss_nothing: if counts1[2] == 2: #both =open1 self.switchState = self.ss_start print("ss_start") self.switchTime = now else: print("still nothing") elif self.switchState == self.ss_start: if counts1[0] == 2: #both =closed1 self.switchState = self.ss_reverse1 self.switchTime = now print("ss_reverse1") elif counts1[1] == 1 and counts1[3] == 1: #one ~open1, one ~closed1 self.switchState = self.ss_forward1 self.swtichTime = now print("ss_forward1") else: print("still start") elif self.switchState == self.ss_reverse1: if counts1[1] == 1 and counts1[3] == 1: #one ~open1, one ~closed1 self.start_reverse(now) elif counts1[2] == 2: #both =open1 self.switchState = self.ss_start self.switchTime = now print("cancel") else: print("still reverse1") elif self.switchState == self.ss_forward1: if counts1[0] == 2: #both =closed1 self.switchState = self.ss_forward2 self.switchTime = now print("ss_forward2") elif counts1[2] == 2: #both =open1 self.switchState = self.ss_start self.switchTime = now print("cancel") else: print("still forward1") elif self.switchState == self.ss_forward2: if counts1[2] == 2: #both =open1 self.start_forward(now) elif counts1[0] == 1 and counts1[2] == 1: #one =open1, one =closed1 self.switchState = self.ss_start self.switchTime = now print("cancel") else: print("still forward2") else: #something went terribly wrong print("aww nuts") self.stop(now) else: #something went terribly wrong print("awww nutz") self.stop(now) def count_blinks(self, useGroups, timeReq, now): #return format: [blinking_exact, blinking_denoised, notBlinking_exact, notBlinking_denoised] be = sum((now - group.lastOpen) > timeReq for group in useGroups) bd = sum((now - group.lastDefOpen) > timeReq for group in useGroups) ne = sum((now - group.lastClosed) > timeReq for group in useGroups) nd = sum((now - group.lastDefClosed) > timeReq for group in useGroups) return [be, bd, ne, nd] def stop(self, now): self.runState = self.rs_stopped self.switchState = self.ss_nothing self.joystick.stop() self.runTime = now def start_forward(self, now): self.runState = self.rs_forward self.switchState = self.ss_nothing self.joystick.forward(self.prevX, self.prevY) self.runTime = now print("start forward") def start_reverse(self, now): self.runState = self.rs_reverse self.switchState = self.ss_nothing self.joystick.reverse() self.runTime = now print("start reverse") def should_shut_down(self): return io.input(self.switchDetect) == io.LOW def set_led(self, state): io.output(self.errorLightOut, state) def release_assets(self): print("releasing all assets") self.joystick.stop() self.joystick.shutdown() io.cleanup()