Changeset 461

Timestamp:

03/15/09 01:43:19 (3 years ago)

Author:

mosttw

Message:

License fixes; hopefully fixed motor speed graphing for real motors

Location:

trunk/software/rb

Files:

• controller.py (modified) (1 diff)
• drive.py (modified) (5 diffs)
• gps.py (modified) (2 diffs)
• gui/analog.py (modified) (2 diffs)
• gui/compass.py (modified) (1 diff)
• gui/gui.py (modified) (11 diffs)
• gui/speedgraph.py (modified) (1 diff)
• kalman.py (modified) (2 diffs)
• microstrain.py (modified) (5 diffs)
• shell.py (modified) (2 diffs)
• wiimote.py (modified) (1 diff)

trunk/software/rb/controller.py

@@ +1 @@
+# Copyright (C) 2009 Thomas W. Most
+# Copyright (C) 2008 Andy Spencer
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
 import gobject
 

trunk/software/rb/drive.py

@@ -1 +1 @@
 # Copyright (C) 2008 Thomas W. Most
+# Copyright (C) 2009 Andy Spencer
 #
 # This program is free software: you can redistribute it and/or modify
@@ -74 +75 @@
         '''
         
-        def __init__(self, ldev, rdev):
-                self.left  = Motor(ldev)
-                self.right = Motor(rdev)
-                #self.setspeed(0.0, 0.0)
-        
-        def _get_running(self):
-                return self.left.running
-        
-        def _set_running(self, value):
-                self.left.running = value
-                self.right.running = value
-        
-        running = property(_get_running, _set_running)
+        def __init__(self, controller, ldev, rdev):
+                self.controller = controller
+                self.running = True
+                self.left  = Motor(self, ldev)
+                self.right = Motor(self, rdev)
+                start_new_thread(self._main, ())
+        
+        def _main(self):
+                while self.running:
+                        lcur, rcur = self._split('getspeed', (), ())
+                        if lcur is not None:
+                                self.left.current = (lcur*50*60 / 256.0) / MAX
+                        else:
+                                print "Drive._main: timeout getting left speed"
+                        if rcur is not None:
+                                self.right.current = (rcur*50*60 / 256.0) / MAX
+                        else:
+                                print "Drive._main: timeout getting right speed"
+                        if lcur is not None and rcur is not None:
+                                self.controller.send('drive_speeds', self.getspeed())
+                        time.sleep(.05)
 
         def _split(self, type, lt_data, rt_data):
@@ -116 +125 @@
                 self.left.target  = clip(left,  -1.0, 1.0) * MAX
                 self.right.target = clip(right, -1.0, 1.0) * MAX
+                self.controller.send('drive_targets', self.gettarget())
         
         def gettarget(self):
@@ -138 +148 @@
         '''
         
-        def __init__(self, port):
-                self._port_name = port
-                self.running    = True
+        def __init__(self, drive, port):
+                self.drive = drive
                 self.target     = 0 # Speed the motor controllers should be at
                 self.current    = 0 # Latest reported speed of the motors
+                self._port_name = port
+                self._port_lock = Lock()
                 self._port      = Serial(port, 19200, timeout=.5)
                 time.sleep(1)
-                self._port_lock = Lock()
                 start_new_thread(self._main, ())
 
         def _main(self):
                 print "Motor._main: starting"
-                while self.running:
-                        #print "Motor._main: stepping"
-                        cur = self._send("getspeed")
-                        if cur is not None:
-                                #print "updating speed, cur=%d target=%d" % (cur, self.target)
-                                #inc = min(0.05*MAX, abs(self.target - cur))
-                                #if cur < self.target:
-                                #       self._send("setspeed", cur + inc)
-                                #elif cur > self.target:
-                                #       self._send("setspeed", cur - inc)
-                                #else:
-                                self.current = (cur*50*60 / 256.0) / MAX
-                                self._send("setspeed", self.target) # Ping!
-                        else:
-                                print "Motor._main: timeout getting speed"
+                while self.drive.running:
+                        self._send("setspeed", self.target) # Ping!
                         time.sleep(.05)
                 print "Motor._main: exiting"
@@ -257 +254 @@
                 print "FakeMotor sending %s - %s" % (type, repr(t_data))
         
-        current = property(lambda s: s.target)
+        current = property(lambda s: s.target, lambda s: None)

trunk/software/rb/gps.py

