Changeset 3 for rc

Timestamp:

01/27/10 18:23:04 (13 years ago)

Author:

sc

Message:

brainstorms_rc:

• driving functions added

File:

• rc/puzzlebox_brainstorms_rc.py (modified) (7 diffs)

rc/puzzlebox_brainstorms_rc.py

@@ -11 +11 @@
 # For more information please refer to http://www.gnu.org/copyleft/gpl.html
 #
-# Last Update: 2010.01.25
+# Last Update: 2010.01.27
 #
 #####################################################################
@@ -19 +19 @@
 import jaraco.nxt
 import jaraco.nxt.messages
-import jaraco.nxt.routine
 
 #####################################################################
@@ -28 +27 @@
 
 BLUETOOTH_DEVICE='/dev/rfcomm0'
+MOTORS_MOUNTED_BACKWARDS = True
+MOTOR_PORT_RIGHT = 'a'
+MOTOR_PORT_LEFT = 'b'
 
 #####################################################################
@@ -39 +41 @@
                 self.DEBUG = DEBUG
                 
-                self.device=device
+                self.device = device
+                
+                self.connection = None
         
         
@@ -46 +50 @@
         def connect_to_nxt(self, device):
                 
-                conn = jaraco.nxt.Connection(self.device)
+                connection = jaraco.nxt.Connection(self.device)
                 
                 if self.DEBUG > 1:
-                        battery_voltage = jaraco.nxt.routine.get_voltage(conn)
+                        battery_voltage = self.get_battery_voltage(connection)
                         print "Battery voltage:",
                         print battery_voltage
                 
                 
-                return(conn)
-        
-        
-        ##################################################################
-        
-        def cycle_motor(self, conn, port):
+                return(connection)
+        
+        
+        ##################################################################
+        
+        def connect_to_port(self, device_port):
+        
+                if isinstance(device_port, basestring):
+                        port = getattr(jaraco.nxt.messages.OutputPort, device_port)
+                
+                assert port in jaraco.nxt.messages.OutputPort.values()
+                
+                return(port)
+        
+        
+        ##################################################################
+        
+        def get_battery_voltage(self, connection):
+                
+                cmd = jaraco.nxt.messages.GetBatteryLevel()
+                connection.send(cmd)
+                
+                response = connection.receive()
+                voltage = response.get_voltage()
+                
+                return(voltage)
+        
+        
+        ##################################################################
+        
+        def cycle_motor(self, connection, motor_port='a'):
                 
                 "Turn the motor one direction, then the other, then stop it"
                 
-                port = jaraco.nxt.routine.get_port(port, \
-                                                   jaraco.nxt.messages.OutputPort)
+                port = self.connect_to_port(motor_port)
                 cmd = jaraco.nxt.messages.SetOutputState(port, \
                                                          motor_on=True, \
                                                          set_power=60, \
                                    run_state=jaraco.nxt.messages.RunState.running)
-                conn.send(cmd)
+                connection.send(cmd)
                 
                 time.sleep(2)
@@ -77 +105 @@
                                                          set_power=-60, \
                                    run_state=jaraco.nxt.messages.RunState.running)
-                conn.send(cmd)
+                connection.send(cmd)
                 
                 time.sleep(2)
                 
                 cmd = jaraco.nxt.messages.SetOutputState(port)
-                conn.send(cmd)
+                connection.send(cmd)
+        
+        
+        ##################################################################
+        
+        def drive(self, connection, left_power, right_power, duration):
+                
+                "Operate both motors, each at certain power, for a certain duration"
+                
+                left_motor = self.connect_to_port(MOTOR_PORT_LEFT)
+                right_motor = self.connect_to_port(MOTOR_PORT_RIGHT)
+                
+                
+                # If motors are mounted on the robot in reverse then all 
+                # power settings need to be reversed
+                if MOTORS_MOUNTED_BACKWARDS:
+                        left_power = -left_power
+                        right_power = -right_power
+                
+                # Issue operation settings to both motors
+                cmd = jaraco.nxt.messages.SetOutputState(left_motor, \
+                                                                                                                          motor_on=True, \
+                                                                                                                          set_power=left_power, \
+                                                                 run_state=jaraco.nxt.messages.RunState.running)
+                connection.send(cmd)
+                
+                cmd = jaraco.nxt.messages.SetOutputState(right_motor, \
+                                                                                                                          motor_on=True, \
+                                                                                                                          set_power=right_power, \
+                                                                 run_state=jaraco.nxt.messages.RunState.running)
+                connection.send(cmd)
+                
+                # Continue operating motors while control program pauses
+                time.sleep(duration)
+                
+                # Issue command to stop both motors
+                cmd = jaraco.nxt.messages.SetOutputState(left_motor)
+                connection.send(cmd)
+                cmd = jaraco.nxt.messages.SetOutputState(right_motor)
+                connection.send(cmd)
+        
+        
+        ##################################################################
+        
+        def drive_forward(self, connection, power=80, duration=3):
+                
+                "Drive the robot forward at a certain speed for a certain duration"
+                
+                self.drive(connection, power, power, duration)
+        
+        
+        ##################################################################
+        
+        def turn_in_reverse(self, connection, power=80, duration=3):
+                
+                "Turn the robot clockwise while backing up at a"
+                " certain speed for a certain duration"
+                
+                left_power = -(power/3)
+                right_power = -power
+                
+                self.drive(connection, left_power, right_power, duration)
+        
+        
+        ##################################################################
+        
+        def turn_left(self, connection, power=80, duration=3):
+                
+                "Turn the robot counter-clockwise at a"
+                " certain speed for a certain duration"
+                
+                left_power = -(power/2)
+                right_power = power
+                
+                self.drive(connection, left_power, right_power, duration)
+        
+        
+        ##################################################################
+        
+        def turn_right(self, connection, power=80, duration=3):
+                
+                "Turn the robot counter-clockwise at a"
+                " certain speed for a certain duration"
+                
+                left_power = power
+                right_power = -power
+                
+                self.drive(connection, left_power, right_power, duration)
+        
+        
+        ##################################################################
+        
+        def test_drive(self, connection):
+                
+                #self.cycle_motor(self.connection, motor_port='a')
+                
+                self.drive_forward(self.connection, power=100, duration=3)
+                
+                # half turn in reverse
+                self.turn_in_reverse(self.connection, power=100, duration=2)
+                
+                self.drive_forward(self.connection, power=80, duration=2)
+                
+                # quarter turn left
+                self.turn_left(self.connection, power=80, duration=2)
+                
+                self.drive_forward(self.connection, power=80, duration=1)
+                
+                # half turn right
+                self.turn_right(self.connection, power=80, duration=2)
+                
+                self.drive_forward(self.connection, power=80, duration=1)
         
         
@@ -89 +228 @@
         def run(self):
                 
-                conn = self.connect_to_nxt(self.device)
-                
-                self.cycle_motor(conn, 'a')
-                
-                conn.close()
+                self.connection = self.connect_to_nxt(self.device)
+                
+                self.test_drive(self.connection)
+                
+                self.connection.close()