Changeset 487

Timestamp:

05/12/09 20:37:14 (3 years ago)

Author:

mosttw

Message:

Added graphing of motor controller pid values

Location:

trunk/software/rb

Files:

• controller.py (modified) (1 diff)
• drive.py (modified) (7 diffs)
• gui/speedgraph.py (modified) (3 diffs)

trunk/software/rb/controller.py

@@ -43 +43 @@
                 self.data_queue = Queue()
                 self.init_drive()
-                self.init_camera()
+                #self.init_camera()
                 #self.init_gps()
                 #self.init_microstrain()

trunk/software/rb/drive.py

@@ -63 +63 @@
 
 
-# TODO:
-#  - Make motors public so they can be accessed directly
-#  - Rename:  TestPlatformDrive -> Drive
-#             legacy2008.py     -> Drive.py
-#  - Remove helper methods from DiffDrive
-
 class Drive(object):
         '''
@@ -78 +72 @@
                 self.controller = controller
                 self.running = True
+                self._stats = ((0, 0, 0), (0, 0, 0))
                 self.left  = Motor(self, ldev)
                 self.right = Motor(self, rdev)
@@ -84 +79 @@
         def _main(self):
                 while self.running:
+                        # Get the current speed
                         lcur, rcur = self._split('getspeed', (), ())
                         if lcur is not None:
@@ -95 +91 @@
                         if lcur is not None and rcur is not None:
                                 self.controller.send('drive_speeds', self.getspeed())
+                        
+                        # Get the PID numbers
+                        try:
+                                (lp, li, ld), (rp, ri, rd) = self._split('getstat', (), ())
+                        except ValueError: # Failed to get so unpacking failed
+                                print "Drive._main: timeout getting stats"
+                        else:
+                                self._stats = ((lp, li, ld), (rp, ri, rd))
+                                self.controller.sent('drive_stats', (lp, li, ld), (rp, ri, rd))
+                        
                         time.sleep(.05)
 
@@ -128 +134 @@
         def getspeed(self):
                 return (self.left.current, self.right.current)
-
+        
+        def getstat(self):
+                return self._stats
+        
         def disable(self):     return self._split("disable", (), ())
         def enable (self):     return self._split("enable" , (), ())
@@ -218 +227 @@
                 self.left = FakeMotor()
                 self.right = FakeMotor()
+                self._stat_counter = 0
                 start_new_thread(self._main, ())
         
@@ -234 +244 @@
         def gettarget(self):
                 return (self.left.target, self.right.target)
+        
+        def getstat(self):
+                s = (self._stat_counter % 0xffff)
+                self._stat_counter += 300
+                return (((s + 1000) % 0xffff - 32767,
+                                  (s + 3000) % 0xffff - 32767,
+                                  (s + 6000) % 0xffff - 32767),
+                                 ((s + 8000) % 0xffff - 32767,
+                                  (s + 10000) % 0xffff - 32767,
+                                  (s + 14000) % 0xffff - 32767))
         
         def getspeed(self):

trunk/software/rb/gui/speedgraph.py

@@ -29 +29 @@
                 super(SpeedGraph, self).__init__()
                 self.drive = drive
-                self.ldata = []
-                self.ltargets = []
-                self.rdata = []
-                self.rtargets = []
+                self.colors = [
+                        ( 0, .5,  0), # Left speeds
+                        ( 0,  1,  0), # Left targets
+                        (.5,  0,  0), # Right speeds
+                        ( 1,  0,  0), # Right targets
+                        ( 0,  0,  0), # Left p
+                        ( 0,  0,  0), # Left i
+                        ( 0,  0,  0), # Left d
+                        ( 0,  0,  0), # Right p
+                        ( 0,  0,  0), # Right i
+                        ( 0,  0,  0), # Right d
+                ]
+                self.data = [[] for color in self.colors]
                 self.connect("expose_event", self.expose)
                 gobject.timeout_add(150, self.graph_tick)
@@ -54 +63 @@
                 context.fill()
                 
-                if len(self.ldata) < DATA_WINDOW:
-                        start_x = a.width * (1 - len(self.ldata) / float(DATA_WINDOW))
+                if len(self.data[0]) < DATA_WINDOW:
+                        start_x = a.width * (1 - len(self.data[0]) / float(DATA_WINDOW))
                 else:
                         start_x = 0
                 step_x = a.width / float(DATA_WINDOW)
-                self.draw_line(context, center_y, start_x, step_x, ( 0, .5,  0), self.ldata[-DATA_WINDOW:])
-                self.draw_line(context, center_y, start_x, step_x, ( 0,  1,  0), self.ltargets[-DATA_WINDOW:])
-                self.draw_line(context, center_y, start_x, step_x, (.5,  0,  0), self.rdata[-DATA_WINDOW:])
-                self.draw_line(context, center_y, start_x, step_x, ( 1,  0,  0), self.rtargets[-DATA_WINDOW:])
+                for color, data in zip(self.colors, self.data):
+                        self.draw_line(context, center_y, start_x, step_x, color, data[-DATA_WINDOW:])
                 
                 return False
@@ -78 +85 @@
         
         def graph_tick(self):
-                ltarget, rtarget = self.drive.gettarget()
-                lspeed, rspeed = self.drive.getspeed()
-                self.ldata.append(lspeed)
-                self.ltargets.append(ltarget)
-                self.rdata.append(rspeed)
-                self.rtargets.append(rtarget)
+                lt, rt = self.drive.gettarget()
+                ls, rs = self.drive.getspeed()
+                lst, rst = self.drive.getstat()
+                new_data = [lt, rt, ls, rs]
+                new_data += [s / 32767.0 for s in (lst + rst)]
+                
+                for datum, list in zip(new_data, self.data):
+                        list.append(datum)
                 self.queue_draw()
                 return True