@@ -81 +81 @@
         '''
         
-        def __init__(self, dev):
+        def __init__(self, controller, dev):
                 '''
                 Create a new `GPS` object.  `dev` is the device to start gpsd
                 listening on.
                 '''
-                self.latest_coords = None
+                self.controller = controller
                 self.dev = dev
                 self.running = True
+                self.latest_coords = None
                 self.gpsd = None
                 self.gps = None
-                super(GPS, self).__init__()
+                
                 thread.start_new_thread(self._run_gpsd, ())
                 thread.start_new_thread(self._main, ())
@@ -164 +165 @@
                                                         log_debug("GPS: got update")
                                                         self.latest_coords = GPSCoord(*data)
+                                                        self.controller.send('gps_fix', self.latest_coords)
                                                         last_data = data
 
-
-def main():
-        '''
-        Print constant updates from the GPS.
-        '''
-        logging.log_level = logging.DEBUG
-        gps = GPS(sys.argv[1])
-        last_data = None
-        try:
-                while True:
-                        data = gps.latest_coords
-                        if data is not last_data:
-                                last_data = data
-                                print data.pformat()
-        except KeyboardInterrupt:
-                print "Shutting down, just a sec"
-                gps.running = False
-
-if __name__ == '__main__':
-        main()

trunk/software/rb/gui/analog.py

@@ +1 @@
+# Copyright (C) 2008-2009 Andy Spencer
+# Copyright (C) 2008-2009 Thomas W. Most
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
 from math import pi
 import gtk
@@ -77 +93 @@
                 l = x+y
                 r = y-x
-
-                #log_debug("\tgui: x=%f, y=%f, l=%f, r=%f" % (x,y,l,r))
-                self.controller.set_speed(l, r)
+                self.controller.drive.setspeed(l, r)

trunk/software/rb/gui/compass.py

@@ -1 +1 @@
 #!/usr/bin/env python
 
-# Copyright (C) 2008 Nathan T. Mendel, Ching-Chen Ma, Thomas W. Most
+# Copyright (C) 2008 Nathan T. Mendel
+# Copyright (C) 2008 Ching-Chen Ma
+# Copyright (C) 2009 Thomas W. Most
 #
 # This program is free software: you can redistribute it and/or modify

trunk/software/rb/gui/gui.py

@@ +1 @@
+# Copyright (C) 2008-2009 Andy Spencer
+# Copyright (C) 2008-2009 Thomas W. Most
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+from __future__ import division
+
 import gobject
 import pygtk; pygtk.require("2.0")
@@ -4 +22 @@
 
 import os.path
+from math import pi
 from thread import start_new_thread
 
@@ -27 +46 @@
 }
 
+# Blue color for indicating the robot's orientation
+RAW_ORIENT_BLUE = (85/255, 107/255, 160/255) # Raw MicroStrain data
+ORIENT_BLUE = (6/255, 67/255, 213/255) # Kalman filter estimation
+
 
 class GUI(Controller):
@@ -55 +78 @@
                 self.builder.add_from_file(GTKBUILDER_FILE)
                 self.builder.connect_signals(self)
-                self.window  = self.builder.get_object('window')
+                self.window = self.builder.get_object('window')
 
                 # Glade-3 -> GtkBuilder fix
@@ -65 +88 @@
                 self.analog = AnalogWidget(self)
                 self.compass = CompassWidget()
+                self.raw_orient_dir = self.compass.new_direction(RAW_ORIENT_BLUE, pi / 2)
                 self.speed_graph = SpeedGraph(self.drive)
                 self.builder.get_object("analog_body").add(self.analog)
@@ -79 +103 @@
                 Shell(self).cmdloop()
                 gobject.idle_add(gtk.main_quit)
+        
+        def update_compass_raw_orient(self, state_accel):
+                self.raw_orient_dir.update(state_accel.orient[2])
         
         def process(self, type, data):
@@ -86 +113 @@
                 elif type == 'drive_speeds':
                         self.analog.queue_draw()
+                elif type == 'microstrain':
+                        self.update_compass_orient(data)
         
         def set_navigator(self, nav):
@@ -105 +134 @@
                         if nav == 'stop':
                                 print "Switching to 'stop' navigator"
-                                self.set_speed(0, 0)
+                                self.drive.setspeed(0, 0)
                         elif nav == 'analog':
                                 print "Switching to analog navigator"
@@ -141 +170 @@
                 # E-Stop 
                 if event.keyval in (gtk.keysyms.Escape, gtk.keysyms.space):
-                        self.set_mode('stop')
+                        self.set_navigator('stop')
 
                 # Switch tabs
@@ -150 +179 @@
                 elif event.keyval in self.arrow_keys_map:
                         lo, ro = self.arrow_keys_map[event.keyval]
-                        self.set_speed(self.drive.left.target+lo,
-                                                        self.drive.right.target+ro)
+                        self.drive.setspeed(self.drive.left.target+lo,
+                                                                self.drive.right.target+ro)
 
                 # Key not found
@@ -205 +234 @@
                 self.drive.eewrite()
 
-        # utilities
-        def set_speed(self, lspeed, rspeed):
-                # Constrain l and r
-                self.drive.setspeed(lspeed, rspeed)
-                self.analog.redraw(None)
-
         # GTK Callbacks
         def on_window_destroy(self, widget, data=None):

trunk/software/rb/gui/speedgraph.py

@@ -1 @@
-# Copyright (C) 2008 Thomas W. Most
+# Copyright (C) 2009 Thomas W. Most
 #
 # This program is free software: you can redistribute it and/or modify

trunk/software/rb/kalman.py

@@ +1 @@
+# Copyright (C) 2009 Thomas W. Most
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
 
 from thread import start_new_thread
+
+from rb.utils import meter_offset
 
 
@@ -46 +63 @@
                         # Use the robot's initial location as zero
                         self.gps_zero = coords.lat_lon
+                x, y = meter_offset(self.gps_zero, coords.lat_lon)
                 
-                # Convert GPS coordinates to meters
-                # FIXME!
+                self._x_y = (x, y)
         
         def update_drive_speed(self, (lspeed, rspeed)):

trunk/software/rb/microstrain.py

@@ -1 @@
-# Copyright (C) 2008 Thomas W. Most
+# Copyright (C) 2008-2009 Thomas W. Most
 # Copyright (C) 2008 Andy Spencer
 #
@@ -90 +90 @@
         '''
         
