Changeset 749 for trunk

Timestamp:

06/06/10 16:01:35 (20 months ago)

Author:

mosttw

Message:

CAN work

Location:

trunk/software/rb

Files:

: 2 modified

can.py (modified) (11 diffs)
controller.py (modified) (1 diff)

Legend:

: Unmodified
: Added
: Removed

trunk/software/rb/can.py

r747	r749
1	1	#!/usr/bin/python -u
2	2	# Copyright (C) 2010 Thomas W. Most
3		#
4		~~# For the moment, don't consider this GPL --- need to make sure that~~
5		~~# the terms of the documentation are compatible.~~
6	3
7	4	from __future__ import division
…	…
14	11	import logging
15	12	from serial import Serial
	13	from rb.utils import clip
	14
	15	MAX_RPM = 150
	16	VOLTAGE_RAMP = int(0xffff * 0.2)
16	17
17	18	# CAN device types
…	…
64	65	escaped_bytes = jaguar_escape(bytes)
65	66	message = '\xff%c%s' % (len(bytes), bytes)
66		print "writing", hex_bytes(message)
	67	#print "writing", hex_bytes(message)
67	68	try:
68	69	self.serial.write(message)
…	…
81	82	while self.serial.inWaiting():
82	83	message += self.serial.read(self.serial.inWaiting())
83		print "read", hex_bytes(message)
	84	#print "read", hex_bytes(message)
84	85	return message
85	86
…	…
191	192	def __init__(self, can_bus, mode='voltage'):
192	193	assert can_bus is not None
	194	assert mode in ('voltage', 'speed')
193	195	self.can_bus = can_bus
	196	self.mode = mode
	197
194	198	self.left_controller_nums = [2, 3]
195	199	self.right_controller_nums = [4, 5]
196	200	self.controller_nums = (self.left_controller_nums +
197	201	self.right_controller_nums)
198		if mode == 'speed':
	202
	203	self.lspeed = 0
	204	self.rspeed = 0
	205
	206	if self.mode == 'voltage':
	207	self.init_voltage_control()
	208	elif self.mode == 'speed':
199	209	self.init_speed_control(p=0.35, i=0.003, d=0.001)
	210
	211	def init_voltage_control(self):
	212	'''
	213	Configure the voltage ramp rate so that we don't maul the hardware.
	214	'''
	215	for num in self.controller_nums:
	216	id = build_can_id(
	217	api_class=API_VOLTAGE_CONTROL,
	218	api_index=VOLTAGE_RAMP_SET,
	219	device_number=num
	220	)
	221	self.can_bus.send(id, struct.pack('<H', VOLTAGE_RAMP))
200	222
201	223	def init_speed_control(self, p, i, d):
…	…
245	267	Set the target speed of the wheels.
246	268	'''
247		assert lvalue <= 1.0
248		assert lvalue >= -1.0
249		assert rvalue <= 1.0
250		assert rvalue >= -1.0
251
252		self.set_voltage(lvalue, rvalue)
	269	print "setspeed({0}, {1}) called, {2} mode".format(lvalue, rvalue, self.mode)
	270	lvalue = clip(lvalue, -1.0, 1.0)
	271	rvalue = clip(rvalue, -1.0, 1.0)
	272
	273	self.lspeed = lvalue
	274	self.rspeed = rvalue
	275
	276	if self.mode == 'voltage':
	277	self.set_voltage(lvalue, rvalue)
	278	else:
	279	self.set_speed(lvalue, rvalue)
	280
	281	def getspeed(self):
	282	# The speed is a lie
	283	return (self.lspeed, self.rspeed)
	284
	285	def gettarget(self):
	286	# The target is a lie
	287	return (self.lspeed, self.rspeed)
253	288
254	289	def set_voltage(self, lvoltage, rvoltage):
…	…
285	320	from -1 to 1.
286	321	'''
	322	lrpm = lspeed * MAX_RPM
	323	rrpm = rspeed * MAX_RPM
287	324
288	325	for num in self.left_controller_nums:
…	…
292	329	device_number=num
293	330	)
294		self.can_bus.send(id, float_to_fixed1616(l~~speed~~))
	331	self.can_bus.send(id, float_to_fixed1616(lrpm))
295	332
296	333	for num in self.right_controller_nums:
…	…
300	337	device_number=num
301	338	)
302		self.can_bus.send(id, float_to_fixed1616(r~~speed~~))
	339	self.can_bus.send(id, float_to_fixed1616(rrpm))
