Context Navigation

source: robotics/car/python/Car_Demo.py @ c2cf7f2

Last change on this file since c2cf7f2 was c2cf7f2, checked in by Steve Castellotti <sc@…>, 8 years ago
working on command line control
Property mode set to `100755`
File size: 13.2 KB

Line
1	#!/usr/bin/env python
2	# -- coding: utf-8 --
3	#
4	# Puzzlebox - Robotics - Car - Demo
5	#
6	# Copyright Puzzlebox Productions, LLC (2014)
7
8	__changelog__ = """\
9	Last Update: 2014.06.10
10	"""
11
12	__todo__ = """
13	"""
14
15	import string, sys, time
16	import serial
17	import signal
18
19	#import Configuration as configuration
20
21	#if configuration.ENABLE_PYSIDE:
22	try:
23	import PySide as PyQt4
24	from PySide import QtCore
25	except Exception, e:
26	print "ERROR: Exception importing PySide:",
27	print e
28	#configuration.ENABLE_PYSIDE = False
29	#else:
30	#print "INFO: [TAG] Using PySide module"
31
32	#if not configuration.ENABLE_PYSIDE:
33	#print "INFO: [TAG] Using PyQt4 module"
34	#from PyQt4 import QtCore
35
36	#import Puzzlebox.Jigsaw.Configuration as configuration
37	##import puzzlebox_logger
38
39
40	#####################################################################
41	# Globals
42	#####################################################################
43
44	DEBUG = 1
45
46	TAG = 'Robotics:Car'
47
48	DEFAULT_COMMAND = 'neutral'
49	#DEFAULT_SERIAL_DEVICE = '/dev/ttyACM0'
50	DEFAULT_SERIAL_DEVICE_STEPPER = '/dev/ttyACM0'
51	DEFAULT_SERIAL_DEVICE_PYRAMID = '/dev/ttyUSB0'
52
53	DEFAULT_MODE = "pyramid"
54	#DEFAULT_MODE = "pair"
55
56	DEFAULT_ARDUINO_BAUD_RATE = 9600
57	DEFAULT_STEPPER_BAUD_RATE = 115200
58	DEFAULT_PYRAMID_BAUD_RATE = 115200
59
60	ARDUINO_INITIALIZATION_TIME = 2
61	#ARDUINO_INITIALIZATION_TIME = 12
62
63	#THROTTLE_POWER_MODIFIER = 0.25
64	#STEERING_POWER_MODIFIER = 0.75
65
66	LOOP_TIMER = 80 # 80 ms
67
68	KEY_CODES = {
69	'hover': 'muf008001t100020y000000p000000d', \
70	'forward': 'mff008001t100020y000000p063005d', \
71	'backward': 'mbf008001t100020y000000p015003d', \
72	'left': 'mlf008001t100020y127004p000000d', \
73	'right': 'mrf008001t100020y000008p000000d', \
74	'land': 'O', \
75	}
76
77
78	#####################################################################
79	# Classes
80	#####################################################################
81
82	class puzzlebox_robotics_car_demo(QtCore.QThread):
83
84	def __init__(self, log, \
85	serial_port_stepper=DEFAULT_SERIAL_DEVICE_STEPPER, \
86	serial_port_pyramid=DEFAULT_SERIAL_DEVICE_PYRAMID, \
87	mode=DEFAULT_MODE, \
88	command=DEFAULT_COMMAND, \
89	DEBUG=DEBUG, parent=None):
90
91	QtCore.QThread.__init__(self,parent)
92
93	self.log = log
94	self.DEBUG = DEBUG
95	self.parent = parent
96	self.serial_port_stepper = serial_port_stepper
97	self.serial_port_pyramid = serial_port_pyramid
98	self.mode = mode
99
100	self.command = command
101
102	self.device_stepper = None
103	self.device_pyramid = None
104
105	#self.configuration = configuration
106
107	self.name = "Robotics-Car-Demo"
108
109	self.command_queue = []
110
111
112	try:
113	self.initializeSerial()
114	except Exception, e:
115	if self.DEBUG:
116	print "<-- [TAG] Device connection failed"
117	print "ERROR [TAG]:",
118	print e
119
120
121	self.keep_running = True
122
123
124	##################################################################
125
126	def initializeSerial(self):
127
128	if self.mode == 'pyramid':
129	baudrate = DEFAULT_PYRAMID_BAUD_RATE
130	rts_cts_flow_control = 'f'
131	else:
132	baudrate = DEFAULT_ARDUINO_BAUD_RATE
133	rts_cts_flow_control = 't'
134
135	if self.DEBUG:
136	print "INFO [TAG]: Stepper serial device:",
137	print self.serial_port_stepper
138	print "INFO [TAG]: Connecting at baud rate",
139	print baudrate
140
141	print "INFO [TAG]: Pyramid serial device:",
142	print self.serial_port_pyramid
143	print "INFO [TAG]: Pyramid at baud rate",
144	print baudrate
145
146	bytesize = 8
147	parity = 'NONE'
148	stopbits = 1
149	software_flow_control = 'f'
150	#timeout = 15
151	timeout = 5
152
153	# convert bytesize
154	if (bytesize == 5):
155	init_byte_size = serial.FIVEBITS
156	elif (bytesize == 6):
157	init_byte_size = serial.SIXBITS
158	elif (bytesize == 7):
159	init_byte_size = serial.SEVENBITS
160	elif (bytesize == 8):
161	init_byte_size = serial.EIGHTBITS
162	else:
163	#self.log.perror("Invalid value for %s modem byte size! Using default (8)" % modem_type)
164	init_byte_size = serial.EIGHTBITS
165
166	# convert parity
167	if (parity == 'NONE'):
168	init_parity = serial.PARITY_NONE
169	elif (parity == 'EVEN'):
170	init_parity = serial.PARITY_EVEN
171	elif (parity == 'ODD'):
172	init_parity = serial.PARITY_ODD
173	else:
174	#self.log.perror("Invalid value for %s modem parity! Using default (NONE)" % modem_type)
175	init_parity = serial.PARITY_NONE
176
177	# convert stopbits
178	if (stopbits == 1):
179	init_stopbits = serial.STOPBITS_ONE
180	elif (stopbits == 2):
181	init_stopbits = serial.STOPBITS_TWO
182	else:
183	#self.log.perror("Invalid value for %s modem stopbits! Using default (8)" % modem_type)
184	init_byte_size = serial.STOPBITS_ONE
185
186	# convert software flow control
187	if (software_flow_control == 't'):
188	init_software_flow_control = 1
189	else:
190	init_software_flow_control = 0
191
192	# convert rts cts flow control
193	if (rts_cts_flow_control == 't'):
194	init_rts_cts_flow_control = 1
195	else:
196	init_rts_cts_flow_control = 0
197
198	#self.device_stepper = serial.Serial(port = self.serial_port_stepper, \
199	##baudrate = baudrate, \
200	##bytesize = init_byte_size, \
201	##parity = init_parity, \
202	##stopbits = init_stopbits, \
203	##xonxoff = init_software_flow_control, \
204	##rtscts = init_rts_cts_flow_control, \
205	##timeout = timeout)
206
207	self.device_pyramid = serial.Serial(port = self.serial_port_pyramid, \
208	baudrate = baudrate, \
209	bytesize = init_byte_size, \
210	parity = init_parity, \
211	stopbits = init_stopbits, \
212	xonxoff = init_software_flow_control, \
213	rtscts = init_rts_cts_flow_control, \
214	timeout = timeout)
215
216	#self.land()
217	self.left()
218
219	time.sleep(ARDUINO_INITIALIZATION_TIME)
220
221
222	##################################################################
223
224	def get_status(self, connection):
225
226	status = 'Connected'
227
228	return(status)
229
230
231	##################################################################
232
233	def hover(self):
234
235	"Set throttle to hover"
236
237	# Issue command
238	if self.DEBUG:
239	print '--> [TAG] Hover ("P")'
240
241	#self.device.write("P")
242	self.command_queue.append(command)
243
244
245	##################################################################
246
247	def land(self):
248
249	"Set throttle to zero"
250
251	# Issue command
252	if self.DEBUG:
253	print '--> [%s] Land ("O")' % TAG
254
255	self.command_queue.append("O")
256	#self.device_pyramid.write("O")
257
258
259	##################################################################
260
261	def left(self):
262
263	"Drive left"
264
265	# Issue command
266	if self.DEBUG:
267	print '--> [%s] Left ("mlf008001t100020y127004p000000d")' % TAG
268
269	self.command_queue.append("mlf008001t100020y127004p000000d")
270	#self.device_pyramid.write("O")
271
272
273	##################################################################
274
275	def right(self):
276
277	"Drive right"
278
279	# Issue command
280	if self.DEBUG:
281	print '--> [%s] Left ("mrf008001t100020y000008p000000d")' % TAG
282
283	self.command_queue.append("mrf008001t100020y000008p000000d")
284	#self.device_pyramid.write("O")
285
286
287	##################################################################
288
289	def padValue(self, value):
290
291	while len(value) < 3:
292	value = '0%s' % value
293
294	return value
295
296
297	##################################################################
298
299	def setThrottle(self, throttle):
300
301	value = self.padValue( str(throttle) )
302
303	command = 't%s' % value
304
305	if self.DEBUG:
306	print "--> [%s] Sending Orbit Command:" % TAG,
307	print command
308
309	#self.device.write(command)
310	self.command_queue.append(command)
311
312
313	##################################################################
314
315	def setYaw(self, yaw):
316
317	value = self.padValue( str(yaw) )
318
319	command = 'y%s' % value
320
321	if self.DEBUG:
322	print "--> [TAG] Sending Orbit Command:",
323	print command
324
325	#self.device.write(command)
326	self.command_queue.append(command)
327
328
329	##################################################################
330
331	def setPitch(self, pitch):
332
333	value = self.padValue( str(pitch) )
334
335	command = 'p%s' % value
336
337	if self.DEBUG:
338	print "--> [TAG] Sending Orbit Command:",
339	print command
340
341	#self.device.write(command)
342	self.command_queue.append(command)
343
344
345	##################################################################
346
347	def sendCommand(self, command, power=None):
348
349	if self.DEBUG:
350	print "INFO: [TAG] Sending Orbit Command:",
351	print command
352
353	self.command_queue.append(command)
354
355
356	##################################################################
357
358	def convertColorToString(self, color):
359
360	if len("%s" % color) == 1:
361	output = "00%i" % color
362	elif len("%s" % color) == 2:
363	output = "0%i" % color
364	elif len("%s" % color) == 3:
365	output = "%i" % color
366	else:
367	output = "%i" % color
368	output = output[:3]
369
370
371	return(output)
372
373
374	##################################################################
375
376	def setColorWheel(self, red, green, blue):
377
378	red = self.convertColorToString(red)
379	green = self.convertColorToString(green)
380	blue = self.convertColorToString(blue)
381
382	command = "w%s%s%s" % (red, green, blue)
383
384	if self.DEBUG:
385	print "INFO: [TAG] Sending Orbit Command:",
386	print command
387
388	self.command_queue.append(command)
389	#self.device.write(command)
390
391
392	##################################################################
393
394	def setColor(self, position, red, green, blue):
395
396	position = position - 1 # convert from counting number
397
398	if len("%s" % position) == 1:
399	position = "0%s" % position
400	else:
401	position = "%s" % position
402
403	red = convertColorToString(red)
404	green = convertColorToString(green)
405	blue = convertColorToString(blue)
406
407	command = "c%s%s%s%s" % (position, red, green, blue)
408
409	if self.DEBUG:
410	print "INFO: [TAG] Sending Orbit Command:",
411	print command
412
413	self.command_queue.append(command)
414	#self.device.write(command)
415
416
417	##################################################################
418
419	def processCommand(self, command):
420
421	if (command == 'hover'):
422	self.hover()
423
424	elif (command == 'land'):
425	self.land()
426
427	elif (command =='left'):
428	self.left()
429
430	elif (command == 'right'):
431	self.right()
432
433
434	else:
435	self.device_pyramid.write(command)
436
437
438	##################################################################
439
440	def cycleTest(self):
441
442	print "cycleTest(On)"
443	print "..."
444	self.serial_port_pyramid.write('P\n')
445	print "written: P\r"
446	#QtCore.QThread.msleep(1000)
447	time.sleep(1)
448
449	print "cycleTest(Off)"
450	self.serial_port_pyramid.write('O\n')
451	print "written: O\r"
452	#QtCore.QThread.msleep(1000)
453	time.sleep(1)
454
455
456	##################################################################
457
458	def cycleDemo(self):
459
460	print "cycleDemo()"
461
462	self.serial_port_stepper.write('r090\r')
463	self.serial_port_pyramid.write(KEY_CODES['right'] + '\r')
464
465	self.serial_port_stepper.write('l090\r')
466	QtCore.QThread.msleep(1000)
467	self.serial_port_pyramid.write(KEY_CODES['left'] + '\r')
468	QtCore.QThread.msleep(1000)
469
470
471	##################################################################
472
473	def run(self):
474
475	while self.keep_running:
476
477	#self.cycleDemo()
478
479	#self.cycleTest()
480
481
482	#if self.command_queue != []:
483	while self.command_queue != []:
484
485	command = self.command_queue[0]
486	self.command_queue = self.command_queue[1:]
487
488	try:
489	self.processCommand(command)
490	pass
491	except Exception, e:
492	print "ERROR: [TAG] Exception sending car commands:",
493	print e
494
495
496	# Sleep 80 ms
497	QtCore.QThread.msleep(LOOP_TIMER)
498
499
500	##################################################################
501
502	def stop(self):
503
504	#self.land()
505
506	self.keep_running = False
507
508	self.command_queue = []
509
510	try:
511	self.device.close()
512	except Exception, e:
513	if self.DEBUG:
514	print "ERROR: [TAG] failed to close device in stop():",
515	print e
516
517
518	##################################################################
519
520	def exitThread(self, callThreadQuit=True):
521
522	self.stop()
523
524	if callThreadQuit:
525	try:
526	QtCore.QThread.quit(self)
527	except Exception, e:
528	if self.DEBUG:
529	print "ERROR: [TAG] failed to call QtCore.QThread.quit(self) in exitThread():",
530	print e
531
532
533	#####################################################################
534	# Functions
535	#####################################################################
536
537	#####################################################################
538	# Main
539	#####################################################################
540
541	if __name__ == '__main__':
542
543	# Perform correct KeyboardInterrupt handling
544	signal.signal(signal.SIGINT, signal.SIG_DFL)
545
546	# Collect default settings and command line parameters
547	device = DEFAULT_SERIAL_DEVICE_PYRAMID
548	mode = DEFAULT_MODE
549	command = DEFAULT_COMMAND
550
551	for each in sys.argv:
552
553	if each.startswith("--device="):
554	device = each[ len("--device="): ]
555	elif each.startswith("--mode="):
556	mode = each[ len("--mode="): ]
557	elif each.startswith("--command="):
558	command = each[ len("--command="): ]
559
560
561	app = QtCore.QCoreApplication(sys.argv)
562
563	demo = puzzlebox_robotics_car_demo( \
564	None, \
565	serial_port_stepper=DEFAULT_SERIAL_DEVICE_STEPPER, \
566	serial_port_pyramid=DEFAULT_SERIAL_DEVICE_PYRAMID, \
567	mode=mode, \
568	command=command, \
569	DEBUG=DEBUG)
570
571	demo.start()
572
573	sys.exit(app.exec_())

Note: See TracBrowser for help on using the repository browser.

Download in other formats: