Changeset 420

Timestamp:

02/20/09 17:44:27 (3 years ago)

Author:

spenceal

Message:

adding fancyier drive code, and support for more motor controller commands

Location:

trunk/software/rb

Files:

• controller.py (modified) (1 diff)
• drive.py (modified) (9 diffs)
• shell.py (modified) (1 diff)

trunk/software/rb/controller.py

@@ -4 +4 @@
 class Controller():
         def __init__(self):
-                self.drive = False
-                self.drive = rb.drive.FakeDrive(rbconfig.left_motor_port,
-                                                rbconfig.right_motor_port)
-                #self.drive = rb.drive.Drive(rbconfig.left_motor_port,
-                #                            rbconfig.right_motor_port)
+                #self.drive = rb.drive.FakeDrive(rbconfig.left_motor_port,
+                #                               rbconfig.right_motor_port)
+                self.drive = rb.drive.Drive(rbconfig.left_motor_port,
+                                            rbconfig.right_motor_port)

trunk/software/rb/drive.py

@@ -14 +14 @@
 # along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
-from __future__ import division, with_statement
+from __future__ import division
 
 '''
@@ -26 +26 @@
 from serial import Serial
 from thread import start_new_thread
-from threading import RLock
+from threading import Lock
 from time import sleep
 
@@ -45 +45 @@
 
 Ok, Err = range(2)
-(Tdisable,  Rdisable,
- Tenable,   Renable,
- Tsetdc,    Rsetdc,
- Tgetdc,    Rgetdc,
- Tsetgain,  Rsetgain,
- Tgetgain,  Rgetgain,
- Tsetspeed, Rsetspeed,
- Tgetspeed, Rgetspeed,
- Tgetstat,  Rgetstat,
- Teewrite,  Reewrite) = range(20)
-
+
+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)                         
+        '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) 
+        '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)                        
+        '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)                                
+}
 
 
@@ -73 +75 @@
                 self.left  = Motor(ldev)
                 self.right = Motor(rdev)
-                self.set_speeds(0.0, 0.0)
+                self.setspeed(0.0, 0.0)
         
         def _get_running(self):
@@ -83 +85 @@
         
         running = property(_get_running, _set_running)
-        
-        def set_speeds(self, left, right):
+
+        def _split(self, type, lt_data, rt_data):
+                '''
+                Call _send on the left and right motor passing in type and the
+                corresponding data. If data is None, it will not be set
+                '''
+                if lt_data == None:
+                        l = None
+                else:
+                        l = self.left._send(type,  *lt_data)
+                if rt_data == None:
+                        r = None
+                else:
+                        r = self.right._send(type,  *rt_data)
+                return (l, r)
+        
+        def setspeed(self, left, right):
                 '''
                 Set the motor speeds directly.  `left` and `right` are floats
