Changeset 3


Ignore:
Timestamp:
01/27/10 18:23:04 (11 years ago)
Author:
sc
Message:

brainstorms_rc:

  • driving functions added
File:
1 edited

Legend:

Unmodified
Added
Removed
  • rc/puzzlebox_brainstorms_rc.py

    r2 r3  
    1111# For more information please refer to http://www.gnu.org/copyleft/gpl.html 
    1212# 
    13 # Last Update: 2010.01.25 
     13# Last Update: 2010.01.27 
    1414# 
    1515##################################################################### 
     
    1919import jaraco.nxt 
    2020import jaraco.nxt.messages 
    21 import jaraco.nxt.routine 
    2221 
    2322##################################################################### 
     
    2827 
    2928BLUETOOTH_DEVICE='/dev/rfcomm0' 
     29MOTORS_MOUNTED_BACKWARDS = True 
     30MOTOR_PORT_RIGHT = 'a' 
     31MOTOR_PORT_LEFT = 'b' 
    3032 
    3133##################################################################### 
     
    3941                self.DEBUG = DEBUG 
    4042                 
    41                 self.device=device 
     43                self.device = device 
     44                 
     45                self.connection = None 
    4246         
    4347         
     
    4650        def connect_to_nxt(self, device): 
    4751                 
    48                 conn = jaraco.nxt.Connection(self.device) 
     52                connection = jaraco.nxt.Connection(self.device) 
    4953                 
    5054                if self.DEBUG > 1: 
    51                         battery_voltage = jaraco.nxt.routine.get_voltage(conn) 
     55                        battery_voltage = self.get_battery_voltage(connection) 
    5256                        print "Battery voltage:", 
    5357                        print battery_voltage 
    5458                 
    5559                 
    56                 return(conn) 
    57          
    58          
    59         ################################################################## 
    60          
    61         def cycle_motor(self, conn, port): 
     60                return(connection) 
     61         
     62         
     63        ################################################################## 
     64         
     65        def connect_to_port(self, device_port): 
     66         
     67                if isinstance(device_port, basestring): 
     68                        port = getattr(jaraco.nxt.messages.OutputPort, device_port) 
     69                 
     70                assert port in jaraco.nxt.messages.OutputPort.values() 
     71                 
     72                return(port) 
     73         
     74         
     75        ################################################################## 
     76         
     77        def get_battery_voltage(self, connection): 
     78                 
     79                cmd = jaraco.nxt.messages.GetBatteryLevel() 
     80                connection.send(cmd) 
     81                 
     82                response = connection.receive() 
     83                voltage = response.get_voltage() 
     84                 
     85                return(voltage) 
     86         
     87         
     88        ################################################################## 
     89         
     90        def cycle_motor(self, connection, motor_port='a'): 
    6291                 
    6392                "Turn the motor one direction, then the other, then stop it" 
    6493                 
    65                 port = jaraco.nxt.routine.get_port(port, \ 
    66                                                    jaraco.nxt.messages.OutputPort) 
     94                port = self.connect_to_port(motor_port) 
    6795                cmd = jaraco.nxt.messages.SetOutputState(port, \ 
    6896                                                         motor_on=True, \ 
    6997                                                         set_power=60, \ 
    7098                                   run_state=jaraco.nxt.messages.RunState.running) 
    71                 conn.send(cmd) 
     99                connection.send(cmd) 
    72100                 
    73101                time.sleep(2) 
     
    77105                                                         set_power=-60, \ 
    78106                                   run_state=jaraco.nxt.messages.RunState.running) 
    79                 conn.send(cmd) 
     107                connection.send(cmd) 
    80108                 
    81109                time.sleep(2) 
    82110                 
    83111                cmd = jaraco.nxt.messages.SetOutputState(port) 
    84                 conn.send(cmd) 
     112                connection.send(cmd) 
     113         
     114         
     115        ################################################################## 
     116         
     117        def drive(self, connection, left_power, right_power, duration): 
     118                 
     119                "Operate both motors, each at certain power, for a certain duration" 
     120                 
     121                left_motor = self.connect_to_port(MOTOR_PORT_LEFT) 
     122                right_motor = self.connect_to_port(MOTOR_PORT_RIGHT) 
     123                 
     124                 
     125                # If motors are mounted on the robot in reverse then all  
     126                # power settings need to be reversed 
     127                if MOTORS_MOUNTED_BACKWARDS: 
     128                        left_power = -left_power 
     129                        right_power = -right_power 
     130                 
     131                # Issue operation settings to both motors 
     132                cmd = jaraco.nxt.messages.SetOutputState(left_motor, \ 
     133                                                                                                                          motor_on=True, \ 
     134                                                                                                                          set_power=left_power, \ 
     135                                                                 run_state=jaraco.nxt.messages.RunState.running) 
     136                connection.send(cmd) 
     137                 
     138                cmd = jaraco.nxt.messages.SetOutputState(right_motor, \ 
     139                                                                                                                          motor_on=True, \ 
     140                                                                                                                          set_power=right_power, \ 
     141                                                                 run_state=jaraco.nxt.messages.RunState.running) 
     142                connection.send(cmd) 
     143                 
     144                # Continue operating motors while control program pauses 
     145                time.sleep(duration) 
     146                 
     147                # Issue command to stop both motors 
     148                cmd = jaraco.nxt.messages.SetOutputState(left_motor) 
     149                connection.send(cmd) 
     150                cmd = jaraco.nxt.messages.SetOutputState(right_motor) 
     151                connection.send(cmd) 
     152         
     153         
     154        ################################################################## 
     155         
     156        def drive_forward(self, connection, power=80, duration=3): 
     157                 
     158                "Drive the robot forward at a certain speed for a certain duration" 
     159                 
     160                self.drive(connection, power, power, duration) 
     161         
     162         
     163        ################################################################## 
     164         
     165        def turn_in_reverse(self, connection, power=80, duration=3): 
     166                 
     167                "Turn the robot clockwise while backing up at a" 
     168                " certain speed for a certain duration" 
     169                 
     170                left_power = -(power/3) 
     171                right_power = -power 
     172                 
     173                self.drive(connection, left_power, right_power, duration) 
     174         
     175         
     176        ################################################################## 
     177         
     178        def turn_left(self, connection, power=80, duration=3): 
     179                 
     180                "Turn the robot counter-clockwise at a" 
     181                " certain speed for a certain duration" 
     182                 
     183                left_power = -(power/2) 
     184                right_power = power 
     185                 
     186                self.drive(connection, left_power, right_power, duration) 
     187         
     188         
     189        ################################################################## 
     190         
     191        def turn_right(self, connection, power=80, duration=3): 
     192                 
     193                "Turn the robot counter-clockwise at a" 
     194                " certain speed for a certain duration" 
     195                 
     196                left_power = power 
     197                right_power = -power 
     198                 
     199                self.drive(connection, left_power, right_power, duration) 
     200         
     201         
     202        ################################################################## 
     203         
     204        def test_drive(self, connection): 
     205                 
     206                #self.cycle_motor(self.connection, motor_port='a') 
     207                 
     208                self.drive_forward(self.connection, power=100, duration=3) 
     209                 
     210                # half turn in reverse 
     211                self.turn_in_reverse(self.connection, power=100, duration=2) 
     212                 
     213                self.drive_forward(self.connection, power=80, duration=2) 
     214                 
     215                # quarter turn left 
     216                self.turn_left(self.connection, power=80, duration=2) 
     217                 
     218                self.drive_forward(self.connection, power=80, duration=1) 
     219                 
     220                # half turn right 
     221                self.turn_right(self.connection, power=80, duration=2) 
     222                 
     223                self.drive_forward(self.connection, power=80, duration=1) 
    85224         
    86225         
     
    89228        def run(self): 
    90229                 
    91                 conn = self.connect_to_nxt(self.device) 
    92                  
    93                 self.cycle_motor(conn, 'a') 
    94                  
    95                 conn.close() 
     230                self.connection = self.connect_to_nxt(self.device) 
     231                 
     232                self.test_drive(self.connection) 
     233                 
     234                self.connection.close() 
    96235 
    97236 
Note: See TracChangeset for help on using the changeset viewer.