source: rc/puzzlebox_brainstorms_rc.py @ 7

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

all driving commands added to client and server

  • Property svn:executable set to *
File size: 7.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# 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=3):
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 turn_in_reverse(self, connection, power=80, duration=3):
172               
173                "Turn the robot clockwise while backing up at a"
174                " certain speed for a certain duration"
175               
176                left_power = -(power/3)
177                right_power = -power
178               
179                self.drive(connection, left_power, right_power, duration)
180       
181       
182        ##################################################################
183       
184        def turn_left(self, connection, power=80, duration=3):
185               
186                "Turn the robot counter-clockwise at a"
187                " certain speed for a certain duration"
188               
189                left_power = -(power/2)
190                right_power = power
191               
192                self.drive(connection, left_power, right_power, duration)
193       
194       
195        ##################################################################
196       
197        def turn_right(self, connection, power=80, duration=3):
198               
199                "Turn the robot counter-clockwise at a"
200                " certain speed for a certain duration"
201               
202                left_power = power
203                right_power = -power
204               
205                self.drive(connection, left_power, right_power, duration)
206       
207       
208        ##################################################################
209       
210        def test_drive(self, connection):
211               
212                #self.cycle_motor(self.connection, motor_port='a')
213               
214                self.drive_forward(self.connection, power=100, duration=3)
215               
216                # half turn in reverse
217                self.turn_in_reverse(self.connection, power=100, duration=2)
218               
219                self.drive_forward(self.connection, power=80, duration=2)
220               
221                # quarter turn left
222                self.turn_left(self.connection, power=80, duration=2)
223               
224                self.drive_forward(self.connection, power=80, duration=1)
225               
226                # half turn right
227                self.turn_right(self.connection, power=80, duration=2)
228               
229                self.drive_forward(self.connection, power=80, duration=1)
230       
231       
232        ##################################################################
233       
234        def run(self, command):
235               
236                if (command == 'test_drive'):
237                        self.test_drive(self.connection)
238               
239                elif (command == 'drive_forward'):
240                        self.drive_forward(self.connection)
241               
242                elif (command == 'turn_in_reverse'):
243                        self.turn_in_reverse(self.connection)
244               
245                elif (command == 'turn_left'):
246                        self.turn_left(self.connection)
247               
248                elif (command == 'turn_right'):
249                        self.turn_right(self.connection)
250       
251       
252        ##################################################################
253       
254        def stop(self):
255               
256                self.connection.close()
257
258
259#####################################################################
260# Functions
261#####################################################################
262
263
264#####################################################################
265# Main
266#####################################################################
267
268if __name__ == '__main__':
269       
270        # Collect default settings and command line parameters
271        device = BLUETOOTH_DEVICE
272       
273        for each in sys.argv:
274               
275                if each.startswith("--device="):
276                        device = each[ len("--device="): ]
277                elif each.startswith("--command="):
278                        device = each[ len("--command="): ]
279       
280       
281        rc = puzzlebox_brainstorms_rc(device=device, command=command, DEBUG=DEBUG)
282       
283        rc.run(rc.command)
284        rc.stop()
285
Note: See TracBrowser for help on using the repository browser.