@@ -94 +111 @@
                 for each, respectively.
                 '''
-                #print "Drive.set_speeds: %f, %f" % (left, right)
-                self.left.target  = clip(left,  -1.0, 1.0)
-                self.right.target = clip(right, -1.0, 1.0)
+                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
+
+        def getspeed(self):
+                return (float(self.left._send( "getspeed")) / MAX,
+                        float(self.right._send("getspeed")) / MAX)
+
+        def disable(self):     return self._split("disable", (), ())
+        def enable (self):     return self._split("enable" , (), ())
+        def setdc  (self,l,r): return self._split("setdc"  , l,  r )
+        def getdc  (self):     return self._split("getdc"  , (), ())
+        def setgain(self,l,r): return self._split("setgain", l,  r )
+        def getgain(self):     return self._split("getgain", (), ())
+        def getstat(self):     return self._split("getstat", (), ())
+        def eewrite(self):     return self._split("eewrite", (), ())
 
 
@@ -105 +135 @@
         
         def __init__(self, port):
-                self.target = 0
-                self.running = True
-                self.current = 0 # Current value reported by the motors
-                self._current_set = 0 # Current value the motors have been set to
-                self._port = Serial(port, 19200, timeout=.5)
-                self.lock = RLock()
+                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+")
+                self._port_lock = Lock()
                 start_new_thread(self._main, ())
-        
+
         def _main(self):
-                #print "Motor._main: starting"
-                self.running = True
                 while self.running:
-                        #print "Motor._main: stepping"
-                        try:
-                                self.step()
-                        except Exception, e:
-                                log_error('Motor: Exception in step() %r' % e)
+                        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!
                         sleep(.1)
-                self._set_speed(0)
-                #print "Motor._main: exiting"
-        
-        def step(self):
-                with self.lock:
-                        inc = min(0.05, abs(self.target - self._current_set))
-                        if self._current_set < self.target:
-                                self._current_set += inc
-                        elif self._current_set > self.target:
-                                self._current_set -= inc
-                        
-                        self._set_speed(self._current_set)
-                        #~ self.current = (self._get_speed() / MAX)
-        
-        def _send(self, type, msg):
-                #print 'Motor._send: start'
-                self._port.write(chr(type))
-                self._port.write(chr(len(msg)))
-                self._port.write(msg)
-                #print 'Motor._send: end'
-        
-        def _receive(self):
-                type   = self._port.read(1)
-                length = self._port.read(1)
-
-                if len(type)   != 1:
-                        print "Timeout reading `type'   from motor controller"
-                if len(length) != 1:
-                        print "Timeout reading `length' from motor controller"
-
-                if ord(length) == 0:
-                        return ord(type), ''
-                else: 
-                        return ord(type), self._port.read(ord(length))
-        
-        def _set_speed(self, fspeed):
-                with self.lock:
-                        speed = int(fspeed * MAX)
-                        #print 'Motor._set_speed: %d' % speed
-                        self._send(Tsetspeed, struct.pack('>h', speed))
-                        type, res = self._receive()
-                if type != Rsetspeed or ord(res) != Ok:
-                        log_error('motor: Error setting speed')
-        
-        def _set_gain(self, kp, ki, kd):
-                with self.lock:
-                        self._send(Tsetgain, struct.pack('>hhh', kp, ki, kd))
-                        type, res = self._receive()
-                if type != Rsetgain or ord(res) != Ok:
-                        log_error('motor: Error setting gain')
-        
-        def _write_eeprom(self):
-                with self.lock:
-                        self._send(Teewrite, '')
-                        type, res = self._receive()
-                        if type != Reewrite or ord(res) != Ok:
-                                log_error('motor: Error writing settings to eeprom')
-        
-        def _get_speed(self):
-                with self.lock:
-                        self._send(Tgetspeed, '')
-                        type, s = self._receive()
-                        if type != Rgetspeed:
-                                log_error('motor: Tag mismatch')
-                (speed,) = struct.unpack('>h', s)
-                # speed returned is unscaled, this assumes 50Hz loop
-                return speed * 60.0 * 50 / 256.0
+                self._send("setspeed", 0)
+        
+        def _send(self, type, *t_data):
+                self._port_lock.acquire()
+
+                t, r = messages[type]
+
+                # Transmit query
+                t_st = struct.pack(t['fmt'], *t_data)
+                self._port.write(chr(t['op']))
+                self._port.write(chr(len(t_st)))
+                self._port.write(t_st)
+                
+                # Recieve response
+                r_op = self._port.read(1)
+                if len(r_op) != 1:
+                        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"
+
+                r_length = self._port.read(1)
+                if len(r_length) != 1:
+                        print "Motor._send: Timeout reading `r_length' from motor controller"
+                        return
+                r_length = ord(r_length)
+                if r_length != struct.calcsize(r['fmt']):
+                        print "Motor._send: Recieved incorrect `r_length' from motor controller"
+
+                if r_length == 0:
+                        # No need to recieve/unpack structure
+                        return
+                
+                r_st = self._port.read(r_length)
+                if len(r_st) != r_length:
+                        print "Motor._send: Timeout reading `r_data' from motor controller"
+                r_data = struct.unpack(r['fmt'], r_st)
+
+                if (len(r_data) == 1):
+                        # struct's return format is annoying
+                        return r_data(0)
+
+                self._port_lock.release()
+                return r_data
 
 
@@ -202 +209 @@
                 self.right = FakeMotor()
         
-        def set_speeds(self, left, right):
+        def setspeed(self, left, right):
                 log_debug("MockDrive: left  motor speed % 3.03f" % left)
                 log_debug("MockDrive: right motor speed % 3.03f" % right)
@@ -212 +219 @@
         def __init__(self, *args, **kwargs):
                 self.target = 0
-        
-        current = property(lambda s: s.target)
+        def _send(type, *t_data):
+                print "sending %s - %s" % (type, repr(t_data))

trunk/software/rb/shell.py

@@ -114 +114 @@
         
         # Motor control
+        def do_setspeed(self, cmd):
+                try:
+                        lspeed, rspeed = unpack(cmd, (0.0, float), (0.0, float))
+                        self.drive.setspeed(lspeed, rspeed)
+                except ValueError:
+                        print "Invalid parameters"
+
+        def do_getspeed(self, cmd):
+                self.drive.getspeed()
+
         def do_setgain(self, cmd):
                 try:
                         kp, ki, kd, m = unpack(cmd, (0, int), (0, int), (0, int), ('b', str))
-                        if m in ('l', 'b'):
-                                self.drive.left._set_gain(kp, ki, kd)
-                        if m in ('r', 'b'):
-                                self.drive.right._set_gain(kp, ki, kd)
+                        if   m == 'l':
+                                self.drive.setgain((kp,ki,kd),None)
+                        elif m == 'r':
+                                self.drive.setgain(None,(kp,ki,kd))
+                        else:
+                                self.drive.setgain((kp,ki,kd),(kp,ki,kd))
                 except ValueError:
                         print "Invalid Parameters"
-                        
+
         def do_getspeed(self, cmd):
-                try:
-                        (m,) = unpack(cmd, ('b', str))
-                        print 'm = %r' % m
-                        if m in ('l', 'b'):
-                                speed = self.drive.left._get_speed()
-                                print 'l: %d\n' % speed
-                        if m in ('r', 'b'):
-                                speed = self.drive.right._get_speed()
-                                print 'r: %d\n' % speed
-
-                except ValueError:
-                        print "Invalid Parameters"
-
-
-        def do_speeds(self, cmd):
-                try:
-                        lspeed, rspeed = unpack(cmd, (0.0, float), (0.0, float))
-                        self.drive.set_speeds(lspeed, rspeed)
-                except ValueError:
-                        print "Invalid parameters"
+                print self.drive.getspeed()
+
+        def do_setdc(self, cmd):
+                ldc, rdc = unpack(cmd, (0, int), (0, int))
+                print self.drive.setdc(ldc, rdc)
+
+        def do_disable(self, cmd):
+                print self.drive.disable()
+
+        def do_enable(self, cmd):
+                print self.drive.enable()
+
+        def do_getdc(self, cmd):
+                print self.drive.getdc()
+
+        def do_getgain(self, cmd):
+                print self.drive.getgain()
+
+        def do_getstat(self, cmd):
+                print self.drive.getstat()
+
+        def do_eewrite(self, cmd):
+                print self.drive.eewrite()
         
         def do_stop(self, cmd):