303	340
304	341	def demo():
…	…
317	354	print "Ramping up"
318	355	for dc in range(0, 32767, 100):
319		~~#bus.send(heartbeat_can_id)~~
320	356	bus.send(set_voltage_can_id, struct.pack('<h', dc))
321	357	time.sleep(0.05)
322	358	print "At full speed (Ctrl-C to ramp down)"
323	359	while True:
324		~~#bus.send(heartbeat_can_id)~~
325	360	bus.send(set_voltage_can_id, struct.pack('<h', 32767))
326	361	time.sleep(0.05)
…	…
328	363	print "Ramping down"
329	364	for dc in range(32767, 0, -100):
330		~~#bus.send(heartbeat_can_id)~~
331	365	bus.send(set_voltage_can_id, struct.pack('<h', dc))
332	366	time.sleep(0.05)

trunk/software/rb/controller.py

r727	r749
33	33
34	34	class Controller(object):
35		'''
36		A controller forms the core logic of the robot. It is responsible for
37		initializing hardware and routing sensor data everywhere it must go.
38		'''
39
40		def __init__(self, options):
41		self._thread_id = get_ident()
42
43		# Core status attributes
44		self.data_queue = Queue()
45		self.navigator = 'gui'
46		self.prev_navigator = 'gui'
47
48		self.gps = False
49		self.microstrain = False
50		self.camera = False
51		self.drive = False
52
53		self.kalman = False
54		self.transcript = False
55		self.drive_ctrl = False
56
57		self.auto = False
58		self.shell = False
59		self.jaus = False
60		self.wiimote = False
61		self.gui = False
62
63		# Initialize hardware interfaces
64		self._init_gps()
65		self._init_microstrain()
66		self._init_camera()
67		self._init_drive()
68
69		# Navigator helpers
70		self._init_kalman()
71		self._init_transcript()
72		self._init_drive_ctrl()
73
74		# Initialize navigators
75		self._run_auto()
76		self._run_shell()
77		self._run_jaus()
78		self._run_wiimote()
79		self._run_gui()
80
81		# Start the controller
82		self._main()
83
84		def test_drive_ctrl(self):
85		'Temporary method to run the drive control stuff for testing.'
86		print ">>>>>>>>> Driving the robot <<<<<<<"
87		if self.drive_ctrl:
88		self.drive_ctrl.move()
89
90
91		# Imports are kept in the initialization methods so that
92		# they need not be done when not necessary (since it's nice
93		# to not have to build C modules if all you want to do is
94		# use the GPS).
95
96		# Hardware interfaces
97		def _init_gps(self):
98		if rbconfig.gps == REAL:
99		from rb.gps import GPS
100		self.gps = GPS(self)
101		elif rbconfig.gps == FAKE:
102		from rb.simulator import FakeGPS
103		self.gps = FakeGPS(self)
104
105		def _init_microstrain(self):
106		if rbconfig.microstrain == REAL:
107		from rb.microstrain import MicroStrain
108		self.microstrain = MicroStrain(self, rbconfig.microstrain_port)
109		elif rbconfig.microstrain == FAKE:
110		from rb.simulator import FakeMicroStrain
111		self.microstrain = FakeMicroStrain(self)
112
113		def _init_camera(self):
114		if rbconfig.camera == REAL:
115		from rb.vision.gstcam import GstCamera
116		self.camera = GstCamera(self, rbconfig.camera_gst_input)
117		elif rbconfig.camera == FAKE:
118		logger.error("Fake camera TODO")
119
120		def _init_drive(self):
121		if rbconfig.drive == REAL:
122		from rb.drive import Drive
123		self.drive = Drive(self,
124		rbconfig.drive_left_port,
125		rbconfig.drive_right_port)
126		elif rbconfig.drive == FAKE:
127		from rb.simulator import FakeDrive
128		self.drive = FakeDrive(self,
129		rbconfig.drive_left_port,
130		rbconfig.drive_right_port)
131
132		# Navigator helpers
133		def _init_kalman(self):
134		if rbconfig.kalman != NONE:
135		from rb.kalman import KalmanFilter
136		self.kalman = KalmanFilter(self)
137
138
139		def _init_transcript(self):
140		if rbconfig.transcript != NONE:
141		from rb.transcript import TranscriptLogger
142		self.transcript = TranscriptLogger(
143		datetime.now().strftime(rbconfig.transcript_filename),
144		self.drive, self.gps, self.microstrain)
145
146		def _init_drive_ctrl(self):
147		if rbconfig.drive_ctrl != NONE:
148		from rb.drive_ctrl import DriveCtrl
149		self.drive_ctrl = DriveCtrl(self)
150
151		# Navigators
152		def _run_auto(self):
153		#if rbconfig.auto != NONE:
154		#from rb.auto import Auto
155		#self.auto = Auto(self)
156		# Hardcode this here to allow starting the motor control test code as "auto" from the GUI
157		self.auto = True
158
159		def _run_jaus(self):
160		if rbconfig.jaus != NONE:
161		from rb.jaus import JAUS
162		self.jaus = JAUS(self, rbconfig.jaus_subsystem_id)
163
164		def _run_wiimote(self):
165		if rbconfig.wiimote != NONE:
166		from rb.wiimote import Wiimote
167		self.wiimote = Wiimote(self)
168
169		def _run_shell(self):
170		if rbconfig.shell != NONE:
171		from rb.shell import Shell
172		self.shell = Shell(self)
173
174		def _run_gui(self):
175		if rbconfig.gui != NONE:
176		from rb.gui.gui import GUI
177		self.gui = GUI(self)
178
179		##
180		# Misc Methods
181		def set_navigator(self, nav):
182		'''
183		Set the current navigator, performing any initialization necessary. This
184		method blocks for up to 30 seconds when first switching to the 'wiimote'
185		navigator while it connects. Return True on success, else False. Will
186		return True if `nav` matches the already-set navigator (but won't do any
187		extra initialization).
188		'''
189		if nav == self.navigator:
190		logger.debug("Tried to set navigator to %r but already was %r", nav, self.navigator)
191		return True
192
193		if nav == 'wiimote':
194		if not self.wiimote:
195		logger.error("Wiimote navigator is not loaded; cannot switch to it")
196		return False
197		if not self.wiimote.is_connected():
198		return False
199
200		elif nav == 'auto':
201		if not self.auto:
202		logger.error("Autonomous navigator is not loaded; cannot switch to it")
203		return False
204		start_new_thread(self.test_drive_ctrl, ())
205
206		elif nav == 'gui':
207		if not self.gui:
208		logger.error("GUI is not loaded; cannot use it as a navigator")
209		return False
210
211		else:
212		logger.error("Cannot switch to unknown navigator %r", nav)
213		return False
214
215		logger.info("Switching to navigator %r", nav)
216		self.prev_navigator = self.navigator
217		self.navigator = nav
218		self.process('navigator_changed', nav)
219		return True
220
221		def set_prev_navigator(self):
222		'''
223		Go to the navigator used before the current one.
224		'''
225		self.set_navigator(self.prev_navigator)
226
227		##
228		# Data routing
229		def quit(self):
230		'''
231		Helper method for killing main loop, can be used in a callback
232		'''
233		self.running = False
234
235		def _main(self):
236		'''
237		The controller's GObject mainloop and data processing loop.
238		'''
239		# Run a GLib main loop, which doesn't require GTK
240		# This is a bit complicated, but seems to work well
241		loop = gobject.MainLoop()
242
243		def dataloop(self):
244		"The controller's main data processing loop."
245		logger.debug("Entering data processing loop")
246		self.running = True
247		while self.running:
248		try:
249		type, data = self.data_queue.get(timeout=.1)
250		self.process(type, data)
251		except Empty:
252		pass
253		except Exception, e:
254		logger.exception("Error when processing %s data %r", type, data)
255		logger.debug("Exiting data processing loop")
256		gobject.idle_add(loop.quit)
257
258		start_new_thread(dataloop, (self,))
259		logger.debug("Entering glib main loop")
260		loop.run()
261
262
263		def send(self, type, data):
264		'''
265		Send the data `data` for processing. `type` is a string
266		indicating the type of data, such as "gps" or "microstrain".
267		'''
268		self.data_queue.put((type, data))
269
270		def process(self, type, data):
271		'''
272		Process the data `data` of the type `type`. By default
273		this just updates the Kalman filter. This function is
274		called inside the controller event loop, so stuff dealing
275		with GTK+ will need to move things to GTK's main loop.
276		'''
277		if type == 'gps_fix':
278		if self.kalman:
279		self.kalman.update_gps(data)
280
281		elif type == 'microstrain':
282		if self.kalman:
283		self.kalman.update_microstrain(data)
284
285		elif type == 'drive_targets':
286		pass
287
288		elif type == 'drive_speeds':
289		if self.kalman:
290		self.kalman.update_drive_speed(data)
291
292
293		for interface in (self.auto, self.wiimote, self.shell, self.gui, self.jaus):
294		if interface is not None and hasattr(interface, "process"):
295		interface.process(type, data)
296
	35	'''
	36	A controller forms the core logic of the robot. It is responsible for
	37	initializing hardware and routing sensor data everywhere it must go.
	38	'''
	39
	40	def __init__(self, options):
	41	self._thread_id = get_ident()
	42
	43	# Core status attributes
	44	self.data_queue = Queue()
	45	self.navigator = 'gui'
	46	self.prev_navigator = 'gui'
	47
	48	self.gps = False
	49	self.microstrain = False
	50	self.camera = False
	51	self.drive = False
	52
	53	self.kalman = False
	54	self.transcript = False
	55	self.drive_ctrl = False
	56
	57	self.auto = False
	58	self.shell = False
	59	self.jaus = False
	60	self.wiimote = False
	61	self.gui = False
	62
	63	# Initialize hardware interfaces
	64	self._init_gps()
	65	self._init_microstrain()
	66	self._init_camera()
	67	self._init_drive()
	68
	69	# Navigator helpers
	70	self._init_kalman()
	71	self._init_transcript()
	72	self._init_drive_ctrl()
	73
	74	# Initialize navigators
	75	self._run_auto()
	76	self._run_shell()
	77	self._run_jaus()
	78	self._run_wiimote()
	79	self._run_gui()
	80
	81	# Start the controller
	82	self._main()
	83
	84	def test_drive_ctrl(self):
	85	'Temporary method to run the drive control stuff for testing.'
	86	print ">>>>>>>>> Driving the robot <<<<<<<"
	87	if self.drive_ctrl:
	88	self.drive_ctrl.move()
	89
	90
	91	# Imports are kept in the initialization methods so that
	92	# they need not be done when not necessary (since it's nice
	93	# to not have to build C modules if all you want to do is
	94	# use the GPS).
	95
	96	# Hardware interfaces
	97	def _init_gps(self):
	98	if rbconfig.gps == REAL:
	99	from rb.gps import GPS
	100	self.gps = GPS(self)
	101	elif rbconfig.gps == FAKE:
	102	from rb.simulator import FakeGPS
	103	self.gps = FakeGPS(self)
	104
	105	def _init_microstrain(self):
	106	if rbconfig.microstrain == REAL:
	107	from rb.microstrain import MicroStrain
	108	self.microstrain = MicroStrain(self, rbconfig.microstrain_port)
	109	elif rbconfig.microstrain == FAKE:
	110	from rb.simulator import FakeMicroStrain
	111	self.microstrain = FakeMicroStrain(self)
	112
	113	def _init_camera(self):
	114	if rbconfig.camera == REAL:
	115	from rb.vision.gstcam import GstCamera
	116	self.camera = GstCamera(self, rbconfig.camera_gst_input)
	117	elif rbconfig.camera == FAKE:
	118	logger.error("Fake camera TODO")
	119
	120	def _init_drive(self):
	121	if rbconfig.drive == REAL:
	122	from rb.can import CANBus, JaguarDrive
	123	self.can_bus = CANBus(rbconfig.can_bridge_port)
	124	self.drive = JaguarDrive(self.can_bus, mode='voltage')
	125	elif rbconfig.drive == FAKE:
	126	from rb.simulator import FakeDrive
	127	self.drive = FakeDrive(self,
	128	rbconfig.drive_left_port,
	129	rbconfig.drive_right_port)
	130
	131	# Navigator helpers
	132	def _init_kalman(self):
	133	if rbconfig.kalman != NONE:
	134	from rb.kalman import KalmanFilter
	135	self.kalman = KalmanFilter(self)
	136
	137
	138	def _init_transcript(self):
	139	if rbconfig.transcript != NONE:
	140	from rb.transcript import TranscriptLogger
	141	self.transcript = TranscriptLogger(
	142	datetime.now().strftime(rbconfig.transcript_filename),
	143	self.drive, self.gps, self.microstrain)
	144
	145	def _init_drive_ctrl(self):
	146	if rbconfig.drive_ctrl != NONE:
	147	from rb.drive_ctrl import DriveCtrl
	148	self.drive_ctrl = DriveCtrl(self)
	149
	150	# Navigators
	151	def _run_auto(self):
	152	#if rbconfig.auto != NONE:
	153	#from rb.auto import Auto
	154	#self.auto = Auto(self)
	155	# Hardcode this here to allow starting the motor control test code as "auto" from the GUI
	156	self.auto = True
	157
	158	def _run_jaus(self):
	159	if rbconfig.jaus != NONE:
	160	from rb.jaus import JAUS
	161	self.jaus = JAUS(self, rbconfig.jaus_subsystem_id)
	162
	163	def _run_wiimote(self):
	164	if rbconfig.wiimote != NONE:
	165	from rb.wiimote import Wiimote
	166	self.wiimote = Wiimote(self)
	167
	168	def _run_shell(self):
	169	if rbconfig.shell != NONE:
	170	from rb.shell import Shell
	171	self.shell = Shell(self)
	172
	173	def _run_gui(self):
	174	if rbconfig.gui != NONE:
	175	from rb.gui.gui import GUI
	176	self.gui = GUI(self)
	177
	178	##
	179	# Misc Methods
	180	def set_navigator(self, nav):
	181	'''
	182	Set the current navigator, performing any initialization necessary. This
	183	method blocks for up to 30 seconds when first switching to the 'wiimote'
	184	navigator while it connects. Return True on success, else False. Will
	185	return True if `nav` matches the already-set navigator (but won't do any
	186	extra initialization).
	187	'''
	188	if nav == self.navigator:
	189	logger.debug("Tried to set navigator to %r but already was %r", nav, self.navigator)
	190	return True
	191
	192	if nav == 'wiimote':
	193	if not self.wiimote:
	194	logger.error("Wiimote navigator is not loaded; cannot switch to it")
	195	return False
	196	if not self.wiimote.is_connected():
	197	return False
	198
	199	elif nav == 'auto':
	200	if not self.auto:
	201	logger.error("Autonomous navigator is not loaded; cannot switch to it")
	202	return False
	203	start_new_thread(self.test_drive_ctrl, ())
	204
	205	elif nav == 'gui':
	206	if not self.gui:
	207	logger.error("GUI is not loaded; cannot use it as a navigator")
	208	return False
	209
	210	else:
	211	logger.error("Cannot switch to unknown navigator %r", nav)
	212	return False
	213
	214	logger.info("Switching to navigator %r", nav)
	215	self.prev_navigator = self.navigator
	216	self.navigator = nav
	217	self.process('navigator_changed', nav)
	218	return True
	219
	220	def set_prev_navigator(self):
	221	'''
	222	Go to the navigator used before the current one.
	223	'''
	224	self.set_navigator(self.prev_navigator)
	225
	226	##
	227	# Data routing
	228	def quit(self):
	229	'''
	230	Helper method for killing main loop, can be used in a callback
	231	'''
	232	self.running = False
	233
	234	def _main(self):
	235	'''
	236	The controller's GObject mainloop and data processing loop.
	237	'''
	238	# Run a GLib main loop, which doesn't require GTK
	239	# This is a bit complicated, but seems to work well
	240	loop = gobject.MainLoop()
	241
	242	def dataloop(self):
	243	"The controller's main data processing loop."
	244	logger.debug("Entering data processing loop")
	245	self.running = True
	246	while self.running:
	247	try:
	248	type, data = self.data_queue.get(timeout=.1)
	249	self.process(type, data)
	250	except Empty:
	251	pass
	252	except Exception, e:
	253	logger.exception("Error when processing %s data %r", type, data)
	254	logger.debug("Exiting data processing loop")
	255	gobject.idle_add(loop.quit)
	256
	257	start_new_thread(dataloop, (self,))
	258	logger.debug("Entering glib main loop")
	259	loop.run()
	260
	261
	262	def send(self, type, data):
	263	'''
	264	Send the data `data` for processing. `type` is a string
	265	indicating the type of data, such as "gps" or "microstrain".
	266	'''
	267	self.data_queue.put((type, data))
	268
	269	def process(self, type, data):
	270	'''
	271	Process the data `data` of the type `type`. By default
	272	this just updates the Kalman filter. This function is
	273	called inside the controller event loop, so stuff dealing
	274	with GTK+ will need to move things to GTK's main loop.
	275	'''
	276	if type == 'gps_fix':
	277	if self.kalman:
	278	self.kalman.update_gps(data)
	279
	280	elif type == 'microstrain':
	281	if self.kalman:
	282	self.kalman.update_microstrain(data)
	283
	284	elif type == 'drive_targets':
	285	pass
	286
	287	elif type == 'drive_speeds':
	288	if self.kalman:
	289	self.kalman.update_drive_speed(data)
	290
	291
	292	for interface in (self.auto, self.wiimote, self.shell, self.gui, self.jaus):
	293	if interface is not None and hasattr(interface, "process"):
	294	interface.process(type, data)
	295