/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: 2014-04-02 20:11:38 UTC
  • mfrom: (31.1.2 bzr-project)
  • Revision ID: dan-20140402201138-5iy09kxg2pg0m4i5
Commit local changes

Show diffs side-by-side

added added

removed removed

Lines of Context:
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()