/elec/quadcopter

To get this branch, use:
bzr branch http://bzr.ed.am/elec/quadcopter

« back to all changes in this revision

Viewing changes to test/rc-interface/rc-interface-test

  • Committer: dan
  • Date: 2013-10-16 19:34:55 UTC
  • Revision ID: dan-20131016193455-dpdna7f6tzdxr2re
Added notes

Show diffs side-by-side

added added

removed removed

test/rc-interface/rc-interface-test
1
 
#!/usr/bin/python
2
 
#
3
 
# rc-interface-test
4
 
 
5
 
 
6
 
import pygtk
7
 
pygtk.require( '2.0' );
8
 
import gtk, serial, struct
9
 
from io import BytesIO
10
 
 
11
 
 
12
 
class MainDialogue:
13
 
 
14
 
 
15
 
        # number of channels to read
16
 
        NUM_CHANNELS = 8
17
 
 
18
 
        # number of motor channels
19
 
        NUM_MOTORS = 4
20
 
 
21
 
        # maximum channel value
22
 
        MAX_CHANNEL_VALUE = 1000
23
 
 
24
 
        # serial device to use
25
 
        SERIAL_DEVICE = '/dev/ttyUSB0'
26
 
 
27
 
 
28
 
        def __init__( self ):
29
 
                try:
30
 
                        self.link = serial.Serial( self.SERIAL_DEVICE, 9600 )
31
 
                        self.link_error = None
32
 
                except serial.serialutil.SerialException as e:
33
 
                        self.link = None
34
 
                        self.link_error = e
35
 
                self.got_one = False
36
 
                self.motor_values = [ 0 ] * self.NUM_MOTORS
37
 
                self.quit = False
38
 
                self.buffer = bytearray()
39
 
 
40
 
 
41
 
        def add_info( self, vbox, heading = False, text = '' ):
42
 
                label = gtk.Label()
43
 
                if( heading ):
44
 
                        label.set_markup( "<b>%s</b>" % text )
45
 
                else:
46
 
                        label.set_label( text )
47
 
#               label.set_justify( gtk.JUSTIFY_LEFT )
48
 
                label.set_alignment( 0, 0 )
49
 
                vbox.pack_start( label, False, False )
50
 
 
51
 
        def setup_window( self ):
52
 
 
53
 
                # create and set up window
54
 
                self.window = gtk.Window( gtk.WINDOW_TOPLEVEL )
55
 
                self.window.connect( "delete_event", self.destroy )
56
 
                self.window.connect( "destroy", self.destroy )
57
 
                self.window.set_title( "Testing rc-interface serial comms" );
58
 
                self.window.set_border_width( 30 )
59
 
                self.window.set_modal( True )
60
 
                self.window.connect( "key-press-event", self.key_press_event )
61
 
 
62
 
                hbox = gtk.HBox( False, 10 )
63
 
                self.window.add( hbox )
64
 
 
65
 
                # controls frame
66
 
                frame = gtk.Frame( "Channels" )
67
 
                frame.set_shadow_type( gtk.SHADOW_IN )
68
 
                hbox.pack_start( frame )
69
 
 
70
 
                table = gtk.Table( 2, self.NUM_CHANNELS )
71
 
                table.set_border_width( 25 )
72
 
                table.set_row_spacings( 10 )
73
 
                table.set_col_spacings( 15 )
74
 
                frame.add( table )
75
 
 
76
 
                # add channels
77
 
                self.controls = []
78
 
                for a in range( self.NUM_CHANNELS ):
79
 
 
80
 
                        label = gtk.Label( a + 1 )
81
 
                        table.attach( label, a, a + 1, 1, 2 )
82
 
 
83
 
                        adj = gtk.Adjustment( 0, 0, self.MAX_CHANNEL_VALUE, 1, 0, 0 )
84
 
                        control = gtk.VScale( adj )
85
 
                        control.set_draw_value( False )
86
 
                        control.set_inverted( True )
87
 
                        control.set_size_request( -1, 200 )
88
 
                        control.set_sensitive( False )
89
 
                        table.attach( control, a, a + 1, 0, 1 )
90
 
 
91
 
                        self.controls.append( control )
92
 
 
93
 
                # motors frame
94
 
                frame = gtk.Frame( "Motors" )
95
 
                frame.set_shadow_type( gtk.SHADOW_IN )
96
 
                hbox.pack_start( frame )
97
 
 
98
 
                table = gtk.Table( 2, self.NUM_MOTORS )
99
 
                table.set_border_width( 25 )
100
 
                table.set_row_spacings( 10 )
101
 
                table.set_col_spacings( 15 )
102
 
                frame.add( table )
103
 
 
104
 
                # add motor controls
105
 
                self.motors = []
106
 
                for a in range( self.NUM_MOTORS ):
