4 | 5 | ||
---|---|---|---|
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 |