source: rc/puzzlebox_brainstorms_rc.py @ 14

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

rc:

  • connection handling cleanup

client_interface:

  • turn_in_reverse support fixed
  • 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
19
20import jaraco.nxt
21import jaraco.nxt.messages
22
23import puzzlebox_brainstorms_configuration as configuration
24
25#####################################################################
26# Globals
27#####################################################################
28
29DEBUG = 2
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                time.sleep(duration)
160               
161                # Issue command to stop both motors
162                cmd = jaraco.nxt.messages.SetOutputState(left_motor)
163                connection.send(cmd)
164                cmd = jaraco.nxt.messages.SetOutputState(right_motor)
165                connection.send(cmd)
166       
167       
168        ##################################################################
169       
170        def drive_forward(self, connection, power=80, duration=2):
171               
172                "Drive the robot forward at a certain speed for a certain duration"
173               
174                self.drive(connection, power, power, duration)
175       
176       
177        ##################################################################
178       
179        def drive_reverse(self, connection, power=80, duration=2):
180               
181                "Drive the robot reverse at a certain speed for a certain duration"
182               
183                power = -power
184               
185                self.drive(connection, power, power, duration)
186       
187       
188        ##################################################################
189       
190        def turn_in_reverse(self, connection, power=80, duration=3):
191               
192                "Turn the robot clockwise while backing up at a"
193                " certain speed for a certain duration"
194               
195                left_power = -(power/3)
196                right_power = -power
197               
198                self.drive(connection, left_power, right_power, duration)
199       
200       
201        ##################################################################
202       
203        def turn_left(self, connection, power=80, duration=2):
204               
205                "Turn the robot counter-clockwise at a"
206                " certain speed for a certain duration"
207               
208                left_power = -(power/2)
209                right_power = power
210               
211                self.drive(connection, left_power, right_power, duration)
212       
213       
214        ##################################################################
215       
216        def turn_right(self, connection, power=80, duration=2):
217               
218                "Turn the robot counter-clockwise at a"
219                " certain speed for a certain duration"
220               
221                left_power = power
222                right_power = -(power/2)
223               
224                self.drive(connection, left_power, right_power, duration)
225       
226       
227        ##################################################################
228       
229        def test_drive(self, connection):
230               
231                #self.cycle_motor(self.connection, motor_port='a')
232               
233                self.drive_forward(self.connection, power=100, duration=3)
234               
235                # half turn in reverse
236                self.turn_in_reverse(self.connection, power=100, duration=2)
237               
238                self.drive_forward(self.connection, power=80, duration=2)
239               
240                # quarter turn left
241                self.turn_left(self.connection, power=80, duration=2)
242               
243                self.drive_forward(self.connection, power=80, duration=1)
244               
245                # half turn right
246                self.turn_right(self.connection, power=80, duration=2)
247               
248                self.drive_forward(self.connection, power=80, duration=1)
249               
250                self.drive_reverse(self.connection, power=80, duration=1)
251       
252       
253        ##################################################################
254       
255        def run(self, command):
256               
257                if (command == 'test_drive'):
258                        self.test_drive(self.connection)
259               
260                elif (command == 'drive_forward'):
261                        self.drive_forward(self.connection)
262               
263                elif (command == 'drive_reverse'):
264                        self.drive_reverse(self.connection)
265               
266                elif (command == 'turn_left'):
267                        self.turn_left(self.connection)
268               
269                elif (command == 'turn_right'):
270                        self.turn_right(self.connection)
271               
272                elif (command == 'turn_in_reverse'):
273                        self.turn_in_reverse(self.connection)
274       
275       
276        ##################################################################
277       
278        def stop(self):
279               
280                self.connection.close()
281
282
283#####################################################################
284# Functions
285#####################################################################
286
287
288#####################################################################
289# Main
290#####################################################################
291
292if __name__ == '__main__':
293       
294        # Collect default settings and command line parameters
295        device = BLUETOOTH_DEVICE
296        command = DEFAULT_RC_COMMAND
297       
298        for each in sys.argv:
299               
300                if each.startswith("--device="):
301                        device = each[ len("--device="): ]
302                elif each.startswith("--command="):
303                        command = each[ len("--command="): ]
304       
305       
306        rc = puzzlebox_brainstorms_rc(device=device, command=command, DEBUG=DEBUG)
307       
308        if rc.connection == None:
309                sys.exit()
310       
311        rc.run(rc.command)
312        rc.stop()
313
Note: See TracBrowser for help on using the repository browser.