| 20 | 21 | ||
|---|---|---|---|
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) ] |