45
22
gyro_yaw = 0
22
gyro_yaw = 0
23
gyro_roll = 1
23
gyro_roll = 1
24
gyro_pitch = 2
24
gyro_pitch = 2
 
 
25
gyro_x = 2
 
 
26
gyro_y = 1
 
 
27
gyro_z = 0
25
 
28
 
26
interval_duration = timedelta(0)
29
interval_duration = timedelta(0)
27
time_last_report = datetime.today()
30
time_last_report = datetime.today()
...
 
...
 
84
 
87
 
85
        self.Home  = (ord(button_report[1]) & 0x80) != 0
88
        self.Home  = (ord(button_report[1]) & 0x80) != 0
86
 
89
 
87
        
 
 
88
        
 
 
89
 
90
 
90
class AccelReading:
91
class AccelReading:
91
    #reading of the accelerometers on 10 bits
92
    #reading of the accelerometers on 10 bits
...
 
...
 
140
        for c in range(3) :
141
        for c in range(3) :
141
            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)
142
            if self.fast[c] :
143
            if self.fast[c] :
143
                self.value[c] = (self.raw[c] - 0x1F7F) * 5.
144
                self.value[c] = (self.raw[c] - 0x1F7F) / 4.096
144
            else :
145
            else :
145
                self.value[c] = self.raw[c] - wiimote_gyro_calibration[c]
146
                self.value[c] = self.raw[c] - wiimote_gyro_calibration[c] / 4.096 * 0.22 
 
 
147
                # 0.22 = 1/4.5454..
146
        #rectification of the base
148
        #rectification of the base
147
        self.value[gyro_yaw] = -self.value[gyro_yaw]
149
        self.value[gyro_yaw] = -self.value[gyro_yaw]
148
        self.value[gyro_pitch] = -self.value[gyro_pitch]
150
        self.value[gyro_pitch] = -self.value[gyro_pitch]
149
 
151
 
 
 
152
 
 
 
153
 
 
 
154
# for addressing data in the wmp readings
 
 
155
wmp_yaw   = 0
 
 
156
wmp_z     = 0
 
 
157
wmp_roll  = 1
 
 
158
wmp_y     = 1
 
 
159
wmp_pitch = 2
 
 
160
wmp_x     = 2
 
 
161
 
 
 
162
# doesn't attempt a conversion to °/s
 
 
163
class WmpParser:
 
 
164
    def parse(self, ext_report)
 
 
165
        fast  = [False] * 3
 
 
166
        raw   = [0] * 3
 
 
167
        value = [0.] * 3  # rotation speeed in °/s
 
 
168
 
 
 
169
        #http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus but without modifications
 
 
170
        fast[wmp_yaw]   = ((ord(ext_report[3]) & 0x02) >> 1) == 0 #Yaw
 
 
171
        fast[wmp_roll]  = ((ord(ext_report[4]) & 0x02) >> 1) == 0 #Roll
 
 
172
        fast[wmp_pitch] = ((ord(ext_report[3]) & 0x01) >> 0) == 0 #Pitch
 
 
173
 
 
 
174
        for c in range(3):
 
 
175
            raw[c] = (ord(ext_report[c]) | (ord(ext_report[c + 3]) & 0xfc) << 6)
 
 
176
 
 
 
177
        return {"fast", fast, "raw", raw, "value", value}
 
 
178
 
 
 
179
 
 
 
180
 
 
 
181
class WiibrewOriginalWmpParser(WmpParser):
 
 
182
#calibration needed :
 
 
183
    wmp_offset = [0] * 3
150
 
184
 
 
 
185
#some constants
 
 
186
    # in slow mode, values are "wmp_slow_to_deg" times bigger than the values expressed in °/s
 
 
187
    wmp_slow_to_deg = 20.
 
 
188
    # slow values are "wmp_amplification_coef" times bigger than fast value for the same rotation speed
 
 
189
    wmp_amplification_coef = 5.
 
 
190
 
 
 
191
    def parse(self, ext_report)
 
 
192
        res = WmpParser.parse(ext_report)
 
 
193
        fast = res["fast"]
 
 
194
        raw = res["raw"]
 
 
195
        value = res["value"]
 
 
196
 
 
 
197
        for c in range(3) :
 
 
