source: remote_control/puzzlebox_brainstorms_remote_control.py @ 37

Last change on this file since 37 was 36, checked in by sc, 13 years ago

rc:

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