Rose-Hulman Robotics Team

Changeset 458

Show
Ignore:
Timestamp:
03/14/09 16:57:27 (3 years ago)
Author:
mosttw
Message:

Fixed graph issues

Location:
trunk/software
Files:
1 added
8 modified

Legend:

Unmodified
Added
Removed

  • trunk/software/rb/auto.py

    r445 r458  
    1818''' 
    1919 
    20 import sys 
    2120import time 
    22 import itertools 
    2321from math import pi 
    24  
    2522 
    2623from rb.core.logging import log_debug, log_info, log_warn, log_error, log_die 
    2724from rb.utils import points_to_vec, rad_delta 
    28 from rb.gps import GPS 
    29 from rb.microstrain import MicroStrain 
    30 from rb.drive import Drive, FakeDrive 
    31 import rbconfig 
     25 
    3226 
    3327 
    … …  
    4034 
    4135 
    42  
    4336class PointNavigator(object): 
    4437        ''' 
    4538        Implements the navigation algorithm of heading directly toward 
    46         a point. 
     39        a GPS coordinate. 
    4740        ''' 
    4841         
    49         def __init__(self): 
    50                 ''' 
    51                 Create a new `PointNavigator`. 
    52                 ''' 
    53                 self.running = True 
    54                 # Whether the robot is under manual Wiimote control or  
    55                 # is autonomous. 
    56                 self.autonomous = False 
    57                 self.led = LED4_ON 
     42        def __init__(self, controller): 
     43                self.controller = controller 
    5844                 
    59                 # Hardware 
    60                 self.gps = None 
    61                 self.microstrain = None 
    62                 self.drive = None 
    63                 self.wiimote = None 
    64                  
    65                 # Data structures 
    66                 self.accel = None # Microstrain data 
    67                 self.coords = None # GPS data 
    68                 self.stick = STICK_CENTER # Wiimote's stick 
    6945         
    70         def run(self): 
    71                 self.init_sensors() 
    72                 self.init_output() 
    73                 self.init_wiimote() 
    74                 self.set_leds() 
    75                 # Get a function that returns False every 5 calls 
    76                 skip_led = itertools.cycle(range(5)).next 
    77                 try: 
    78                         while self.running: 
    79                                 if self.autonomous: 
    80                                         if self.get_inputs(): 
    81                                                 self.set_outputs() 
    82                                 else: 
    83                                         self.wiimote_control() 
    84                                 time.sleep(.1) 
    85                                 if not skip_led(): 
    86                                         self.set_leds() 
    87                 finally: 
    88                         log_info("auto: Halting") 
    89                         self.stop_output() 
    90                         self.stop_sensors() 
    91                         self.stop_wiimote() 
    92          
    93         def stop(self): 
    94                 self.running = False 
    95          
    96         def init_sensors(self): 
    97                 self.gps = GPS(rbconfig.gps_port) 
    98                 self.microstrain = MicroStrain(rbconfig.microstrain_port) 
    99          
    100         def stop_sensors(self): 
    101                 self.gps.running = False 
    102                 self.microstrain.running = False 
    103  
    104         def init_output(self): 
    105                 self.drive = Drive(rbconfig.left_motor_port, rbconfig.right_motor_port) 
    106          
    107         def stop_output(self): 
    108                 self.drive.set_speeds(0, 0) 
    109                 time.sleep(.5) 
    110                 self.drive.running = False 
    111          
    112          
    113          
    114         def get_inputs(self): 
    115                 ''' 
    116                 Get input from the sensors, placing it in the `current_data` 
    117                 attribute.  Return True if anything has changed. 
    118                 ''' 
    119                 old = (self.accel, self.coords) 
    120                 self.accel = self.microstrain.latest_state 
    121                 self.coords = self.gps.latest_coords 
    122                 if self.accel is not None and (time.time() - self.accel.timestamp) > 1: 
    123                         self.accel = None 
    124                 if self.coords is not None and (time.time() - self.coords.timestamp) > 5: 
    125                         self.coords = None 
    126                 return old != (self.accel, self.coords) 
    127          
    128         def set_outputs(self): 
     46        def _main(self): 
    12947                ''' 
    13048                Update the output based on `current_data`. 
    … …  
    13250                SPEED = .75 
    13351                if self.accel is not None: 
    134                         # The MicroStrain things that North is zero radians and that 
    135                         # radians increase clockwise.  Change it so that 0 rad is east 
    136                         # and radians increase counter-clockwise. 
    137                         facing_dir = (((2 * pi) - self.accel.orient[2]) + (pi / 2)) % (2 * pi) 
    13852                        log_info('Facing: %6.02f' % facing_dir) 
    13953                else: 
    … …  
    17690                                time.sleep(1) 
    17791                                self.wiimote.rumble = False 
    178          
    179  
    180  
    181 if __name__ == '__main__': 
    182         pn = PointNavigator() 
    183         pn.run() 

  • trunk/software/rb/controller.py

    r445 r458  
     1import gobject 
     2 
    13import rbconfig 
    24import rb.drive 
    … …  
    46import rb.microstrain 
    57import rb.wiimote 
     8import rb.kalman 
    69 
    710class Controller(object): 
     11        ''' 
     12        A controller forms the core logic of the robot.  It is responsible for 
     13        initializing hardware and routing sensor data everywhere it must go. 
     14        ''' 
     15         
    816        def __init__(self): 
    917                self.navigator = None 
    … …  
    1220                self.microstrain = None 
    1321                self.wiimote = None 
     22                self.kalman = rb.kalman.KalmanFilter(self) 
    1423         
    1524        def set_navigator(self, nav): 
    … …  
    2332        def init_drive(self): 
    2433                if rbconfig.fake_drive: 
    25                         self.drive = rb.drive.FakeDrive(rbconfig.left_motor_port, 
     34                        self.drive = rb.drive.FakeDrive(self, 
     35                                                                                        rbconfig.left_motor_port, 
    2636                                                                                        rbconfig.right_motor_port) 
    2737                else: 
    28                         self.drive = rb.drive.Drive(rbconfig.left_motor_port, 
     38                        self.drive = rb.drive.Drive(self, 
     39                                                                                rbconfig.left_motor_port, 
    2940                                                                                rbconfig.right_motor_port) 
    3041         
    … …  
    3748        def init_wiimote(self): 
    3849                self.wiimote = rb.wiimote.Wiimote(rbconfig.wiimote_addr, self) 
     50         
     51         
     52        ## 
     53        # Data routing 
     54         
     55        def send(self, type, data): 
     56                ''' 
     57                Send the data `data` for processing. `type` is a string 
     58                indicating the type of data, such as "gps" or "microstrain". 
     59                ''' 
     60                # Use gobject.idle_add to call `process()` in the main 
     61                # thread. 
     62                gobject.idle_add(self.process, type, data) 
     63         
     64        def process(self, type, data): 
     65                ''' 
     66                Process the data `data` of the type `type`.  By default 
     67                this just updates the Kalman filter. 
     68                ''' 
     69                if type == 'gps_fix': 
     70                        self.kalman.update_gps(data) 
     71                elif type == 'microstrain': 
     72                        self.kalman.update_microstrain(data) 
     73                elif type == 'drive_targets': 
     74                        pass 
     75                elif type == 'drive_speeds': 
     76                        self.kalman.update_drive_speed(data) 

  • trunk/software/rb/drive.py

    r454 r458  
    2424 
    2525import struct 
     26import time 
     27import random 
    2628from serial import Serial 
    2729from thread import start_new_thread 
    2830from threading import Lock 
    29 from time import sleep 
     31from math import sin, cos 
    3032 
    3133from rb.core.logging import log_error, log_debug 
    … …  
    142144                self.current    = 0 # Latest reported speed of the motors 
    143145                self._port      = Serial(port, 19200, timeout=.5) 
    144                 sleep(1) 
     146                time.sleep(1) 
    145147                self._port_lock = Lock() 
    146148                start_new_thread(self._main, ()) 
    … …  
    163165                        else: 
    164166                                print "Motor._main: timeout getting speed" 
    165                         sleep(.05) 
     167                        time.sleep(.05) 
    166168                print "Motor._main: exiting" 
    167169                self._send("setspeed", 0) 
    … …  
    216218        ''' 
    217219         
    218         def __init__(self, *args, **kwargs): 
    219                 log_debug("MockDrive: __init__(*%s, **%s)" % (args, kwargs)) 
     220        def __init__(self, controller, *args, **kwargs): 
     221                log_debug("MockDrive: __init__(%s, *%s, **%s)" % (controller, args, kwargs)) 
     222                self.controller = controller 
     223                self.running = True 
    220224                self.left = FakeMotor() 
    221225                self.right = FakeMotor() 
     226                start_new_thread(self._main, ()) 
     227         
     228        def _main(self): 
     229                while self.running: 
     230                        self.controller.send('drive_speeds', self.getspeed()) 
     231                        time.sleep(.05) 
    222232         
    223233        def setspeed(self, left, right): 
    … …  
    226236                self.left.target = clip(left,  -1.0, 1.0) 
    227237                self.right.target = clip(right,  -1.0, 1.0) 
     238                self.controller.send('drive_targets', (self.left.target, self.right.target)) 
     239         
     240        def gettarget(self): 
     241                return (self.left.target, self.right.target) 
    228242         
    229243        def getspeed(self): 
    230                 return (self.left.target, self.right.target) 
     244                v1 = sin(time.time()) * .05 - random.random() * .01 
     245                v2 = cos(time.time()) * .05 - random.random() * .01 
     246                return (self.left.target + v1, self.right.target + v2) 
    231247 
    232248 

  • trunk/software/rb/gui/analog.py

    r454 r458  
    1414                self.add_events(gtk.gdk.BUTTON_PRESS_MASK) 
    1515                self.add_events(gtk.gdk.POINTER_MOTION_MASK) 
    16                 self.connect("expose-event",        self.expose_cb) 
     16                self.connect("expose-event", self.expose_cb) 
    1717                self.connect("motion-notify-event", self.motion_cb) 
    1818 

  • trunk/software/rb/gui/gui.py

    r454 r458  
    6767                self.builder.get_object("compass_body").add(self.compass) 
    6868                self.builder.get_object("speed_graph").add(self.speed_graph) 
    69  
     69                 
    7070                self.window.show_all() 
    7171                self.set_navigator('analog') 
    7272                 
    7373                gtk.main() 
    74  
    75         def wiimote_connect(self): 
    76                 sb = self.builder.get_object('statusbar') 
    77                 context_id = sb.get_context_id('wiimote_connect') 
    78                 # FIXME: Make the first message show up before we do the blocking start() call 
    79                 msg = 'Connecting to the Wiimote at %s' % rbconfig.wiimote_addr 
    80                 print msg 
    81                 sb.push(context_id, msg) 
    82                 try: 
    83                         self.wiimote.start() 
    84                 except IOError: 
    85                         success = False 
    86                         msg = 'Wiimote connection failed' 
    87                 else: 
    88                         success = True 
    89                         msg = 'Connected to the Wiimote at %s' % rbconfig.wiimote_addr 
    90                 sb.pop(context_id) 
    91                 message_id = sb.push(context_id, msg) 
    92                 gobject.timeout_add(3000, lambda: sb.remove(context_id, message_id)) 
    93                 return success 
    94  
     74         
     75        def process(self, type, data): 
     76                Controller.process(self, type, data) 
     77                if type == 'drive_targets': 
     78                        self.analog.queue_draw() 
     79                elif type == 'drive_speeds': 
     80                        self.analog.queue_draw() 
     81         
    9582        def set_navigator(self, nav): 
    9683                ''' 
    … …  
    121108                        self.update_nav_toggles() 
    122109 
     110        def wiimote_connect(self): 
     111                sb = self.builder.get_object('statusbar') 
     112                context_id = sb.get_context_id('wiimote_connect') 
     113                # FIXME: Make the first message show up before we do the blocking start() call 
     114                msg = 'Connecting to the Wiimote at %s' % rbconfig.wiimote_addr 
     115                print msg 
     116                sb.push(context_id, msg) 
     117                try: 
     118                        self.wiimote.start() 
     119                except IOError: 
     120                        success = False 
     121                        msg = 'Wiimote connection failed' 
     122                else: 
     123                        success = True 
     124                        msg = 'Connected to the Wiimote at %s' % rbconfig.wiimote_addr 
     125                sb.pop(context_id) 
     126                message_id = sb.push(context_id, msg) 
     127                gobject.timeout_add(3000, lambda: sb.remove(context_id, message_id)) 
     128                return success 
     129 
    123130        def key_press_cb(self, widget, event): 
    124131                nb = self.builder.get_object("notebook") 
    … …  
    131138                # Switch tabs 
    132139                elif event.keyval == gtk.keysyms.Tab: 
    133                         nb.set_current_page(not tab) # FIXME: make this robust 
     140                        nb.set_current_page((tab + 1) % nb.get_n_pages()) 
    134141 
    135142                # Set motor speeds 

  • trunk/software/rb/gui/speedgraph.py

    r454 r458  
    5050                center_y = a.height / 2.0 
    5151                 
     52                context.rectangle(0, 0, a.width, a.height) 
     53                context.set_source_rgb(1, 1, 1) 
     54                context.fill() 
     55                 
    5256                if len(self.ldata) < DATA_WINDOW: 
    5357                        start_x = a.width * (1 - len(self.ldata) / float(DATA_WINDOW)) 
    … …  
    5559                        start_x = 0 
    5660                step_x = a.width / float(DATA_WINDOW) 
    57                 self.draw_line(context, center_y, start_x, step_x, (0, 100, 0), self.ldata[-DATA_WINDOW:]) 
    58                 self.draw_line(context, center_y, start_x, step_x, (0, 200, 0), self.ltargets[-DATA_WINDOW:]) 
    59                 self.draw_line(context, center_y, start_x, step_x, (100, 0, 0), self.rdata[-DATA_WINDOW:]) 
    60                 self.draw_line(context, center_y, start_x, step_x, (200, 0, 0), self.rtargets[-DATA_WINDOW:]) 
     61                self.draw_line(context, center_y, start_x, step_x, ( 0, .5, 0), self.ldata[-DATA_WINDOW:]) 
     62                self.draw_line(context, center_y, start_x, step_x, ( 0,  1, 0), self.ltargets[-DATA_WINDOW:]) 
     63                self.draw_line(context, center_y, start_x, step_x, (.5,  0, 0), self.rdata[-DATA_WINDOW:]) 
     64                self.draw_line(context, center_y, start_x, step_x, ( 1,  0, 0), self.rtargets[-DATA_WINDOW:]) 
    6165                 
    6266                return False 

  • trunk/software/rb/shell.py

    r454 r458  
    4949                } 
    5050                self.init_drive() 
     51                 
    5152                try: 
    5253                        sessfile = os.environ['RB_SHELL_AUTORUN'] 

  • trunk/software/rbconfig.py

    r454 r458  
    1515 
    1616# Serial port for the motor controller 
    17 fake_drive = False 
     17fake_drive = True 
    1818left_motor_port  = '/dev/ttyUSB0' 
    1919right_motor_port = '/dev/ttyUSB1'