Changeset 422

Timestamp:

02/20/09 19:11:05 (3 years ago)

Author:

spenceal

Message:

fixing bugs in the python motor code

Location:

trunk/software/rb

Files:

• drive.py (modified) (7 diffs)
• gui/analog.py (modified) (1 diff)
• gui/gui.py (modified) (1 diff)

trunk/software/rb/drive.py

@@ -47 +47 @@
 
 messages = {
-        'disable' : [{'op':0 , 'fmt':''    }, {'op':1 , 'fmt':''    }], # (nil)                                
-        'enable'  : [{'op':2 , 'fmt':''    }, {'op':3 , 'fmt':''    }], # (nil)                                
-        'setdc'   : [{'op':4 , 'fmt':'>h'  }, {'op':5 , 'fmt':''    }], # (int16_t dc)                         
+        'disable' : [{'op':0 , 'fmt':''    }, {'op':1 , 'fmt':'>B'  }], # (nil)                                
+        'enable'  : [{'op':2 , 'fmt':''    }, {'op':3 , 'fmt':'>B'  }], # (nil)                                
+        'setdc'   : [{'op':4 , 'fmt':'>h'  }, {'op':5 , 'fmt':'>B'  }], # (int16_t dc)                         
         'getdc'   : [{'op':6 , 'fmt':''    }, {'op':7 , 'fmt':'>h'  }], # (int16_t dc)                         
-        'setgain' : [{'op':8 , 'fmt':'>hhh'}, {'op':9 , 'fmt':''    }], # (int16_t Kp, int16_t Ki, int16_t Kd) 
+        'setgain' : [{'op':8 , 'fmt':'>hhh'}, {'op':9 , 'fmt':'>B'  }], # (int16_t Kp, int16_t Ki, int16_t Kd) 
         'getgain' : [{'op':10, 'fmt':''    }, {'op':11, 'fmt':'>hhh'}], # (int16_t Kp, int16_t Ki, int16_t Kd) 
-        'setspeed': [{'op':12, 'fmt':'>h'  }, {'op':13, 'fmt':''    }], # (int16_t rpm)                        
+        'setspeed': [{'op':12, 'fmt':'>h'  }, {'op':13, 'fmt':'>B'  }], # (int16_t rpm)                        
         'getspeed': [{'op':14, 'fmt':''    }, {'op':15, 'fmt':'>h'  }], # (int16_t speed)                       
         'getstat' : [{'op':16, 'fmt':''    }, {'op':17, 'fmt':'>hhh'}], # (int16_t p, int16_t i, int16_t d)    
-        'eewrite' : [{'op':18, 'fmt':''    }, {'op':19, 'fmt':''    }], # (nil)                                
+        'eewrite' : [{'op':18, 'fmt':''    }, {'op':19, 'fmt':'>B'  }], # (nil)                                
 }
 
@@ -75 +75 @@
                 self.left  = Motor(ldev)
                 self.right = Motor(rdev)
-                self.setspeed(0.0, 0.0)
+                #self.setspeed(0.0, 0.0)
         
         def _get_running(self):
@@ -111 +111 @@
                 for each, respectively.
                 '''
-                print "Drive.setspeed: %f, %f" % (left, right)
+                #print "Drive.setspeed: %f, %f" % (left, right)
                 self.left.target  = clip(left,  -1.0, 1.0)*MAX
                 self.right.target = clip(right, -1.0, 1.0)*MAX
@@ -135 +135 @@
         
         def __init__(self, port):
+                self._port_name = port
                 self.running    = True
                 self.target     = 0 # Speed the motor coltrollers should be at
                 self._port      = Serial(port, 19200, timeout=.5)
-                #self._port      = open("/tmp/fake", "w+")
+                sleep(1)
+                #print "wrote: ", self._port.write(chr(14) + chr(0))
+                #print "read: ", len(self._port.read(4))
                 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:
-                                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._send("setspeed", cur) # Ping!
+                        if cur != 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._send("setspeed", self.target) # Ping!
+                        else:
+                                print "Motor._main: timeout getting speed"
                         sleep(.1)
+                print "Motor._main: exiting"
                 self._send("setspeed", 0)
         
@@ -163 +172 @@
                 # Transmit query
                 t_st = struct.pack(t['fmt'], *t_data)
-                self._port.write(chr(t['op']))
+                #print "Motor(%s)._send: %s (%d) - %d bytes" % \
+                #       (self._port_name, type, t['op'], len(t_st))
+                ln = self._port.write(chr(t['op']))
                 self._port.write(chr(len(t_st)))
                 self._port.write(t_st)
@@ -172 +183 @@
                         print "Motor._send: Timeout reading `r_type' from motor controller"
                 elif r_op != chr(r['op']):
-                        print "Motor._send: Recieved incorrect `r_type' from motor controller"
+                        print "Motor._send: Recieved incorrect `r_type' (%d) from motor controller" % ord(r_op)
 
                 r_length = self._port.read(1)
                 if len(r_length) != 1:
                         print "Motor._send: Timeout reading `r_length' from motor controller"
+                        self._port_lock.release()
                         return
                 r_length = ord(r_length)
                 if r_length != struct.calcsize(r['fmt']):
-                        print "Motor._send: Recieved incorrect `r_length' from motor controller"
+                        print "Motor._send: Recieved incorrect `r_length' (%d) from motor controller" % r_length
 
                 if r_length == 0:
                         # No need to recieve/unpack structure
+                        self._port_lock.release()
                         return
+
                 
                 r_st = self._port.read(r_length)
@@ -193 +207 @@
                 if (len(r_data) == 1):
                         # struct's return format is annoying
-                        return r_data(0)
+                        self._port_lock.release()
+                        return r_data[0]
 
                 self._port_lock.release()

trunk/software/rb/gui/analog.py

@@ -57 +57 @@
 
                 # Draw Current Speed
-                x = float(self.controller.drive.left.current - self.controller.drive.right.current)
-                y = float(self.controller.drive.left.current + self.controller.drive.right.current)
+                lcur, rcur = self.controller.drive.getspeed()
+                x = float(lcur - rcur)
+                y = float(lcur + rcur)
                 x =  ( x/4+0.5) * w
                 y =  (-y/4+0.5) * h

trunk/software/rb/gui/gui.py

@@ -80 +80 @@
         def set_speed(self, lspeed, rspeed):
                 # Constrain l and r
-                self.drive.set_speeds(lspeed, rspeed)
+                self.drive.setspeed(lspeed, rspeed)
                 self.analog.redraw(None)