source: rc/puzzlebox_brainstorms_rc.py @ 8

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

turn_reverse function added

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