AliceWiiMotionPlus is an open source project powered by Assembla

Assembla offers free public and private SVN/Git repositories and project hosting with bug/issue tracking and collaboration tools.

alicewiimotionplus

Commit 21

User picture
  • Author: xevel
  • 2009-09-21 13:38 (over 4 years ago)

Tweak of the demo version on the soft (grawii_mplus) + it seems that there is a scale factor to apply to Z axis

Files Affected

 
2021
24
    def update(self, rotated_base):
24
    def update(self, rotated_base):
25
        self.base.axis = rotated_base[1]
25
        self.base.axis = rotated_base[1]
26
        self.base.up = rotated_base[2]
26
        self.base.up = rotated_base[2]
27
 
27
        
28
 
28
 
29
class Orientation:
29
class Orientation:
30
    raise_when_updated = []
30
    raise_when_updated = []
...
 
...
 
46
        self.orientation = self.orientation*m
46
        self.orientation = self.orientation*m
47
 
47
 
48
        axis1 = self.orientation*matrix( [[1],[0],[0]] )
48
        axis1 = self.orientation*matrix( [[1],[0],[0]] )
49
        axis2 = self.orientation*matrix( [[0],[2],[0]] )
49
        axis2 = self.orientation*matrix( [[0],[1],[0]] )
50
        axis3 = self.orientation*matrix( [[0],[0],[1]] )
50
        axis3 = self.orientation*matrix( [[0],[0],[1]] )
51
        self.base[0] = vector(axis1[0,0], axis1[1,0], axis1[2,0] )
51
        self.base[0] = vector(axis1[0,0], axis1[1,0], axis1[2,0] )
52
        self.base[1] = vector(axis2[0,0], axis2[1,0], axis2[2,0] )
52
        self.base[1] = vector(axis2[0,0], axis2[1,0], axis2[2,0] )
2021
82
        #self.data is the result of integration (here, axis + angle)
82
        #self.data is the result of integration (here, axis + angle)
83
 
83
 
84
        if self.last_input_data:
84
        if self.last_input_data:
85
 
85
            interval_duration = data.time_this_report - self.last_input_data.time_this_report
86
            interval_duration = self.last_input_data.time_this_report - data.time_this_report
 
 
87
 
86
 
88
            dt = float(interval_duration.microseconds)/1000000.0
87
            dt = float(interval_duration.microseconds)/1000000.0
 
 
88
            print data.time_this_report, dt , data.gyros
89
 
89
 
 
 
90
 
90
            rotation_values = [0]*3
91
            rotation_values = [0]*3
91
            for c in range(3):
92
            for c in range(3):
92
                val = data.gyros[c]
93
                val = data.gyros[c]
...
 
...
 
185
 
186
 
186
    def finish_calibration(self):
187
    def finish_calibration(self):
187
        self.do_process = self.interpret
188
        self.do_process = self.interpret
188
        nb = self.nb_samples
189
        nb = self.nb_samples + 1
189
        self.wmp_offsets = [ float(c)/nb for c in self.wmp_offsets ]
190
        self.wmp_offsets = [ float(c)/nb for c in self.wmp_offsets ]
190
        print "Calibration of Wmp finished :", self.wmp_offsets
191
        print "Calibration of Wmp finished :", self.wmp_offsets
191
 
192
 
...
 
...
 
221
                values[c] = (raw[c] - offsets[c]) / coeff
222
                values[c] = (raw[c] - offsets[c]) / coeff
222
                if fast[c]:
223
                if fast[c]:
223
                    values[c] = values[c] * self.wmp_amplification_coef
224
                    values[c] = values[c] * self.wmp_amplification_coef
 
 
225
            self.data.time_this_report = data.time_this_report
224
            self.raise_processed()
226
            self.raise_processed()
225
 
227
 
226
 
228
 
2021
141
        for c in range(3) :
141
        for c in range(3) :
142
            self.raw[c] = (ord(ext_report[c]) | (ord(ext_report[c + 3]) & 0xfc) << 6)
142
            self.raw[c] = (ord(ext_report[c]) | (ord(ext_report[c + 3]) & 0xfc) << 6)
143
            if self.fast[c] :
