source: remote_control/puzzlebox_brainstorms_remote_control.py @ 107

Last change on this file since 107 was 107, checked in by sc, 11 years ago
  • support for user-configurable NXT bluetooth device port in interface
  • window icon for interface manually set
  • Property svn:executable set to *
File size: 9.7 KB
Line 
1#!/usr/bin/env python
2# -*- coding: utf-8 -*-
3#
4# Puzzlebox - Brainstorms - Remote Control
5#
6# Copyright Puzzlebox Productions, LLC (2010)
7#
8# This code is released under the GNU Pulic License (GPL) version 2
9# For more information please refer to http://www.gnu.org/copyleft/gpl.html
10#
11# Last Update: 2010.07.10
12#
13#####################################################################
14
15import sys, time
16import serial
17
18import jaraco.nxt
19import jaraco.nxt.messages
20
21import puzzlebox_brainstorms_configuration as configuration
22
23#####################################################################
24# Globals
25#####################################################################
26
27DEBUG = 1
28
29VARIABLE_CONTROL_DURATION = configuration.BRAINSTORMS_VARIABLE_CONTROL_DURATION
30
31BLUETOOTH_DEVICE = configuration.NXT_BLUETOOTH_DEVICE
32
33DEFAULT_NXT_POWER_LEVEL = configuration.DEFAULT_NXT_POWER_LEVEL
34
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
39
40#####################################################################
41# Classes
42#####################################################################
43
44class puzzlebox_brainstorms_remote_control:
45       
46        def __init__(self, \
47                     device=BLUETOOTH_DEVICE, \
48                     command=DEFAULT_RC_COMMAND, \
49                     DEBUG=DEBUG):
50               
51                self.DEBUG = DEBUG
52               
53                self.device = device
54                self.command = command
55               
56                self.connection = None
57               
58                try:
59                        self.connection = self.connect_to_nxt(self.device)
60                except Exception, e:
61                        if self.DEBUG:
62                                print "<-- [RC] Connection failed to NXT device [%s]" % self.device
63                                print "ERROR [RC]:",
64                                print e
65       
66       
67        ##################################################################
68       
69        def connect_to_nxt(self, device):
70               
71                connection = jaraco.nxt.Connection(self.device)
72               
73                if self.DEBUG:
74                        battery_voltage = self.get_battery_voltage(connection)
75                        print "--> [RC] Battery voltage:",
76                        print battery_voltage
77               
78               
79                return(connection)
80       
81       
82        ##################################################################
83       
84        def connect_to_port(self, device_port):
85       
86                if isinstance(device_port, basestring):
87                        port = getattr(jaraco.nxt.messages.OutputPort, device_port)
88               
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                voltage = 'N/A'
99               
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()
107               
108               
109                return(voltage)
110       
111       
112        ##################################################################
113       
114        def cycle_motor(self, connection, motor_port='a'):
115               
116                "Turn the motor one direction, then the other, then stop it"
117               
118                port = self.connect_to_port(motor_port)
119                cmd = jaraco.nxt.messages.SetOutputState(port, \
120                                                         motor_on=True, \
121                                                         set_power=60, \
122                                   run_state=jaraco.nxt.messages.RunState.running)
123                connection.send(cmd)
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)
131                connection.send(cmd)
132               
133                time.sleep(2)
134               
135                cmd = jaraco.nxt.messages.SetOutputState(port)
136                connection.send(cmd)
137       
138       
139        ##################################################################
140       
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
169                if not VARIABLE_CONTROL_DURATION:
170                        time.sleep(duration)
171                       
172                        self.stop_motors(connection)
173       
174       
175        ##################################################################
176       
177        def stop_motors(self, connection):
178               
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               
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       
193        def drive_forward(self, connection, power=DEFAULT_NXT_POWER_LEVEL, duration=2):
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       
202        def drive_reverse(self, connection, power=DEFAULT_NXT_POWER_LEVEL, duration=2):
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       
213        def turn_left_in_reverse(self, connection, power=DEFAULT_NXT_POWER_LEVEL, duration=3):
214               
215                "Turn the robot counter-clockwise while backing up at a"
216                " certain speed for a certain duration"
217               
218                left_power = -(power/3)
219                right_power = -power
220               
221                self.drive(connection, left_power, right_power, duration)
222       
223       
224        ##################################################################
225       
226        def turn_right_in_reverse(self, connection, power=DEFAULT_NXT_POWER_LEVEL, duration=3):
227               
228                "Turn the robot clockwise while backing up at a"
229                " certain speed for a certain duration"
230               
231                left_power = -power
232                right_power = -(power/3)
233               
234                self.drive(connection, left_power, right_power, duration)
235       
236       
237        ##################################################################
238       
239        def turn_left(self, connection, power=DEFAULT_NXT_POWER_LEVEL, duration=2):
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       
252        def turn_right(self, connection, power=DEFAULT_NXT_POWER_LEVEL, duration=2):
253               
254                "Turn the robot counter-clockwise at a"
255                " certain speed for a certain duration"
256               
257                left_power = power
258                right_power = -(power/2)
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
272                self.turn_right_in_reverse(self.connection, power=100, duration=2)
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)
285               
286                self.drive_reverse(self.connection, power=80, duration=1)
287       
288       
289        ##################################################################
290       
291        def run(self, command, power=DEFAULT_NXT_POWER_LEVEL):
292               
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               
299                if (command == 'drive_forward'):
300                        self.drive_forward(self.connection, power=power, duration=3)
301               
302                elif (command == 'drive_reverse'):
303                        self.drive_reverse(self.connection, power=power)
304               
305                elif (command == 'turn_left'):
306                        self.turn_left(self.connection, power=power)
307               
308                elif (command == 'turn_right'):
309                        self.turn_right(self.connection, power=power)
310               
311                elif (command == 'turn_left_in_reverse'):
312                        self.turn_left_in_reverse(self.connection, power=power)
313               
314                elif (command == 'turn_right_in_reverse'):
315                        self.turn_right_in_reverse(self.connection, power=power)
316               
317                elif (command == 'stop_motors'):
318                        self.stop_motors(self.connection)
319               
320                elif (command == 'test_drive'):
321                        self.test_drive(self.connection)
322       
323       
324        ##################################################################
325       
326        def stop(self):
327               
328                self.connection.close()
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
343        command = DEFAULT_RC_COMMAND
344       
345        for each in sys.argv:
346               
347                if each.startswith("--device="):
348                        device = each[ len("--device="): ]
349                elif each.startswith("--command="):
350                        command = each[ len("--command="): ]
351       
352       
353        rc = puzzlebox_brainstorms_remote_control(device=device, command=command, DEBUG=DEBUG)
354       
355        if rc.connection == None:
356                sys.exit()
357       
358        rc.run(rc.command)
359        rc.stop()
360
Note: See TracBrowser for help on using the repository browser.