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()
