source: remote_control/puzzlebox_brainstorms_remote_control.py @ 63

Last change on this file since 63 was 42, checked in by sc, 12 years ago
  • initial support for discrete control commands
  • Property svn:executable set to *
File size: 9.1 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.05.03
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 = 2
28
29DISCRETE_CONTROL_COMMANDS = configuration.DISCRETE_CONTROL_COMMANDS
30
31BLUETOOTH_DEVICE = configuration.BLUETOOTH_DEVICE
32MOTORS_MOUNTED_BACKWARDS = configuration.MOTORS_MOUNTED_BACKWARDS
33MOTOR_PORT_RIGHT = configuration.MOTOR_PORT_RIGHT
34MOTOR_PORT_LEFT = configuration.MOTOR_PORT_LEFT
35DEFAULT_RC_COMMAND = configuration.DEFAULT_RC_COMMAND
36
37#####################################################################
38# Classes
39#####################################################################
40
41class puzzlebox_brainstorms_rc:
42       
43        def __init__(self, \
44                     device=BLUETOOTH_DEVICE, \
45                     command=DEFAULT_RC_COMMAND, \
46                     DEBUG=DEBUG):
47               
48                self.DEBUG = DEBUG
49               
50                self.device = device
51                self.command = command
52               
53                self.connection = None
54               
55                try:
56                        self.connection = self.connect_to_nxt(self.device)
57                except:
58                        if self.DEBUG:
59                                print "--> [RC] Connection failed to NXT device [%s]" % self.device
60       
61       
62        ##################################################################
63       
64        def connect_to_nxt(self, device):
65               
66                connection = jaraco.nxt.Connection(self.device)
67               
68                if self.DEBUG:
69                        battery_voltage = self.get_battery_voltage(connection)
70                        print "--> [RC] Battery voltage:",
71                        print battery_voltage
72               
73               
74                return(connection)
75       
76       
77        ##################################################################
78       
79        def connect_to_port(self, device_port):
80       
81                if isinstance(device_port, basestring):
82                        port = getattr(jaraco.nxt.messages.OutputPort, device_port)
83               
84                assert port in jaraco.nxt.messages.OutputPort.values()
85               
86                return(port)
87       
88       
89        ##################################################################
90       
91        def get_battery_voltage(self, connection):
92               
93                cmd = jaraco.nxt.messages.GetBatteryLevel()
94                connection.send(cmd)
95               
96                response = connection.receive()
97                voltage = response.get_voltage()
98               
99                return(voltage)
100       
101       
102        ##################################################################
103       
104        def cycle_motor(self, connection, motor_port='a'):
105               
106                "Turn the motor one direction, then the other, then stop it"
107               
108                port = self.connect_to_port(motor_port)
109                cmd = jaraco.nxt.messages.SetOutputState(port, \
110                                                         motor_on=True, \
111                                                         set_power=60, \
112                                   run_state=jaraco.nxt.messages.RunState.running)
113                connection.send(cmd)
114               
115                time.sleep(2)
116               
117                cmd = jaraco.nxt.messages.SetOutputState(port, \
118                                                         motor_on=True, \
119                                                         set_power=-60, \
120                                   run_state=jaraco.nxt.messages.RunState.running)
121                connection.send(cmd)
122               
123                time.sleep(2)
124               
125                cmd = jaraco.nxt.messages.SetOutputState(port)
126                connection.send(cmd)
127       
128       
129        ##################################################################
130       
131        def drive(self, connection, left_power, right_power, duration):
132               
133                "Operate both motors, each at certain power, for a certain duration"
134               
135                left_motor = self.connect_to_port(MOTOR_PORT_LEFT)
136                right_motor = self.connect_to_port(MOTOR_PORT_RIGHT)
137               
138               
139                # If motors are mounted on the robot in reverse then all
140                # power settings need to be reversed
141                if MOTORS_MOUNTED_BACKWARDS:
142                        left_power = -left_power
143                        right_power = -right_power
144               
145                # Issue operation settings to both motors
146                cmd = jaraco.nxt.messages.SetOutputState(left_motor, \
147                                                                                                                          motor_on=True, \
148                                                                                                                          set_power=left_power, \
149                                                                 run_state=jaraco.nxt.messages.RunState.running)
150                connection.send(cmd)
151               
152                cmd = jaraco.nxt.messages.SetOutputState(right_motor, \
153                                                                                                                          motor_on=True, \
154                                                                                                                          set_power=right_power, \
155                                                                 run_state=jaraco.nxt.messages.RunState.running)
156                connection.send(cmd)
157               
158                # Continue operating motors while control program pauses
159                if DISCRETE_CONTROL_COMMANDS:
160                        time.sleep(duration)
161                       
162                        self.stop_motors(connection)
163       
164       
165        ##################################################################
166       
167        def stop_motors(self, connection):
168               
169                "Stop both motors"
170               
171                left_motor = self.connect_to_port(MOTOR_PORT_LEFT)
172                right_motor = self.connect_to_port(MOTOR_PORT_RIGHT)
173               
174                # Issue command to stop both motors
175                cmd = jaraco.nxt.messages.SetOutputState(left_motor)
176                connection.send(cmd)
177                cmd = jaraco.nxt.messages.SetOutputState(right_motor)
178                connection.send(cmd)
179       
180       
181        ##################################################################
182       
183        def drive_forward(self, connection, power=80, duration=2):
184               
185                "Drive the robot forward at a certain speed for a certain duration"
186               
187                self.drive(connection, power, power, duration)
188       
189       
190        ##################################################################
191       
192        def drive_reverse(self, connection, power=80, duration=2):
193               
194                "Drive the robot reverse at a certain speed for a certain duration"
195               
196                power = -power
197               
198                self.drive(connection, power, power, duration)
199       
200       
201        ##################################################################
202       
203        def turn_left_in_reverse(self, connection, power=80, duration=3):
204               
205                "Turn the robot counter-clockwise while backing up at a"
206                " certain speed for a certain duration"
207               
208                left_power = -(power/3)
209                right_power = -power
210               
211                self.drive(connection, left_power, right_power, duration)
212       
213       
214        ##################################################################
215       
216        def turn_right_in_reverse(self, connection, power=80, duration=3):
217               
218                "Turn the robot clockwise while backing up at a"
219                " certain speed for a certain duration"
220               
221                left_power = -power
222                right_power = -(power/3)
223               
224                self.drive(connection, left_power, right_power, duration)
225       
226       
227        ##################################################################
228       
229        def turn_left(self, connection, power=80, duration=2):
230               
231                "Turn the robot counter-clockwise at a"
232                " certain speed for a certain duration"
233               
234                left_power = -(power/2)
235                right_power = power
236               
237                self.drive(connection, left_power, right_power, duration)
238       
239       
240        ##################################################################
241       
242        def turn_right(self, connection, power=80, duration=2):
243               
244                "Turn the robot counter-clockwise at a"
245                " certain speed for a certain duration"
246               
247                left_power = power
248                right_power = -(power/2)
249               
250                self.drive(connection, left_power, right_power, duration)
251       
252       
253        ##################################################################
254       
255        def test_drive(self, connection):
256               
257                #self.cycle_motor(self.connection, motor_port='a')
258               
259                self.drive_forward(self.connection, power=100, duration=3)
260               
261                # half turn in reverse
262                self.turn_right_in_reverse(self.connection, power=100, duration=2)
263               
264                self.drive_forward(self.connection, power=80, duration=2)
265               
266                # quarter turn left
267                self.turn_left(self.connection, power=80, duration=2)
268               
269                self.drive_forward(self.connection, power=80, duration=1)
270               
271                # half turn right
272                self.turn_right(self.connection, power=80, duration=2)
273               
274                self.drive_forward(self.connection, power=80, duration=1)
275               
276                self.drive_reverse(self.connection, power=80, duration=1)
277       
278       
279        ##################################################################
280       
281        def run(self, command):
282               
283                if (command == 'drive_forward'):
284                        self.drive_forward(self.connection, duration=3)
285               
286                elif (command == 'drive_reverse'):
287                        self.drive_reverse(self.connection)
288               
289                elif (command == 'turn_left'):
290                        self.turn_left(self.connection)
291               
292                elif (command == 'turn_right'):
293                        self.turn_right(self.connection)
294               
295                elif (command == 'turn_left_in_reverse'):
296                        self.turn_left_in_reverse(self.connection)
297               
298                elif (command == 'turn_right_in_reverse'):
299                        self.turn_right_in_reverse(self.connection)
300               
301                elif (command == 'stop_motors'):
302                        self.stop_motors(self.connection)
303               
304                elif (command == 'test_drive'):
305                        self.test_drive(self.connection)
306       
307       
308        ##################################################################
309       
310        def stop(self):
311               
312                self.connection.close()
313
314
315#####################################################################
316# Functions
317#####################################################################
318
319#####################################################################
320# Main
321#####################################################################
322
323if __name__ == '__main__':
324       
325        # Collect default settings and command line parameters
326        device = BLUETOOTH_DEVICE
327        command = DEFAULT_RC_COMMAND
328       
329        for each in sys.argv:
330               
331                if each.startswith("--device="):
332                        device = each[ len("--device="): ]
333                elif each.startswith("--command="):
334                        command = each[ len("--command="): ]
335       
336       
337        rc = puzzlebox_brainstorms_rc(device=device, command=command, DEBUG=DEBUG)
338       
339        if rc.connection == None:
340                sys.exit()
341       
342        rc.run(rc.command)
343        rc.stop()
344
Note: See TracBrowser for help on using the repository browser.