source: rc/puzzlebox_brainstorms_rc.py @ 3

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

brainstorms_rc:

  • driving functions added
  • Property svn:executable set to *
File size: 6.8 KB
Line 
1#!/usr/bin/env python
2# -*- coding: utf-8 -*-
3#
4# Puzzlebox - Brainstorms - RC
5#
6# Copyright Steven M. Castellotti (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.27
14#
15#####################################################################
16
17import sys, time
18import serial
19import jaraco.nxt
20import jaraco.nxt.messages
21
22#####################################################################
23# Globals
24#####################################################################
25
26DEBUG = 2
27
28BLUETOOTH_DEVICE='/dev/rfcomm0'
29MOTORS_MOUNTED_BACKWARDS = True
30MOTOR_PORT_RIGHT = 'a'
31MOTOR_PORT_LEFT = 'b'
32
33#####################################################################
34# Classes
35#####################################################################
36
37class puzzlebox_brainstorms_rc:
38       
39        def __init__(self, device=BLUETOOTH_DEVICE, DEBUG=DEBUG):
40               
41                self.DEBUG = DEBUG
42               
43                self.device = device
44               
45                self.connection = None
46       
47       
48        ##################################################################
49       
50        def connect_to_nxt(self, device):
51               
52                connection = jaraco.nxt.Connection(self.device)
53               
54                if self.DEBUG > 1:
55                        battery_voltage = self.get_battery_voltage(connection)
56                        print "Battery voltage:",
57                        print battery_voltage
58               
59               
60                return(connection)
61       
62       
63        ##################################################################
64       
65        def connect_to_port(self, device_port):
66       
67                if isinstance(device_port, basestring):
68                        port = getattr(jaraco.nxt.messages.OutputPort, device_port)
69               
70                assert port in jaraco.nxt.messages.OutputPort.values()
71               
72                return(port)
73       
74       
75        ##################################################################
76       
77        def get_battery_voltage(self, connection):
78               
79                cmd = jaraco.nxt.messages.GetBatteryLevel()
80                connection.send(cmd)
81               
82                response = connection.receive()
83                voltage = response.get_voltage()
84               
85                return(voltage)
86       
87       
88        ##################################################################
89       
90        def cycle_motor(self, connection, motor_port='a'):
91               
92                "Turn the motor one direction, then the other, then stop it"
93               
94                port = self.connect_to_port(motor_port)
95                cmd = jaraco.nxt.messages.SetOutputState(port, \
96                                                         motor_on=True, \
97                                                         set_power=60, \
98                                   run_state=jaraco.nxt.messages.RunState.running)
99                connection.send(cmd)
100               
101                time.sleep(2)
102               
103                cmd = jaraco.nxt.messages.SetOutputState(port, \
104                                                         motor_on=True, \
105                                                         set_power=-60, \
106                                   run_state=jaraco.nxt.messages.RunState.running)
107                connection.send(cmd)
108               
109                time.sleep(2)
110               
111                cmd = jaraco.nxt.messages.SetOutputState(port)
112                connection.send(cmd)
113       
114       
115        ##################################################################
116       
117        def drive(self, connection, left_power, right_power, duration):
118               
119                "Operate both motors, each at certain power, for a certain duration"
120               
121                left_motor = self.connect_to_port(MOTOR_PORT_LEFT)
122                right_motor = self.connect_to_port(MOTOR_PORT_RIGHT)
123               
124               
125                # If motors are mounted on the robot in reverse then all
126                # power settings need to be reversed
127                if MOTORS_MOUNTED_BACKWARDS:
128                        left_power = -left_power
129                        right_power = -right_power
130               
131                # Issue operation settings to both motors
132                cmd = jaraco.nxt.messages.SetOutputState(left_motor, \
133                                                                                                                          motor_on=True, \
134                                                                                                                          set_power=left_power, \
135                                                                 run_state=jaraco.nxt.messages.RunState.running)
136                connection.send(cmd)
137               
138                cmd = jaraco.nxt.messages.SetOutputState(right_motor, \
139                                                                                                                          motor_on=True, \
140                                                                                                                          set_power=right_power, \
141                                                                 run_state=jaraco.nxt.messages.RunState.running)
142                connection.send(cmd)
143               
144                # Continue operating motors while control program pauses
145                time.sleep(duration)
146               
147                # Issue command to stop both motors
148                cmd = jaraco.nxt.messages.SetOutputState(left_motor)
149                connection.send(cmd)
150                cmd = jaraco.nxt.messages.SetOutputState(right_motor)
151                connection.send(cmd)
152       
153       
154        ##################################################################
155       
156        def drive_forward(self, connection, power=80, duration=3):
157               
158                "Drive the robot forward at a certain speed for a certain duration"
159               
160                self.drive(connection, power, power, duration)
161       
162       
163        ##################################################################
164       
165        def turn_in_reverse(self, connection, power=80, duration=3):
166               
167                "Turn the robot clockwise while backing up at a"
168                " certain speed for a certain duration"
169               
170                left_power = -(power/3)
171                right_power = -power
172               
173                self.drive(connection, left_power, right_power, duration)
174       
175       
176        ##################################################################
177       
178        def turn_left(self, connection, power=80, duration=3):
179               
180                "Turn the robot counter-clockwise at a"
181                " certain speed for a certain duration"
182               
183                left_power = -(power/2)
184                right_power = power
185               
186                self.drive(connection, left_power, right_power, duration)
187       
188       
189        ##################################################################
190       
191        def turn_right(self, connection, power=80, duration=3):
192               
193                "Turn the robot counter-clockwise at a"
194                " certain speed for a certain duration"
195               
196                left_power = power
197                right_power = -power
198               
199                self.drive(connection, left_power, right_power, duration)
200       
201       
202        ##################################################################
203       
204        def test_drive(self, connection):
205               
206                #self.cycle_motor(self.connection, motor_port='a')
207               
208                self.drive_forward(self.connection, power=100, duration=3)
209               
210                # half turn in reverse
211                self.turn_in_reverse(self.connection, power=100, duration=2)
212               
213                self.drive_forward(self.connection, power=80, duration=2)
214               
215                # quarter turn left
216                self.turn_left(self.connection, power=80, duration=2)
217               
218                self.drive_forward(self.connection, power=80, duration=1)
219               
220                # half turn right
221                self.turn_right(self.connection, power=80, duration=2)
222               
223                self.drive_forward(self.connection, power=80, duration=1)
224       
225       
226        ##################################################################
227       
228        def run(self):
229               
230                self.connection = self.connect_to_nxt(self.device)
231               
232                self.test_drive(self.connection)
233               
234                self.connection.close()
235
236
237#####################################################################
238# Functions
239#####################################################################
240
241
242#####################################################################
243# Main
244#####################################################################
245
246if __name__ == '__main__':
247       
248        # Collect default settings and command line parameters
249        device = BLUETOOTH_DEVICE
250       
251        for each in sys.argv:
252               
253                if each.startswith("--device="):
254                        device = each[ len("--device="): ]
255       
256       
257        rc = puzzlebox_brainstorms_rc(device=device, DEBUG=DEBUG)
258       
259        rc.run()
260
Note: See TracBrowser for help on using the repository browser.