143
            if self.fast[c] :
144
                self.value[c] = (self.raw[c] - 0x1F7F) / 4.096
144
                self.value[c] = (self.raw[c] - 0x1F7F) / 4.5
145
            else :
145
            else :
146
                self.value[c] = (self.raw[c] - wiimote_gyro_calibration[c]) / 20. 
146
                self.value[c] = (self.raw[c] - wiimote_gyro_calibration[c]) / 20.
 
 
147
        #correction of y axis...
 
 
148
        self.value[0] = self.value[0] *0.985
147
        #rectification of the base
149
        #rectification of the base
148
        self.value[gyro_yaw] = -self.value[gyro_yaw]
150
        self.value[gyro_yaw] = -self.value[gyro_yaw]
149
        self.value[gyro_pitch] = -self.value[gyro_pitch]
151
        self.value[gyro_pitch] = -self.value[gyro_pitch]
...
 
...
 
158
wmp_pitch = 2
160
wmp_pitch = 2
159
wmp_x     = 2
161
wmp_x     = 2
160
 
162
 
161
# doesn't attempt a conversion to deg/s
 
 
162
class WmpParser:
 
 
163
    def parse(self, ext_report):
 
 
164
        fast  = [False] * 3
 
 
165
        raw   = [0] * 3
 
 
166
        value = [0.] * 3  # rotation speeed in deg/s
 
 
167
 
 
 
168
        #http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus but without modifications
 
 
169
        fast[wmp_yaw]   = ((ord(ext_report[3]) & 0x02) >> 1) == 0 #Yaw
 
 
170
        fast[wmp_roll]  = ((ord(ext_report[4]) & 0x02) >> 1) == 0 #Roll
 
 
171
        fast[wmp_pitch] = ((ord(ext_report[3]) & 0x01) >> 0) == 0 #Pitch
 
 
172
 
 
 
173
        for c in range(3):
 
 
174
            raw[c] = (ord(ext_report[c]) | (ord(ext_report[c + 3]) & 0xfc) << 6)
 
 
175
        
 
 
176
        return {"fast":fast, "raw":raw, "value":value}
 
 
177
 
 
 
178
 
 
 
179
 
 
 
180
class WiibrewOriginalWmpParser(WmpParser):
 
 
181
#calibration needed :
 
 
182
    wmp_offset = [0] * 3
 
 
183
 
163
 
184
#some constants
 
 
185
    # in slow mode, values are "wmp_slow_to_deg" times bigger than the values expressed in deg/s
 
 
186
    wmp_slow_to_deg = 20.
 
 
187
    # slow values are "wmp_amplification_coef" times bigger than fast value for the same rotation speed
 
 
188
    wmp_amplification_coef = 5.
 
 
189
 
164
 
190
    def parse(self, ext_report):
 
 
191
        res = WmpParser.parse(ext_report)
 
 
192
        fast = res["fast"]
 
 
193
        raw = res["raw"]
 
 
194
        value = res["value"]
 
 
195
        
 
 
196
        for c in range(3) :
 
 
197
            value[c] = (raw[c] - self.wmp_offset[c]) / self.wmp_slow_to_deg
 
 
198
            if fast[c]:
 
 
199
                value[c] = value[c] * self.wmp_amplification_coef
 
 
200
            
 
 
201
        return res
 
 
202
 
 
 
203
 
 
 
204
class MyNewWmpParser(WmpParser):
 
 
205
# calibration needed:
 
 
206
    wmp_fast_offset = [0x1F7F] * 3
 
 
207
    wmp_slow_offset = [0x1F7F] * 3
 
 
208
#some constants:
 
 
209
    # slow values are "wmp_amplification_coef" times bigger than fast value for the same rotation speed
 
 
210
    wmp_amplification_coef = 2000/440
 
 
211
    # in fast mode, values are "wmp_fast_to_deg" times bigger than the values expressed in deg/s
 
 
212
    wmp_fast_to_deg = 4.096
 
 
213
    # in slow mode, values are "wmp_fast_to_deg" times bigger than the values expressed in deg/s
 
 
214
    wmp_slow_to_deg = wmp_fast_to_deg * wmp_amplification_coef
 
 