-        def __init__(self, dev):
+        def __init__(self, controller, dev):
                 '''
                 Create a new `MicroStrain` object.  `dev` is the serial device
                 on which the MicroStrain is present.
                 '''
-                
+                self.controller = controller
                 self.running = True
                 self.dev = dev
@@ -129 +129 @@
                                         elif isinstance(result, StateAccel):
                                                 self.latest_state = result
-                                                self.smoothed_state = self.calc_smoothed(result)
+                                                self.controller.send('microstrain', result)
                                 
                                 port.write("\x10\x00\x00") # Stop continuous mode
@@ -135 +135 @@
                                 log_error("MicroStrain: disconnetion: %s" % e)
                                 time.sleep(1) # The cable was probably pulled.  Wait for it.
-        
-        def calc_smoothed(self, new_state):
-                self._window.append(new_state)
-                if len(self._window) > WINDOW_SIZE:
-                        self._window = self._window[len(self._window) - WINDOW_SIZE:WINDOW_SIZE]
-                num = len(self._window)
-                return StateAccel(
-                        _smooth_triple(self._window, 'orient', num),
-                        _smooth_triple(self._window, 'ang_vel', num),
-                        _smooth_triple(self._window, 'accel', num)
-                )
-
-
-def _smooth_triple(window, attr, num):
-        '''
-        Return a three-tuple of the average of each dataset
-        `attr` in `window`, a list of StateAccel objects.
-        '''
-        # TODO: Probably should do a weighted average
-        # and/or more filtering.
-        triples = [getattr(sa, attr) for sa in window]
-        return (
-                sum(t[0] for t in triples) / num,
-                sum(t[1] for t in triples) / num,
-                sum(t[2] for t in triples) / num
-        )
+
+
 
 def handle_packet(port):
@@ -268 +244 @@
         0x25: (11, None),
 }
-
-
-def main():
-        '''
-        Print MicroStrain data to the screen.
-        '''
-        logging.log_level = logging.DEBUG
-        ms = MicroStrain(sys.argv[1])
-        last_state = None
-        try:
-                while True:
-                        state = ms.smoothed_state
-                        if state is not last_state:
-                                print state.pformat()
-                                last_state = state
-                        time.sleep(.01)
-        except KeyboardInterrupt:
-                ms.running = False
-
-if __name__ == '__main__':
-        main()

trunk/software/rb/shell.py

@@ -1 @@
-# Copyright (C) 2008 Thomas W. Most
+# Copyright (C) 2008-2009 Thomas W. Most
+# Copyright (C) 2009 Michael Auchter
+# Copyright (C) 2009 Andy Spencer
 #
 # This program is free software: you can redistribute it and/or modify
@@ -160 +162 @@
         
         def do_stop(self, cmd):
-                self.controller.drive.set_speeds(0.0, 0.0)
+                self.controller.drive.setspeed(0.0, 0.0)
         
         def help_stop(self):

trunk/software/rb/wiimote.py

@@ -113 +113 @@
                 rspeed = y / 127.0 if (abs(y) > 5) else 0
 
-                self.controller.set_speed(lspeed, rspeed)
+                self.controller.drive.setspeed(lspeed, rspeed)
 
         def handle_mesg(self, messages):