| 1 | #!/usr/bin/env python |
|---|
| 2 | # -*- coding: utf-8 -*- |
|---|
| 3 | # |
|---|
| 4 | # Puzzlebox - Brainstorms - Wheelchair Control |
|---|
| 5 | # |
|---|
| 6 | # Copyright Puzzlebox Productions, LLC (2010) |
|---|
| 7 | # |
|---|
| 8 | # This code is released under the GNU Pulic License (GPL) version 2 |
|---|
| 9 | # For more information please refer to http://www.gnu.org/copyleft/gpl.html |
|---|
| 10 | |
|---|
| 11 | __changelog__ = """ |
|---|
| 12 | Last Update: 2010.12.04 |
|---|
| 13 | |
|---|
| 14 | """ |
|---|
| 15 | |
|---|
| 16 | import sys |
|---|
| 17 | import time |
|---|
| 18 | import signal |
|---|
| 19 | #import parallel |
|---|
| 20 | import serial |
|---|
| 21 | |
|---|
| 22 | try: |
|---|
| 23 | import PySide as PyQt4 |
|---|
| 24 | from PySide import QtCore |
|---|
| 25 | except: |
|---|
| 26 | print "Using PyQt4 module" |
|---|
| 27 | from PyQt4 import QtCore |
|---|
| 28 | else: |
|---|
| 29 | print "Using PySide module" |
|---|
| 30 | |
|---|
| 31 | ##################################################################### |
|---|
| 32 | # Globals |
|---|
| 33 | ##################################################################### |
|---|
| 34 | |
|---|
| 35 | DEBUG = 1 |
|---|
| 36 | |
|---|
| 37 | DEFAULT_COMMAND = 'console' |
|---|
| 38 | DEFAULT_SERIAL_DEVICE = '/dev/ttyACM0' |
|---|
| 39 | |
|---|
| 40 | DEFAULT_WHEELCHAIR_SPEED = 1 |
|---|
| 41 | DEFAULT_WHEELCHAIR_COMMAND = 'stop' |
|---|
| 42 | |
|---|
| 43 | ARDUINO_INITIALIZATION_TIME = 2 |
|---|
| 44 | COMMAND_CHARACTER = 'x' |
|---|
| 45 | GUI_SLEEP_TIMER = 1 * 100 # 100ms |
|---|
| 46 | |
|---|
| 47 | WHEELCHAIR_COMMANDS = { |
|---|
| 48 | 1: { # speed |
|---|
| 49 | 'forward': '00110001', |
|---|
| 50 | 'reverse': '00111011', |
|---|
| 51 | 'left': '10110011', |
|---|
| 52 | 'right': '00010011', |
|---|
| 53 | 'stop': '00110011', |
|---|
| 54 | }, |
|---|
| 55 | 2: { # speed |
|---|
| 56 | 'forward': '00110010', |
|---|
| 57 | 'reverse': '00110111', |
|---|
| 58 | 'left': '01110011', |
|---|
| 59 | 'right': '00100011', |
|---|
| 60 | 'stop': '00110011', |
|---|
| 61 | }, |
|---|
| 62 | 3: { # speed |
|---|
| 63 | 'forward': '00110000', |
|---|
| 64 | 'reverse': '00111111', |
|---|
| 65 | 'left': '11110011', |
|---|
| 66 | 'right': '00000011', |
|---|
| 67 | 'stop': '00110011', |
|---|
| 68 | }, |
|---|
| 69 | } |
|---|
| 70 | |
|---|
| 71 | # 'dir' : (low_pin, high_pin) |
|---|
| 72 | CONTROLLS = { |
|---|
| 73 | 'fwd' : (1<<1, 1<<0), |
|---|
| 74 | 'rev' : (1<<3, 1<<2), |
|---|
| 75 | 'right' : (1<<5, 1<<4), |
|---|
| 76 | 'left' : (1<<7, 1<<6), |
|---|
| 77 | } |
|---|
| 78 | |
|---|
| 79 | MOVE_COMANDS ={ |
|---|
| 80 | ' ' : 'stop', |
|---|
| 81 | 'i' : 'fwd', |
|---|
| 82 | 'm' : 'rev', |
|---|
| 83 | 'j' : 'left', |
|---|
| 84 | 'k' : 'right', |
|---|
| 85 | } |
|---|
| 86 | |
|---|
| 87 | SPEED_COMMANDS = { |
|---|
| 88 | '1' : 'speed-1', |
|---|
| 89 | '2' : 'speed-2', |
|---|
| 90 | '3' : 'speed-3', |
|---|
| 91 | } |
|---|
| 92 | |
|---|
| 93 | STOP_TIME = 0 |
|---|
| 94 | STOP_INTERVAL = 0.2 |
|---|
| 95 | ALARM_INTERVAL = 0.1 |
|---|
| 96 | |
|---|
| 97 | ##################################################################### |
|---|
| 98 | # Classes |
|---|
| 99 | ##################################################################### |
|---|
| 100 | |
|---|
| 101 | class puzzlebox_brainstorms_wheelchair_control(QtCore.QThread): |
|---|
| 102 | |
|---|
| 103 | def __init__(self, \ |
|---|
| 104 | device_address=DEFAULT_SERIAL_DEVICE, \ |
|---|
| 105 | command=DEFAULT_COMMAND, \ |
|---|
| 106 | DEBUG=DEBUG, \ |
|---|
| 107 | parent=None): |
|---|
| 108 | |
|---|
| 109 | QtCore.QThread.__init__(self, parent) |
|---|
| 110 | |
|---|
| 111 | self.log = None |
|---|
| 112 | self.DEBUG = DEBUG |
|---|
| 113 | self.parent = parent |
|---|
| 114 | |
|---|
| 115 | self.device_address = device_address |
|---|
| 116 | self.command = command |
|---|
| 117 | |
|---|
| 118 | self.wheelchair_speed = DEFAULT_WHEELCHAIR_SPEED |
|---|
| 119 | self.wheelchair_command = DEFAULT_WHEELCHAIR_COMMAND |
|---|
| 120 | |
|---|
| 121 | self.device = None |
|---|
| 122 | self.initializeSerial() |
|---|
| 123 | |
|---|
| 124 | self.keep_running = True |
|---|
| 125 | |
|---|
| 126 | |
|---|
| 127 | ################################################################## |
|---|
| 128 | |
|---|
| 129 | def initializeSerial(self): |
|---|
| 130 | |
|---|
| 131 | baudrate = 9600 |
|---|
| 132 | bytesize = 8 |
|---|
| 133 | parity = 'NONE' |
|---|
| 134 | stopbits = 1 |
|---|
| 135 | software_flow_control = 'f' |
|---|
| 136 | rts_cts_flow_control = 't' |
|---|
| 137 | #timeout = 15 |
|---|
| 138 | timeout = 5 |
|---|
| 139 | |
|---|
| 140 | # convert bytesize |
|---|
| 141 | if (bytesize == 5): |
|---|
| 142 | init_byte_size = serial.FIVEBITS |
|---|
| 143 | elif (bytesize == 6): |
|---|
| 144 | init_byte_size = serial.SIXBITS |
|---|
| 145 | elif (bytesize == 7): |
|---|
| 146 | init_byte_size = serial.SEVENBITS |
|---|
| 147 | elif (bytesize == 8): |
|---|
| 148 | init_byte_size = serial.EIGHTBITS |
|---|
| 149 | else: |
|---|
| 150 | #self.log.perror("Invalid value for %s modem byte size! Using default (8)" % modem_type) |
|---|
| 151 | init_byte_size = serial.EIGHTBITS |
|---|
| 152 | |
|---|
| 153 | # convert parity |
|---|
| 154 | if (parity == 'NONE'): |
|---|
| 155 | init_parity = serial.PARITY_NONE |
|---|
| 156 | elif (parity == 'EVEN'): |
|---|
| 157 | init_parity = serial.PARITY_EVEN |
|---|
| 158 | elif (parity == 'ODD'): |
|---|
| 159 | init_parity = serial.PARITY_ODD |
|---|
| 160 | else: |
|---|
| 161 | #self.log.perror("Invalid value for %s modem parity! Using default (NONE)" % modem_type) |
|---|
| 162 | init_parity = serial.PARITY_NONE |
|---|
| 163 | |
|---|
| 164 | # convert stopbits |
|---|
| 165 | if (stopbits == 1): |
|---|
| 166 | init_stopbits = serial.STOPBITS_ONE |
|---|
| 167 | elif (stopbits == 2): |
|---|
| 168 | init_stopbits = serial.STOPBITS_TWO |
|---|
| 169 | else: |
|---|
| 170 | #self.log.perror("Invalid value for %s modem stopbits! Using default (8)" % modem_type) |
|---|
| 171 | init_byte_size = serial.STOPBITS_ONE |
|---|
| 172 | |
|---|
| 173 | # convert software flow control |
|---|
| 174 | if (software_flow_control == 't'): |
|---|
| 175 | init_software_flow_control = 1 |
|---|
| 176 | else: |
|---|
| 177 | init_software_flow_control = 0 |
|---|
| 178 | |
|---|
| 179 | # convert rts cts flow control |
|---|
| 180 | if (rts_cts_flow_control == 't'): |
|---|
| 181 | init_rts_cts_flow_control = 1 |
|---|
| 182 | else: |
|---|
| 183 | init_rts_cts_flow_control = 0 |
|---|
| 184 | |
|---|
| 185 | self.device = serial.Serial(port = self.device_address, \ |
|---|
| 186 | baudrate = baudrate, \ |
|---|
| 187 | bytesize = init_byte_size, \ |
|---|
| 188 | parity = init_parity, \ |
|---|
| 189 | stopbits = init_stopbits, \ |
|---|
| 190 | xonxoff = init_software_flow_control, \ |
|---|
| 191 | rtscts = init_rts_cts_flow_control, \ |
|---|
| 192 | timeout = timeout) |
|---|
| 193 | |
|---|
| 194 | |
|---|
| 195 | self.sendCommand(self.wheelchair_speed, self.wheelchair_command) |
|---|
| 196 | |
|---|
| 197 | time.sleep(ARDUINO_INITIALIZATION_TIME) |
|---|
| 198 | |
|---|
| 199 | |
|---|
| 200 | ################################################################## |
|---|
| 201 | |
|---|
| 202 | def sendCommand(self, speed, command): |
|---|
| 203 | |
|---|
| 204 | self.device.write('%s%s' % (COMMAND_CHARACTER, \ |
|---|
| 205 | WHEELCHAIR_COMMANDS[speed][command])) |
|---|
| 206 | |
|---|
| 207 | if self.DEBUG: |
|---|
| 208 | print "--> Wheelchair Command: %s [Speed %i]" % \ |
|---|
| 209 | (command, speed) |
|---|
| 210 | |
|---|
| 211 | self.wheelchair_speed = speed |
|---|
| 212 | self.wheelchair_command = command |
|---|
| 213 | |
|---|
| 214 | |
|---|
| 215 | ################################################################## |
|---|
| 216 | |
|---|
| 217 | def alarmHandler(self, arg1, arg2): |
|---|
| 218 | print 'alarm!' |
|---|
| 219 | if STOP_TIME < time.time(): |
|---|
| 220 | self.stopBot() |
|---|
| 221 | |
|---|
| 222 | |
|---|
| 223 | def initAlarm(self): |
|---|
| 224 | signal.alarm(ALARM_INTERVAL) |
|---|
| 225 | signal.signal(signal.SIGALRM, alarmHandler) |
|---|
| 226 | |
|---|
| 227 | |
|---|
| 228 | def setOutput(self, data): |
|---|
| 229 | output = data ^ int('00110011', 2) |
|---|
| 230 | |
|---|
| 231 | output = self.int2bin(output) |
|---|
| 232 | self.device.write('x%s' % output) |
|---|
| 233 | |
|---|
| 234 | #print 'Output set to: ', int2bin(output) |
|---|
| 235 | print 'Output set to: ', output |
|---|
| 236 | print 'commands: i j k m to move, SPACE = stop, x = quit' |
|---|
| 237 | |
|---|
| 238 | |
|---|
| 239 | def moveBot(self, dir, speed): |
|---|
| 240 | output = 0 |
|---|
| 241 | pins = CONTROLLS[dir] |
|---|
| 242 | if speed == 1: |
|---|
| 243 | output = pins[0] |
|---|
| 244 | elif speed == 2: |
|---|
| 245 | output = pins[1] |
|---|
| 246 | elif speed == 3: |
|---|
| 247 | output = pins[0] | pins[1] |
|---|
| 248 | self.setOutput(output) |
|---|
| 249 | time.sleep(STOP_INTERVAL) |
|---|
| 250 | self.stopBot() |
|---|
| 251 | |
|---|
| 252 | |
|---|
| 253 | def stopBot(self): |
|---|
| 254 | self.setOutput(0) |
|---|
| 255 | |
|---|
| 256 | |
|---|
| 257 | def int2bin(self, n, count=8): |
|---|
| 258 | return "".join([str((n >> y) & 1) for y in range(count-1, -1, -1)]) |
|---|
| 259 | |
|---|
| 260 | |
|---|
| 261 | ################################################################## |
|---|
| 262 | |
|---|
| 263 | def consoleControl(self): |
|---|
| 264 | |
|---|
| 265 | if (sys.platform == 'win32'): |
|---|
| 266 | if self.DEBUG: |
|---|
| 267 | print "---> Wheelchair Control: Console mode unavailable under Windows" |
|---|
| 268 | |
|---|
| 269 | self.exitThread() |
|---|
| 270 | |
|---|
| 271 | |
|---|
| 272 | MYGETCH = Getch() |
|---|
| 273 | |
|---|
| 274 | #initAlarm() |
|---|
| 275 | |
|---|
| 276 | self.stopBot() |
|---|
| 277 | |
|---|
| 278 | speed = DEFAULT_WHEELCHAIR_SPEED |
|---|
| 279 | while True: |
|---|
| 280 | cmd = MYGETCH() |
|---|
| 281 | if cmd == 'x': |
|---|
| 282 | exit() |
|---|
| 283 | if cmd in SPEED_COMMANDS.keys(): |
|---|
| 284 | speed = int(cmd) |
|---|
| 285 | print SPEED_COMMANDS[cmd] |
|---|
| 286 | elif cmd in MOVE_COMANDS.keys(): |
|---|
| 287 | if MOVE_COMANDS[cmd] == 'stop': |
|---|
| 288 | self.stopBot() |
|---|
| 289 | else: |
|---|
| 290 | print MOVE_COMANDS[cmd] |
|---|
| 291 | self.moveBot(MOVE_COMANDS[cmd], speed) |
|---|
| 292 | STOP_TIME = time.time() + STOP_INTERVAL |
|---|
| 293 | |
|---|
| 294 | |
|---|
| 295 | ################################################################## |
|---|
| 296 | |
|---|
| 297 | #def guiControl(self): |
|---|
| 298 | |
|---|
| 299 | #while self.keep_running: |
|---|
| 300 | |
|---|
| 301 | #QtCore.QThread.msleep(GUI_SLEEP_TIMER) |
|---|
| 302 | |
|---|
| 303 | |
|---|
| 304 | ################################################################## |
|---|
| 305 | |
|---|
| 306 | def processCommand(self): |
|---|
| 307 | |
|---|
| 308 | if (self.command == 'console'): |
|---|
| 309 | self.consoleControl() |
|---|
| 310 | |
|---|
| 311 | #elif (self.command == 'gui'): |
|---|
| 312 | #self.gui_control() |
|---|
| 313 | |
|---|
| 314 | |
|---|
| 315 | ################################################################## |
|---|
| 316 | |
|---|
| 317 | def run(self): |
|---|
| 318 | |
|---|
| 319 | if self.DEBUG: |
|---|
| 320 | print "<---- [%s] Main thread running" % "Wheelchair Control" |
|---|
| 321 | |
|---|
| 322 | |
|---|
| 323 | self.processCommand() |
|---|
| 324 | |
|---|
| 325 | self.exec_() |
|---|
| 326 | |
|---|
| 327 | |
|---|
| 328 | ################################################################## |
|---|
| 329 | |
|---|
| 330 | def exitThread(self, callThreadQuit=True): |
|---|
| 331 | |
|---|
| 332 | try: |
|---|
| 333 | self.device.stop() |
|---|
| 334 | except: |
|---|
| 335 | pass |
|---|
| 336 | |
|---|
| 337 | #self.wait() |
|---|
| 338 | if callThreadQuit: |
|---|
| 339 | QtCore.QThread.quit(self) |
|---|
| 340 | |
|---|
| 341 | |
|---|
| 342 | ##################################################################### |
|---|
| 343 | ##################################################################### |
|---|
| 344 | |
|---|
| 345 | class Getch: |
|---|
| 346 | |
|---|
| 347 | def __init__(self): |
|---|
| 348 | import tty, sys |
|---|
| 349 | |
|---|
| 350 | def __call__(self): |
|---|
| 351 | import sys, tty, termios |
|---|
| 352 | fd = sys.stdin.fileno() |
|---|
| 353 | old_settings = termios.tcgetattr(fd) |
|---|
| 354 | try: |
|---|
| 355 | tty.setraw(sys.stdin.fileno()) |
|---|
| 356 | ch = sys.stdin.read(1) |
|---|
| 357 | finally: |
|---|
| 358 | termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) |
|---|
| 359 | return ch |
|---|
| 360 | |
|---|
| 361 | |
|---|
| 362 | ##################################################################### |
|---|
| 363 | # Functions |
|---|
| 364 | ##################################################################### |
|---|
| 365 | |
|---|
| 366 | ##################################################################### |
|---|
| 367 | # Main |
|---|
| 368 | ##################################################################### |
|---|
| 369 | |
|---|
| 370 | if __name__ == '__main__': |
|---|
| 371 | |
|---|
| 372 | # Perform correct KeyboardInterrupt handling |
|---|
| 373 | signal.signal(signal.SIGINT, signal.SIG_DFL) |
|---|
| 374 | |
|---|
| 375 | # Collect default settings and command line parameters |
|---|
| 376 | device = DEFAULT_SERIAL_DEVICE |
|---|
| 377 | command = DEFAULT_COMMAND |
|---|
| 378 | |
|---|
| 379 | for each in sys.argv: |
|---|
| 380 | |
|---|
| 381 | if each.startswith("--device="): |
|---|
| 382 | device = each[ len("--device="): ] |
|---|
| 383 | elif each.startswith("--command="): |
|---|
| 384 | command = each[ len("--command="): ] |
|---|
| 385 | |
|---|
| 386 | |
|---|
| 387 | app = QtCore.QCoreApplication(sys.argv) |
|---|
| 388 | |
|---|
| 389 | wheelchair = puzzlebox_brainstorms_wheelchair_control( \ |
|---|
| 390 | device_address=device, \ |
|---|
| 391 | command=command, \ |
|---|
| 392 | DEBUG=DEBUG) |
|---|
| 393 | |
|---|
| 394 | wheelchair.start() |
|---|
| 395 | |
|---|
| 396 | sys.exit(app.exec_()) |
|---|