215
 
165
 
216
    
 
 
217
    def parse(self, ext_report):
 
 
218
        res = WmpParser.parse(ext_report)
 
 
219
        fast = res["fast"]
 
 
220
        raw = res["raw"]
 
 
221
        value = res["value"]
 
 
222
        
 
 
223
        #http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus but with some
 
 
224
        #important modifications, see http://alicemotionplus.assembla.com/spaces/alicewiimotionplus
 
 
225
        
 
 
226
        for c in range(3) :
 
 
227
            if fast[c] :
 
 
228
                value[c] = (raw[c] - self.wmp_fast_offset[c]) / self.wmp_fast_to_deg
 
 
229
            else :
 
 
230
                value[c] = (raw[c] - self.wmp_slow_offset[c]) / self.wmp_slow_to_deg
 
 
231
        
 
 
232
        return res
 
 
233
        
 
 
234
        
 
 
235
 
 
 
236
class alice_wiimote:
 
 
237
    calibrations = {}
 
 
238
    values = {}
 
 
239
    
 
 
240
    def __init__(self, wmp_parser):
 
 
241
        self.wmp_parser = wmp_parser
 
 
242
    
 
 
243
    
 
 
244
    def set_calibration(self, calib_name, calib_value ):
 
 
245
        calibrations[calib_name] = calib_value
 
 
246
    
 
 
247
    def parse_button(self, button_report):
 
 
248
        pass
 
 
249
        
 
 
250
    def parse_accel(self, button_report, accel_report):
 
 
251
        pass
 
 
252
        
 
 
253
    def parse_gyro(self, ext_report):
 
 
254
        res = self.wmp_parser.parse(ext_resport)
 
 
255
        values["wmp"] = res["value"]
 
 
256
        
 
 
257
 
 
 
258
def wiisend(*data):
 
 
259
    fdout.send(reduce(lambda x, y: x + chr(y),data,''))
 
 
260
 
 
 
261
 
 
 
262
    
 
 
263
    
 
 
264
    
 
 
265
button_reading = ButtonReading()
166
button_reading = ButtonReading()
266
 
167
 
267
gyro_reading = GyroReading()
168
gyro_reading = GyroReading()
...
 
...
 
327
 
228
 
328
    def __init__(self, pos):
229
    def __init__(self, pos):
329
        self.arrow1 = arrow (pos=pos, axis=(1,0,0), shaftwidth=1, color=color.blue)
230
        self.arrow1 = arrow (pos=pos, axis=(1,0,0), shaftwidth=1, color=color.blue)
330
        self.arrow2 = arrow (pos=pos, axis=(0,3,0), shaftwidth=1, color=color.green)
231
        self.arrow2 = arrow (pos=pos, axis=(0,4,0), shaftwidth=1, color=color.green)
331
        self.arrow3 = arrow (pos=pos, axis=(0,0,1), shaftwidth=1, color=color.red)
232
        self.arrow3 = arrow (pos=pos, axis=(0,0,1), shaftwidth=1, color=color.red)
332
        self.reset_orientation()
233
        self.reset_orientation()
333
 
234
 
334
    def reset_orientation(self):
235
    def reset_orientation(self):
335
        if hasattr(self, "orientation") :
236
        #if hasattr(self, "orientation") :
336
            print self.orientation
237
        #    print self.orientation
337
        self.orientation = matrix( [[1,0,0],[0,1,0],[0,0,1]])
238
        self.orientation = matrix( [[1,0,0],[0,1,0],[0,0,1]])
338
 
239
 
339
    def apply_rotation(self, rot_mat):
240
    def apply_rotation(self, rot_mat):
...
 
...
 
341
        #renormalize(self.orientation)
242
        #renormalize(self.orientation)
342
 
243
 
343
        axis1 = self.orientation*matrix( [[1],[0],[0]] )
244
        axis1 = self.orientation*matrix( [[1],[0],[0]] )
344
        axis2 = self.orientation*matrix( [[0],[2],[0]] )
245
        axis2 = self.orientation*matrix( [[0],[4],[0]] )
345
        axis3 = self.orientation*matrix( [[0],[0],[1]] )
