source: rc/puzzlebox_brainstorms_rc.py @ 12

Last change on this file since 12 was 12, checked in by sc, 12 years ago

rc:

  • failed NXT connection handling
  • drive_reverse command added

server:

  • improved connection handling

configuration:

  • os module now imported

client:

  • drive_reverse command added

client_interface:

  • keyboard command support added
  • Property svn:executable set to *
File size: 8.2 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# Portions of this code have been previously
9# released under the GNU Pulic License (GPL) version 2
10# and is Copyright Steven M. Castellotti (2010)
11# For more information please refer to http://www.gnu.org/copyleft/gpl.html
12#
13# Last Update: 2010.01.28
14#
15#####################################################################
16
17import sys, time
18import serial
19import jaraco.nxt
20import jaraco.nxt.messages
21import puzzlebox_brainstorms_configuration as configuration
22
23#####################################################################
24# Globals
25#####################################################################
26
27DEBUG = 2
28
29BLUETOOTH_DEVICE = configuration.BLUETOOTH_DEVICE
30MOTORS_MOUNTED_BACKWARDS = configuration.MOTORS_MOUNTED_BACKWARDS
31MOTOR_PORT_RIGHT = configuration.MOTOR_PORT_RIGHT
32MOTOR_PORT_LEFT = configuration.MOTOR_PORT_LEFT
33DEFAULT_RC_COMMAND = configuration.DEFAULT_RC_COMMAND
34
35#####################################################################
36# Classes
37#####################################################################
38
39class puzzlebox_brainstorms_rc:
40       
41        def __init__(self, \
42                     device=BLUETOOTH_DEVICE, \
43                     command=DEFAULT_RC_COMMAND, \
44                     DEBUG=DEBUG):
45               
46                self.DEBUG = DEBUG
47               
48                self.device = device
49                self.command = command
50               
51                try:
52                        self.connection = self.connect_to_nxt(self.device)
53                except:
54                        if self.DEBUG:
55                                print "--> [RC] Connection failed to NXT device [%s]" % self.device
56                        sys.exit()
57       
58       
59        ##################################################################
60       
61        def connect_to_nxt(self, device):
62               
63                connection = jaraco.nxt.Connection(self.device)
64               
65                if self.DEBUG > 1:
66                        battery_voltage = self.get_battery_voltage(connection)
67                        print "--> [RC] Battery voltage:",
68                        print battery_voltage
69               
70               
71                return(connection)
72       
73       
74        ##################################################################
75       
76        def connect_to_port(self, device_port):
77       
78                if isinstance(device_port, basestring):
79                        port = getattr(jaraco.nxt.messages.OutputPort, device_port)
80               
81                assert port in jaraco.nxt.messages.OutputPort.values()
82               
83                return(port)
84       
85       
86        ##################################################################
87       
88        def get_battery_voltage(self, connection):
89               
90                cmd = jaraco.nxt.messages.GetBatteryLevel()
91                connection.send(cmd)
92               
93                response = connection.receive()
94                voltage = response.get_voltage()
95               
96                return(voltage)
97       
98       
99        ##################################################################
100       
101        def cycle_motor(self, connection, motor_port='a'):
102               
103                "Turn the motor one direction, then the other, then stop it"
104               
105                port = self.connect_to_port(motor_port)
106                cmd = jaraco.nxt.messages.SetOutputState(port, \
107                                                         motor_on=True, \
108                                                         set_power=60, \
109                                   run_state=jaraco.nxt.messages.RunState.running)
110                connection.send(cmd)
111               
112                time.sleep(2)
113               
114                cmd = jaraco.nxt.messages.SetOutputState(port, \
115                                                         motor_on=True, \
116                                                         set_power=-60, \
117                                   run_state=jaraco.nxt.messages.RunState.running)
118                connection.send(cmd)
119               
120                time.sleep(2)
121               
122                cmd = jaraco.nxt.messages.SetOutputState(port)
123                connection.send(cmd)
124       
125       
126        ##################################################################
127       
128        def drive(self, connection, left_power, right_power, duration):
129               
130                "Operate both motors, each at certain power, for a certain duration"
131               
132                left_motor = self.connect_to_port(MOTOR_PORT_LEFT)
133                right_motor = self.connect_to_port(MOTOR_PORT_RIGHT)
134               
135               
136                # If motors are mounted on the robot in reverse then all
137                # power settings need to be reversed
138                if MOTORS_MOUNTED_BACKWARDS:
139                        left_power = -left_power
140                        right_power = -right_power
141               
142                # Issue operation settings to both motors
143                cmd = jaraco.nxt.messages.SetOutputState(left_motor, \
144                                                                                                                          motor_on=True, \
145                                                                                                                          set_power=left_power, \
146                                                                 run_state=jaraco.nxt.messages.RunState.running)
147                connection.send(cmd)
148               
149                cmd = jaraco.nxt.messages.SetOutputState(right_motor, \
150                                                                                                                          motor_on=True, \
151                                                                                                                          set_power=right_power, \
152                                                                 run_state=jaraco.nxt.messages.RunState.running)
153                connection.send(cmd)
154               
155                # Continue operating motors while control program pauses
156                time.sleep(duration)
157               
158                # Issue command to stop both motors
159                cmd = jaraco.nxt.messages.SetOutputState(left_motor)
160                connection.send(cmd)
161                cmd = jaraco.nxt.messages.SetOutputState(right_motor)
162                connection.send(cmd)
163       
164       
165        ##################################################################
166       
167        def drive_forward(self, connection, power=80, duration=2):
168               
169                "Drive the robot forward at a certain speed for a certain duration"
170               
171                self.drive(connection, power, power, duration)
172       
173       
174        ##################################################################
175       
176        def drive_reverse(self, connection, power=80, duration=2):
177               
178                "Drive the robot reverse at a certain speed for a certain duration"
179               
180                power = -power
181               
182                self.drive(connection, power, power, duration)
183       
184       
185        ##################################################################
186       
187        def turn_in_reverse(self, connection, power=80, duration=3):
188               
189                "Turn the robot clockwise while backing up at a"
190                " certain speed for a certain duration"
191               
192                left_power = -(power/3)
193                right_power = -power
194               
195                self.drive(connection, left_power, right_power, duration)
196       
197       
198        ##################################################################
199       
200        def turn_left(self, connection, power=80, duration=2):
201               
202                "Turn the robot counter-clockwise at a"
203                " certain speed for a certain duration"
204               
205                left_power = -(power/2)
206                right_power = power
207               
208                self.drive(connection, left_power, right_power, duration)
209       
210       
211        ##################################################################
212       
213        def turn_right(self, connection, power=80, duration=2):
214               
215                "Turn the robot counter-clockwise at a"
216                " certain speed for a certain duration"
217               
218                left_power = power
219                right_power = -(power/2)
220               
221                self.drive(connection, left_power, right_power, duration)
222       
223       
224        ##################################################################
225       
226        def test_drive(self, connection):
227               
228                #self.cycle_motor(self.connection, motor_port='a')
229               
230                self.drive_forward(self.connection, power=100, duration=3)
231               
232                # half turn in reverse
233                self.turn_in_reverse(self.connection, power=100, duration=2)
234               
235                self.drive_forward(self.connection, power=80, duration=2)
236               
237                # quarter turn left
238                self.turn_left(self.connection, power=80, duration=2)
239               
240                self.drive_forward(self.connection, power=80, duration=1)
241               
242                # half turn right
243                self.turn_right(self.connection, power=80, duration=2)
244               
245                self.drive_forward(self.connection, power=80, duration=1)
246               
247                self.drive_reverse(self.connection, power=80, duration=1)
248       
249       
250        ##################################################################
251       
252        def run(self, command):
253               
254                if (command == 'test_drive'):
255                        self.test_drive(self.connection)
256               
257                elif (command == 'drive_forward'):
258                        self.drive_forward(self.connection)
259               
260                elif (command == 'drive_reverse'):
261                        self.drive_reverse(self.connection)
262               
263                elif (command == 'turn_left'):
264                        self.turn_left(self.connection)
265               
266                elif (command == 'turn_right'):
267                        self.turn_right(self.connection)
268               
269                elif (command == 'turn_in_reverse'):
270                        self.turn_in_reverse(self.connection)
271       
272       
273        ##################################################################
274       
275        def stop(self):
276               
277                self.connection.close()
278
279
280#####################################################################
281# Functions
282#####################################################################
283
284
285#####################################################################
286# Main
287#####################################################################
288
289if __name__ == '__main__':
290       
291        # Collect default settings and command line parameters
292        device = BLUETOOTH_DEVICE
293        command = DEFAULT_RC_COMMAND
294       
295        for each in sys.argv:
296               
297                if each.startswith("--device="):
298                        device = each[ len("--device="): ]
299                elif each.startswith("--command="):
300                        command = each[ len("--command="): ]
301       
302       
303        rc = puzzlebox_brainstorms_rc(device=device, command=command, DEBUG=DEBUG)
304       
305        rc.run(rc.command)
306        rc.stop()
307
Note: See TracBrowser for help on using the repository browser.