198
            value[c] = (raw[c] - self.wmp_offset[c]) / self.wmp_slow_to_deg
 
 
199
            if fast[c]:
 
 
200
                value[c] = value[c] * self.wmp_amplification_coef
 
 
201
 
 
 
202
        return res
151
 
203
 
152
def rotation_to_deg_per_sec(val) : 
 
 
153
    #if abs(val) > 20 :
 
 
154
        return val/20.
 
 
155
    #return 0.0
 
 
156
 
204
 
157
def rotation_to_rad_per_sec(val):
205
class MyNewWmpParser(WmpParser):
158
    return math.radians(rotation_to_deg_per_sec(val))
206
# calibration needed:
 
 
207
    wmp_fast_offset = [0x1F7F] * 3
 
 
208
    wmp_slow_offset = [0x1F7F] * 3
 
 
209
#some constants:
 
 
210
    # slow values are "wmp_amplification_coef" times bigger than fast value for the same rotation speed
 
 
211
    wmp_amplification_coef = 2000/440
 
 
212
    # in fast mode, values are "wmp_fast_to_deg" times bigger than the values expressed in °/s
 
 
213
    wmp_fast_to_deg = 4.096
 
 
214
    # in slow mode, values are "wmp_fast_to_deg" times bigger than the values expressed in °/s
 
 
215
    wmp_slow_to_deg = wmp_fast_to_deg * wmp_amplification_coef
 
 
216
    
 
 
217
    
 
 
218
    def parse(self, ext_report)
 
 
219
        res = WmpParser.parse(ext_report)
 
 
220
        fast = res["fast"]
 
 
221
        raw = res["raw"]
 
 
222
        value = res["value"]
 
 
223
        
 
 
224
        #http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus but with some
 
 
225
        #important modifications, see http://alicemotionplus.assembla.com/spaces/alicewiimotionplus
 
 
226
        
 
 
227
        for c in range(3) :
 
 
228
            if fast[c] :
 
 
229
                value[c] = (raw[c] - self.wmp_fast_offset[c]) / self.wmp_fast_to_deg
 
 
230
            else :
 
 
231
                value[c] = (raw[c] - self.wmp_slow_offset[c]) / self.wmp_slow_to_deg
 
 
232
        
 
 
233
        return res
 
 
234
        
 
 
235
        
159
 
236
 
 
 
237
class alice_wiimote:
 
 
238
    calibrations = {}
 
 
239
    values = {}
 
 
240
 
 
 
241
    def __init__(self, wmp_parser):
 
 
242
        self.wmp_parser = wmp_parser
 
 
243
 
 
 
244
 
 
 
245
    def set_calibration(self, calib_name, calib_value ):
 
 
246
        calibrations[calib_name] = calib_value
 
 
247
 
 
 
248
    def parse_button(self, button_report):
 
 
249
        pass
 
 
250
 
 
 
251
    def parse_accel(self, button_report, accel_report):
 
 
252
        pass
 
 
253
 
 
 
254
    def parse_gyro(self, ext_report):
 
 
255
        res = self.wmp_parser.parse(ext_resport)
 
 
256
        values["wmp"] = res["value"]
 
 
257
 
160
 
258
 
161
def wiisend(*data):
259
def wiisend(*data):
162
    fdout.send(reduce(lambda x, y: x + chr(y),data,''))
260
    fdout.send(reduce(lambda x, y: x + chr(y),data,''))
...
 
...
 
342
            val = (gyro_reading.value[c] + last_gyro_reading.value[c]) / 2.
440
            val = (gyro_reading.value[c] + last_gyro_reading.value[c]) / 2.
343
            #val = last_gyro_reading.value[c]
441
            #val = last_gyro_reading.value[c]
344
            #val = gyro_reading.value[c]
442
            #val = gyro_reading.value[c]
345
            rotation_values[c] = rotation_to_rad_per_sec(val) * dt
443
            rotation_values[c] = math.radians(val) * dt
346
        m = get_rotations( -rotation_values[0], -rotation_values[1], rotation_values[2] )
444
        m = get_rotations( -rotation_values[0], -rotation_values[1], rotation_values[2] )
347
        current_view.update_rotation(m)
445
        current_view.update_rotation(m)
348
 
446