source: remote_control/puzzlebox_brainstorms_remote_control.py @ 103

Last change on this file since 103 was 101, checked in by sc, 11 years ago
  • DISCRETE_CONTROL_COMMANDS renamed VARIABLE_CONTROL_DURATION
  • Property svn:executable set to *
File size: 9.7 KB
RevLine 
[1]1#!/usr/bin/env python
2# -*- coding: utf-8 -*-
3#
[5]4# Puzzlebox - Brainstorms - Remote Control
[1]5#
[5]6# Copyright Puzzlebox Productions, LLC (2010)
[1]7#
[31]8# This code is released under the GNU Pulic License (GPL) version 2
[1]9# For more information please refer to http://www.gnu.org/copyleft/gpl.html
10#
[98]11# Last Update: 2010.07.09
[1]12#
13#####################################################################
14
15import sys, time
16import serial
[14]17
[1]18import jaraco.nxt
19import jaraco.nxt.messages
[14]20
[5]21import puzzlebox_brainstorms_configuration as configuration
[1]22
23#####################################################################
24# Globals
25#####################################################################
26
[2]27DEBUG = 2
[1]28
[101]29VARIABLE_CONTROL_DURATION = configuration.BRAINSTORMS_VARIABLE_CONTROL_DURATION
[42]30
[84]31DEFAULT_NXT_POWER_LEVEL = configuration.DEFAULT_NXT_POWER_LEVEL
32
[73]33BLUETOOTH_DEVICE = configuration.NXT_BLUETOOTH_DEVICE
[84]34
[73]35MOTORS_MOUNTED_BACKWARDS = configuration.NXT_MOTORS_MOUNTED_BACKWARDS
36MOTOR_PORT_RIGHT = configuration.NXT_MOTOR_PORT_RIGHT
37MOTOR_PORT_LEFT = configuration.NXT_MOTOR_PORT_LEFT
38DEFAULT_RC_COMMAND = configuration.NXT_DEFAULT_RC_COMMAND
[1]39
40#####################################################################
41# Classes
42#####################################################################
43
[73]44class puzzlebox_brainstorms_remote_control:
[1]45       
[7]46        def __init__(self, \
47                     device=BLUETOOTH_DEVICE, \
48                     command=DEFAULT_RC_COMMAND, \
49                     DEBUG=DEBUG):
[1]50               
51                self.DEBUG = DEBUG
52               
[3]53                self.device = device
[7]54                self.command = command
[3]55               
[14]56                self.connection = None
57               
[12]58                try:
59                        self.connection = self.connect_to_nxt(self.device)
[87]60                except Exception, e:
[12]61                        if self.DEBUG:
[87]62                                print "<-- [RC] Connection failed to NXT device [%s]" % self.device
63                                print "ERROR [RC]:",
64                                print e
[1]65       
66       
67        ##################################################################
68       
[2]69        def connect_to_nxt(self, device):
70               
[3]71                connection = jaraco.nxt.Connection(self.device)
[2]72               
[14]73                if self.DEBUG:
[3]74                        battery_voltage = self.get_battery_voltage(connection)
[12]75                        print "--> [RC] Battery voltage:",
[2]76                        print battery_voltage
77               
78               
[3]79                return(connection)
[2]80       
81       
82        ##################################################################
83       
[3]84        def connect_to_port(self, device_port):
85       
86                if isinstance(device_port, basestring):
87                        port = getattr(jaraco.nxt.messages.OutputPort, device_port)
[1]88               
[3]89                assert port in jaraco.nxt.messages.OutputPort.values()
90               
91                return(port)
92       
93       
94        ##################################################################
95       
96        def get_battery_voltage(self, connection):
97               
[98]98                voltage = 'N/A'
[3]99               
[98]100                if connection != None:
101                       
102                        cmd = jaraco.nxt.messages.GetBatteryLevel()
103                        connection.send(cmd)
104                       
105                        response = connection.receive()
106                        voltage = response.get_voltage()
[3]107               
[98]108               
[3]109                return(voltage)
110       
111       
112        ##################################################################
113       
114        def cycle_motor(self, connection, motor_port='a'):
115               
[1]116                "Turn the motor one direction, then the other, then stop it"
117               
[3]118                port = self.connect_to_port(motor_port)
[1]119                cmd = jaraco.nxt.messages.SetOutputState(port, \
120                                                         motor_on=True, \
121                                                         set_power=60, \
122                                   run_state=jaraco.nxt.messages.RunState.running)
[3]123                connection.send(cmd)
[1]124               
125                time.sleep(2)
126               
127                cmd = jaraco.nxt.messages.SetOutputState(port, \
128                                                         motor_on=True, \
129                                                         set_power=-60, \
130                                   run_state=jaraco.nxt.messages.RunState.running)
[3]131                connection.send(cmd)
[1]132               
133                time.sleep(2)
134               
135                cmd = jaraco.nxt.messages.SetOutputState(port)
[3]136                connection.send(cmd)
[1]137       
138       
139        ##################################################################
140       
[3]141        def drive(self, connection, left_power, right_power, duration):
142               
143                "Operate both motors, each at certain power, for a certain duration"
144               
145                left_motor = self.connect_to_port(MOTOR_PORT_LEFT)
146                right_motor = self.connect_to_port(MOTOR_PORT_RIGHT)
147               
148               
149                # If motors are mounted on the robot in reverse then all
150                # power settings need to be reversed
151                if MOTORS_MOUNTED_BACKWARDS:
152                        left_power = -left_power
153                        right_power = -right_power
154               
155                # Issue operation settings to both motors
156                cmd = jaraco.nxt.messages.SetOutputState(left_motor, \
157                                                                                                                          motor_on=True, \
158                                                                                                                          set_power=left_power, \
159                                                                 run_state=jaraco.nxt.messages.RunState.running)
160                connection.send(cmd)
161               
162                cmd = jaraco.nxt.messages.SetOutputState(right_motor, \
163                                                                                                                          motor_on=True, \
164                                                                                                                          set_power=right_power, \
165                                                                 run_state=jaraco.nxt.messages.RunState.running)
166                connection.send(cmd)
167               
168                # Continue operating motors while control program pauses
[101]169                if not VARIABLE_CONTROL_DURATION:
[42]170                        time.sleep(duration)
171                       
172                        self.stop_motors(connection)
173       
174       
175        ##################################################################
176       
177        def stop_motors(self, connection):
[3]178               
[42]179                "Stop both motors"
180               
181                left_motor = self.connect_to_port(MOTOR_PORT_LEFT)
182                right_motor = self.connect_to_port(MOTOR_PORT_RIGHT)
183               
[3]184                # Issue command to stop both motors
185                cmd = jaraco.nxt.messages.SetOutputState(left_motor)
186                connection.send(cmd)
187                cmd = jaraco.nxt.messages.SetOutputState(right_motor)
188                connection.send(cmd)
189       
190       
191        ##################################################################
192       
[84]193        def drive_forward(self, connection, power=DEFAULT_NXT_POWER_LEVEL, duration=2):
[3]194               
195                "Drive the robot forward at a certain speed for a certain duration"
196               
197                self.drive(connection, power, power, duration)
198       
199       
200        ##################################################################
201       
[84]202        def drive_reverse(self, connection, power=DEFAULT_NXT_POWER_LEVEL, duration=2):
[8]203               
204                "Drive the robot reverse at a certain speed for a certain duration"
205               
206                power = -power
207               
208                self.drive(connection, power, power, duration)
209       
210       
211        ##################################################################
212       
[84]213        def turn_left_in_reverse(self, connection, power=DEFAULT_NXT_POWER_LEVEL, duration=3):
[3]214               
[21]215                "Turn the robot counter-clockwise while backing up at a"
216                " certain speed for a certain duration"
217               
[33]218                left_power = -(power/3)
219                right_power = -power
[21]220               
221                self.drive(connection, left_power, right_power, duration)
222       
223       
224        ##################################################################
225       
[84]226        def turn_right_in_reverse(self, connection, power=DEFAULT_NXT_POWER_LEVEL, duration=3):
[21]227               
[3]228                "Turn the robot clockwise while backing up at a"
229                " certain speed for a certain duration"
230               
[33]231                left_power = -power
232                right_power = -(power/3)
[3]233               
234                self.drive(connection, left_power, right_power, duration)
235       
236       
237        ##################################################################
238       
[84]239        def turn_left(self, connection, power=DEFAULT_NXT_POWER_LEVEL, duration=2):
[3]240               
241                "Turn the robot counter-clockwise at a"
242                " certain speed for a certain duration"
243               
244                left_power = -(power/2)
245                right_power = power
246               
247                self.drive(connection, left_power, right_power, duration)
248       
249       
250        ##################################################################
251       
[84]252        def turn_right(self, connection, power=DEFAULT_NXT_POWER_LEVEL, duration=2):
[3]253               
254                "Turn the robot counter-clockwise at a"
255                " certain speed for a certain duration"
256               
257                left_power = power
[8]258                right_power = -(power/2)
[3]259               
260                self.drive(connection, left_power, right_power, duration)
261       
262       
263        ##################################################################
264       
265        def test_drive(self, connection):
266               
267                #self.cycle_motor(self.connection, motor_port='a')
268               
269                self.drive_forward(self.connection, power=100, duration=3)
270               
271                # half turn in reverse
[21]272                self.turn_right_in_reverse(self.connection, power=100, duration=2)
[3]273               
274                self.drive_forward(self.connection, power=80, duration=2)
275               
276                # quarter turn left
277                self.turn_left(self.connection, power=80, duration=2)
278               
279                self.drive_forward(self.connection, power=80, duration=1)
280               
281                # half turn right
282                self.turn_right(self.connection, power=80, duration=2)
283               
284                self.drive_forward(self.connection, power=80, duration=1)
[12]285               
286                self.drive_reverse(self.connection, power=80, duration=1)
[3]287       
288       
289        ##################################################################
290       
[84]291        def run(self, command, power=DEFAULT_NXT_POWER_LEVEL):
[1]292               
[84]293                # If the LEGO Mindstorms NXT is instructed to drive with
294                # power less than 50% performance is so poor that we'd be
295                # better off simply stopping the motors.
296                if power < 50:
297                        command = 'stop_motors'
298               
[42]299                if (command == 'drive_forward'):
[84]300                        self.drive_forward(self.connection, power=power, duration=3)
[1]301               
[8]302                elif (command == 'drive_reverse'):
[84]303                        self.drive_reverse(self.connection, power=power)
[8]304               
[7]305                elif (command == 'turn_left'):
[84]306                        self.turn_left(self.connection, power=power)
[7]307               
308                elif (command == 'turn_right'):
[84]309                        self.turn_right(self.connection, power=power)
[12]310               
[21]311                elif (command == 'turn_left_in_reverse'):
[84]312                        self.turn_left_in_reverse(self.connection, power=power)
[21]313               
314                elif (command == 'turn_right_in_reverse'):
[84]315                        self.turn_right_in_reverse(self.connection, power=power)
[42]316               
317                elif (command == 'stop_motors'):
318                        self.stop_motors(self.connection)
319               
320                elif (command == 'test_drive'):
321                        self.test_drive(self.connection)
[7]322       
323       
324        ##################################################################
325       
326        def stop(self):
327               
[3]328                self.connection.close()
[1]329
330
331#####################################################################
332# Functions
333#####################################################################
334
335#####################################################################
336# Main
337#####################################################################
338
339if __name__ == '__main__':
340       
341        # Collect default settings and command line parameters
342        device = BLUETOOTH_DEVICE
[8]343        command = DEFAULT_RC_COMMAND
[1]344       
345        for each in sys.argv:
346               
347                if each.startswith("--device="):
348                        device = each[ len("--device="): ]
[7]349                elif each.startswith("--command="):
[8]350                        command = each[ len("--command="): ]
[1]351       
352       
[73]353        rc = puzzlebox_brainstorms_remote_control(device=device, command=command, DEBUG=DEBUG)
[1]354       
[14]355        if rc.connection == None:
356                sys.exit()
357       
[7]358        rc.run(rc.command)
359        rc.stop()
[1]360
Note: See TracBrowser for help on using the repository browser.