Changeset 240
- Timestamp:
- 12/04/10 14:04:26 (12 years ago)
- Location:
- trunk/brainstorms
- Files:
-
- 1 added
- 2 edited
- 1 moved
Legend:
- Unmodified
- Added
- Removed
-
trunk/brainstorms/Puzzlebox/Brainstorms/Helicopter_Control.py
r238 r240 244 244 245 245 def processCommand(self): 246 247 248 #self.configureRemote()249 250 246 251 247 if (self.command == 'dump_packets') or (self.command == 'read'): -
trunk/brainstorms/Puzzlebox/Brainstorms/Wheelchair_Control.py
r239 r240 14 14 """ 15 15 16 import sys 16 17 import time 17 18 import signal … … 34 35 DEBUG = 1 35 36 36 DEFAULT_COMMAND = ' stop'37 DEFAULT_COMMAND = 'console' 37 38 DEFAULT_SERIAL_DEVICE = '/dev/ttyACM0' 38 39 ARDUINO_INITIALIZATION_TIME = 2 39 40 COMMAND_CHARACTER = 'x' 40 41 41 42 WHEELCHAIR_COMMANDS = { 42 'stop': '00110011', 43 'stop': '00110011', 44 'speed0': { 45 'stop': '00110011', 46 }, 47 'speed1': { 48 'forward': '00110001', 49 'reverse': '00111011', 50 'left': '10110011', 51 'right': '00010011' 52 }, 53 'speed2': { 54 'forward': '00110010', 55 'reverse': '00110111', 56 'left': '01110011', 57 'right': '00100011' 58 }, 59 'speed3': { 60 'forward': '00110000', 61 'reverse': '00111111', 62 'left': '11110011', 63 'right': '00000011' 64 }, 43 65 } 44 66 … … 68 90 STOP_INTERVAL = 0.2 69 91 ALARM_INTERVAL = 0.1 70 #PORT = parallel.Parallel()71 92 72 93 ##################################################################### … … 96 117 ################################################################## 97 118 98 def run(self): 99 100 pass 101 102 103 ################################################################## 104 105 def exitThread(self, callThreadQuit=True): 106 107 try: 108 self.device.stop() 109 except: 110 pass 111 112 #self.wait() 113 if callThreadQuit: 114 QtCore.QThread.quit(self) 115 116 117 ################################################################## 118 119 def initializeSerial(): 119 def initializeSerial(self): 120 120 121 121 baudrate = 9600 … … 182 182 timeout = timeout) 183 183 184 device.write('%s%s' % COMMAND_CHARACTER, DEFAULT_COMMAND)184 device.write('%s%s' % (COMMAND_CHARACTER, WHEELCHAIR_COMMANDS['stop'])) 185 185 if self.DEBUG: 186 186 print "--> Wheelchair Command:", 187 print DEFAULT_COMMAND188 189 time.sleep( 2)187 print 'stop' 188 189 time.sleep(ARDUINO_INITIALIZATION_TIME) 190 190 191 191 return(device) 192 193 194 ################################################################## 195 196 def processCommand(self): 197 198 if (self.command == 'console'): 199 self.console_control() 200 201 #elif (self.command == 'neutral'): 202 #self.mode = 'write' 203 #self.neutral() 204 205 206 ################################################################## 207 208 def alarmHandler(self, arg1, arg2): 209 print 'alarm!' 210 if STOP_TIME < time.time(): 211 self.stopBot() 212 213 214 def initAlarm(self): 215 signal.alarm(ALARM_INTERVAL) 216 signal.signal(signal.SIGALRM, alarmHandler) 217 218 219 def setOutput(self, data): 220 output = data ^ int('00110011', 2) 221 222 output = self.int2bin(output) 223 self.device.write('x%s' % output) 224 225 #print 'Output set to: ', int2bin(output) 226 print 'Output set to: ', output 227 print 'commands: i j k m to move, SPACE = stop, x = quit' 228 229 230 def moveBot(self, dir, speed): 231 output = 0 232 pins = CONTROLLS[dir] 233 if speed == 1: 234 output = pins[0] 235 elif speed == 2: 236 output = pins[1] 237 elif speed == 3: 238 output = pins[0] | pins[1] 239 self.setOutput(output) 240 time.sleep(STOP_INTERVAL) 241 self.stopBot() 242 243 244 def stopBot(self): 245 self.setOutput(0) 246 247 248 def int2bin(self, n, count=8): 249 return "".join([str((n >> y) & 1) for y in range(count-1, -1, -1)]) 250 251 252 ################################################################## 253 254 def console_control(self): 255 256 #device = self.initializeSerial() 257 258 MYGETCH = Getch() 259 260 #initAlarm() 261 262 self.stopBot() 263 264 speed = 1 265 while True: 266 cmd = MYGETCH() 267 if cmd == 'x': 268 exit() 269 if cmd in SPEED_COMMANDS.keys(): 270 speed = int(cmd) 271 print SPEED_COMMANDS[cmd] 272 elif cmd in MOVE_COMANDS.keys(): 273 if MOVE_COMANDS[cmd] == 'stop': 274 self.stopBot() 275 else: 276 print MOVE_COMANDS[cmd] 277 self.moveBot(MOVE_COMANDS[cmd], speed) 278 STOP_TIME = time.time() + STOP_INTERVAL 279 280 281 ################################################################## 282 283 def run(self): 284 285 if self.DEBUG: 286 print "<---- [%s] Main thread running" % "Wheelchair Control" 287 288 289 self.processCommand() 290 291 self.exec_() 292 293 294 ################################################################## 295 296 def exitThread(self, callThreadQuit=True): 297 298 try: 299 self.device.stop() 300 except: 301 pass 302 303 #self.wait() 304 if callThreadQuit: 305 QtCore.QThread.quit(self) 192 306 193 307 … … 196 310 197 311 class Getch: 312 198 313 def __init__(self): 199 314 import tty, sys 200 315 201 316 def __call__(self): 202 317 import sys, tty, termios … … 215 330 ##################################################################### 216 331 217 def alarmHandler(arg1, arg2):218 print 'alarm!'219 if STOP_TIME < time.time():220 stopBot()221 222 def initAlarm():223 signal.alarm(ALARM_INTERVAL)224 signal.signal(signal.SIGALRM, alarmHandler)225 226 def setOutput(data):227 output = data ^ int('00110011', 2)228 #PORT.setData(output)229 230 output = int2bin(output)231 device.write('x%s' % output)232 233 #print 'Output set to: ', int2bin(output)234 print 'Output set to: ', output235 print 'commands: i j k m to move, SPACE = stop, x = quit'236 237 def moveBot(dir, speed):238 output = 0239 pins = CONTROLLS[dir]240 if speed == 1:241 output = pins[0]242 elif speed == 2:243 output = pins[1]244 elif speed == 3:245 output = pins[0] | pins[1]246 setOutput(output)247 time.sleep(STOP_INTERVAL)248 stopBot()249 250 def stopBot():251 setOutput(0)252 253 def int2bin(n, count=8):254 return "".join([str((n >> y) & 1) for y in range(count-1, -1, -1)])255 256 #####################################################################257 258 def initializeSerial():259 baudrate = 9600260 bytesize = 8261 parity = 'NONE'262 stopbits = 1263 software_flow_control = 'f'264 rts_cts_flow_control = 't'265 #timeout = 15266 timeout = 5267 268 # convert bytesize269 if (bytesize == 5):270 init_byte_size = serial.FIVEBITS271 elif (bytesize == 6):272 init_byte_size = serial.SIXBITS273 elif (bytesize == 7):274 init_byte_size = serial.SEVENBITS275 elif (bytesize == 8):276 init_byte_size = serial.EIGHTBITS277 else:278 #self.log.perror("Invalid value for %s modem byte size! Using default (8)" % modem_type)279 init_byte_size = serial.EIGHTBITS280 281 # convert parity282 if (parity == 'NONE'):283 init_parity = serial.PARITY_NONE284 elif (parity == 'EVEN'):285 init_parity = serial.PARITY_EVEN286 elif (parity == 'ODD'):287 init_parity = serial.PARITY_ODD288 else:289 #self.log.perror("Invalid value for %s modem parity! Using default (NONE)" % modem_type)290 init_parity = serial.PARITY_NONE291 292 # convert stopbits293 if (stopbits == 1):294 init_stopbits = serial.STOPBITS_ONE295 elif (stopbits == 2):296 init_stopbits = serial.STOPBITS_TWO297 else:298 #self.log.perror("Invalid value for %s modem stopbits! Using default (8)" % modem_type)299 init_byte_size = serial.STOPBITS_ONE300 301 # convert software flow control302 if (software_flow_control == 't'):303 init_software_flow_control = 1304 else:305 init_software_flow_control = 0306 307 # convert rts cts flow control308 if (rts_cts_flow_control == 't'):309 init_rts_cts_flow_control = 1310 else:311 init_rts_cts_flow_control = 0312 313 device = serial.Serial(port = '/dev/ttyACM0', \314 baudrate = baudrate, \315 bytesize = init_byte_size, \316 parity = init_parity, \317 stopbits = init_stopbits, \318 xonxoff = init_software_flow_control, \319 rtscts = init_rts_cts_flow_control, \320 timeout = timeout)321 322 device.write('x00110011')323 print "sent stop"324 time.sleep(2)325 print "slept 2"326 327 return(device)328 329 330 332 ##################################################################### 331 333 # Main … … 333 335 334 336 if __name__ == '__main__': 335 336 device = initializeSerial() 337 338 MYGETCH = Getch() 339 340 #initAlarm() 341 342 stopBot() 343 344 speed = 1 345 while True: 346 cmd = MYGETCH() 347 if cmd == 'x': 348 exit() 349 if cmd in SPEED_COMMANDS.keys(): 350 speed = int(cmd) 351 print SPEED_COMMANDS[cmd] 352 elif cmd in MOVE_COMANDS.keys(): 353 if MOVE_COMANDS[cmd] == 'stop': 354 stopBot() 355 else: 356 print MOVE_COMANDS[cmd] 357 moveBot(MOVE_COMANDS[cmd], speed) 358 STOP_TIME = time.time() + STOP_INTERVAL 337 338 # Perform correct KeyboardInterrupt handling 339 signal.signal(signal.SIGINT, signal.SIG_DFL) 340 341 # Collect default settings and command line parameters 342 device = DEFAULT_SERIAL_DEVICE 343 command = DEFAULT_COMMAND 344 345 for each in sys.argv: 346 347 if each.startswith("--device="): 348 device = each[ len("--device="): ] 349 elif each.startswith("--command="): 350 command = each[ len("--command="): ] 351 352 353 app = QtCore.QCoreApplication(sys.argv) 354 355 wheelchair = puzzlebox_brainstorms_wheelchair_control( \ 356 device_address=device, \ 357 command=command, \ 358 DEBUG=DEBUG) 359 360 wheelchair.start() 361 362 sys.exit(app.exec_()) -
trunk/brainstorms/arduino/puzzlebox_brainstorms_wheelchair_noisebridge/puzzlebox_brainstorms_wheelchair_noisebridge.pde
r239 r240 43 43 digitalWrite(7,g); 44 44 digitalWrite(8,h); 45 46 display_values(); 45 47 46 Serial.println('x'); 48 } 49 50 void display_values() { 47 51 52 Serial.print('x'); 53 54 if (h == true) 55 Serial.print("1"); 56 else 57 Serial.print("0"); 58 59 if (a == true) 60 Serial.print("1"); 61 else 62 Serial.print("0"); 63 64 if (b == true) 65 Serial.print("1"); 66 else 67 Serial.print("0"); 68 69 if (c == true) 70 Serial.print("1"); 71 else 72 Serial.print("0"); 73 74 if (d == true) 75 Serial.print("1"); 76 else 77 Serial.print("0"); 78 79 if (e == true) 80 Serial.print("1"); 81 else 82 Serial.print("0"); 83 84 if (f == true) 85 Serial.print("1"); 86 else 87 Serial.print("0"); 88 89 if (g == true) 90 Serial.print("1"); 91 else 92 Serial.print("0"); 93 94 Serial.println(); 48 95 } 49 96 … … 91 138 a = (inByte =='1'); 92 139 } 93 140 141 display_values(); 94 142 //Serial.println(char(a),char(b),char(c),char(d),char(e),char(f),char(g),char(h)); 95 143 //Serial.println(a, b, c, d, e, f, g ,h);
Note: See TracChangeset
for help on using the changeset viewer.