246
        axis3 = self.orientation*matrix( [[0],[0],[1]] )
346
        self.arrow1.axis = vector(axis1[0,0], axis1[1,0], axis1[2,0] )
247
        self.arrow1.axis = vector(axis1[0,0], axis1[1,0], axis1[2,0] )
347
        self.arrow1.up = vector(axis2[0,0], axis2[1,0], axis2[2,0] )
248
        self.arrow1.up = vector(axis2[0,0], axis2[1,0], axis2[2,0] )
...
 
...
 
358
 
259
 
359
    def __init__(self):
260
    def __init__(self):
360
        self.bases = [
261
        self.bases = [
361
            arrow_base( (    self.diam, 0              ,0) ),
262
            #arrow_base( (    self.diam, 0              ,0) ),
362
            arrow_base( ( .5*self.diam, 0.866*self.diam,0) ),
263
            #arrow_base( ( .5*self.diam, 0.866*self.diam,0) ),
363
            arrow_base( ( .5*self.diam,-0.866*self.diam,0) ),
264
            #arrow_base( ( .5*self.diam,-0.866*self.diam,0) ),
364
            arrow_base( (   -self.diam, 0              ,0) ),
265
            #arrow_base( (   -self.diam, 0              ,0) ),
365
            arrow_base( (-.5*self.diam, 0.866*self.diam,0) ),
266
            #arrow_base( (-.5*self.diam, 0.866*self.diam,0) ),
366
            arrow_base( (-.5*self.diam,-0.866*self.diam,0) ),
267
            #arrow_base( (-.5*self.diam,-0.866*self.diam,0) ),
367
            arrow_base( ( 0, 0, 0) )
268
            arrow_base( ( 0, 0, 0) )
368
            ]
269
            ]
369
    def reset_orientation(self):
270
    def reset_orientation(self):
...
 
...
 
384
        #self.bases[3].apply_rotation(axz*ay)
285
        #self.bases[3].apply_rotation(axz*ay)
385
        #self.bases[4].apply_rotation(az*ay*ax)
286
        #self.bases[4].apply_rotation(az*ay*ax)
386
        #self.bases[5].apply_rotation(ay*axz)
287
        #self.bases[5].apply_rotation(ay*axz)
387
        self.bases[6].apply_rotation(avg_matrix)
288
        self.bases[0].apply_rotation(avg_matrix)
388
 
289
 
389
class View2:
290
class View2:
390
 
291
 
...
 
...
 
420
 
321
 
421
    if button_reading.A :
322
    if button_reading.A :
422
        current_view.reset_orientation()
323
        current_view.reset_orientation()
 
 
324
 
423
 
325
 
424
    
 
 
425
    rotation_values = [0.0] * 3 #rotation angle in the wiimote's base
326
    rotation_values = [0.0] * 3 #rotation angle in the wiimote's base
426
 
327
 
427
    dt = float(interval_duration.microseconds)/1000000.0
328
    dt = float(interval_duration.microseconds)/1000000.0
428
 
329
 
429
 
330
 
430
    direct_computation = True # TODO determine if we will use it or not
331
    direct_computation = False # TODO determine if we will use it or not
431
    interpolate_speed = False
332
    interpolate_speed = False
432
 
333
 
433
    square_sum = sum( [n**2 for n in gyro_reading.value] )
334
    square_sum = sum( [n**2 for n in gyro_reading.value] )
...
 
...
 
457
        if biggest_angle_in_deg > 1.: 
358
        if biggest_angle_in_deg > 1.: 
458
            nb_parts = int(biggest_angle_in_deg / 1. )
359
            nb_parts = int(biggest_angle_in_deg / 1. )
459
            dt = dt / float(nb_parts)
360
            dt = dt / float(nb_parts)
460
        print "nbparts =",nb_parts 
361
        #print "nbparts =",nb_parts 
461
 
362
 
462
        if interpolate_speed :
363
        if interpolate_speed :
463
            var = [ (float(new[c])-float(last[c]))/float(nb_parts) for c in range(3) ]
364
            var = [ (float(new[c])-float(last[c]))/float(nb_parts) for c in range(3) ]