107
 
 
108
 
                        label = gtk.Label( a + 1 )
109
 
                        table.attach( label, a, a + 1, 1, 2 )
110
 
 
111
 
                        adj = gtk.Adjustment( 0, 0, self.MAX_CHANNEL_VALUE, 1, 0, 0 )
112
 
                        control = gtk.VScale( adj )
113
 
                        control.set_draw_value( False )
114
 
                        control.set_inverted( True )
115
 
                        control.set_size_request( -1, 200 )
116
 
                        table.attach( control, a, a + 1, 0, 1 )
117
 
 
118
 
                        control.connect( "value-changed", self.on_motor_change, a )
119
 
 
120
 
                        self.motors.append( control )
121
 
 
122
 
                # info frame
123
 
                frame = gtk.Frame( "Info" )
124
 
                hbox.pack_start( frame )
125
 
 
126
 
                vbox = gtk.VBox()
127
 
                vbox.set_border_width( 10 )
128
 
                frame.add( vbox )
129
 
 
130
 
                self.add_info( vbox, True, "USB Device:" )
131
 
                self.add_info( vbox, False, self.SERIAL_DEVICE )
132
 
                self.add_info( vbox )
133
 
 
134
 
                if( self.link_error is not None ):
135
 
                        self.add_info( vbox, True, "USB Error:" )
136
 
                        label = gtk.Label( self.link_error )
137
 
                        label.set_line_wrap( True )
138
 
                        label.set_size_request( 150, -1 )
139
 
                        vbox.pack_start( label, False )
140
 
                else:
141
 
                        pass
142
 
 
143
 
                vbox.pack_start( gtk.Label( "" ) )
144
 
 
145
 
                # show it
146
 
                self.window.show_all()
147
 
 
148
 
 
149
 
        def on_motor_change( self, widget, motor ):
150
 
                self.motor_values[ motor ] = int( widget.get_value() )
151
 
                self.write_serial()
152
 
 
153
 
 
154
 
        def key_press_event( self, widget, event ):
155
 
                if event.keyval == gtk.keysyms.Escape:
156
 
                        self.window.destroy()
157
 
                else:
158
 
                        return False
159
 
 
160
 
 
161
 
        def destroy( self, widget, data = None ):
162
 
                self.quit = True
163
 
 
164
 
 
165
 
        def process_serial_data_at( self, pos ):
166
 
                for a in range( self.NUM_CHANNELS ):
167
 
                        at = 2 * a + pos
168
 
                        value = struct.unpack( "h", self.buffer[ at : at + 2 ] )[ 0 ]
169
 
 
170
 
                        # update control
171
 
                        self.controls[ a ].set_value( value )
172
 
 
173
 
 
174
 
        def read_serial( self ):
175
 
 
176
 
                # serial problem?
177
 
                if self.link is None: return
178
 
 
179
 
                # try and read
180
 
                in_waiting = self.link.inWaiting()
181
 
                if( in_waiting ):
182
 
                        self.buffer.extend( self.link.read( in_waiting ) )
183
 
 
184
 
                        # check buffer for double -1
185
 
                        got = bytearray()
186
 
                        i = 0
187
 
                        while i < len( self.buffer ):
188
 
 
189
 
                                # check foir double -1
190
 
                                if i > 0 and self.buffer[ i ] == 255 and \
191
 
                                                self.buffer[ i - 1 ] == 255:
192
 
 
193
 
                                        # if we've got enough data, process it
194
 
                                        start_index = i - ( self.NUM_CHANNELS * 2 + 1 )
195
 
                                        if start_index >= 0:
196
 
                                                self.process_serial_data_at( start_index )
197
 
 
198
 
                                        # remove everything up to and including this double -1 from
199
 
                                        # the serial buffer
200
 
                                        self.buffer = self.buffer[ i + 1 : ]
201
 
                                        i = 0
202
 
 
203
 
                                else:
204
 
                                        i += 1
205
 
 
206
 
 
207
 
        def write_serial( self ):
208
 
 
209
 
                # serial problem?
210
 
                if self.link is None: return
211
 
 
212
 
                # construct message
213
 
                bytes = bytearray()
214
 
                bytes.extend( struct.pack( "h", -1 ) )
215
 
                for i in range( self.NUM_MOTORS ):
216
 
                        bytes.extend( struct.pack( "h", self.motor_values[ i ] ) )
217
 
 
218
 
                # send
219
 
                self.link.write( bytes )
220
 
 
221
 
 
222
 
        def main( self ):
223
 
                self.setup_window()
224
 
                while not self.quit:
225
 
 
226
 
                        # update gtk
227
 
                        while gtk.events_pending():
228
 
                                gtk.main_iteration( False )
229
 
 
230
 
                        # check serial interface
231
 
                        self.read_serial()
232
 
 
233
 
 
234
 
MainDialogue().main()