Revision 33c91ff2
| devices/PowerManagement/userthread.cpp | ||
|---|---|---|
| 66 | 66 |
float deadzone; |
| 67 | 67 |
char bt_address[18] = {'\0'};
|
| 68 | 68 |
float wiimoteCalib[3] = {0.0f};
|
| 69 |
uint8_t principal_axis = 1; |
|
| 70 |
int8_t axis_direction = -1; |
|
| 69 | 71 |
|
| 70 | 72 |
uint32_t constexpr maxTranslation = 500e3; |
| 71 | 73 |
uint32_t constexpr maxRotation = 3.1415927f * 1000000.0f * 2.0f; |
| ... | ... | |
| 283 | 285 |
|
| 284 | 286 |
// calibrate accelerometer offset |
| 285 | 287 |
if (wii_steering::wiimote.getButtons()->home) {
|
| 288 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "%f | %f | %f\n", wiimoteAcc[0], wiimoteAcc[1], wiimoteAcc[2]); |
|
| 289 |
|
|
| 290 |
// detect principal axis |
|
| 291 |
if (std::fabs(wiimoteAcc[0]) > std::fabs(wiimoteAcc[1]) && std::fabs(wiimoteAcc[0]) > std::fabs(wiimoteAcc[2])) {
|
|
| 292 |
wii_steering::principal_axis = 0; |
|
| 293 |
} else if (std::fabs(wiimoteAcc[1]) > std::fabs(wiimoteAcc[0]) && std::fabs(wiimoteAcc[1]) > std::fabs(wiimoteAcc[2])) {
|
|
| 294 |
wii_steering::principal_axis = 1; |
|
| 295 |
} else if (std::fabs(wiimoteAcc[2]) > std::fabs(wiimoteAcc[0]) && std::fabs(wiimoteAcc[2]) > std::fabs(wiimoteAcc[1])) {
|
|
| 296 |
wii_steering::principal_axis = 2; |
|
| 297 |
} |
|
| 298 |
wii_steering::axis_direction = (wiimoteAcc[wii_steering::principal_axis] >= 0) ? 1 : -1; |
|
| 299 |
|
|
| 300 |
// get calibration offset |
|
| 286 | 301 |
wii_steering::wiimoteCalib[0] = wiimoteAcc[0]; |
| 287 |
wii_steering::wiimoteCalib[1] = wiimoteAcc[1] + 100.0f;
|
|
| 302 |
wii_steering::wiimoteCalib[1] = wiimoteAcc[1]; |
|
| 288 | 303 |
wii_steering::wiimoteCalib[2] = wiimoteAcc[2]; |
| 304 |
wii_steering::wiimoteCalib[wii_steering::principal_axis] += -100.0f * wii_steering::axis_direction; |
|
| 289 | 305 |
|
| 306 |
// print information |
|
| 290 | 307 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "accelerometer calibrated:\n"); |
| 308 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "\tprincipal axis: %c\n", (wii_steering::principal_axis == 0) ? 'X' : (wii_steering::principal_axis == 1) ? 'Y' : 'Z'); |
|
| 291 | 309 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "\tX = %d\n", (int32_t)wii_steering::wiimoteCalib[0]); |
| 292 | 310 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "\tY = %d\n", (int32_t)wii_steering::wiimoteCalib[1]); |
| 293 | 311 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "\tZ = %d\n", (int32_t)wii_steering::wiimoteCalib[2]); |
| ... | ... | |
| 327 | 345 |
} |
| 328 | 346 |
|
| 329 | 347 |
// only move when A is pressed |
| 330 |
if (wii_steering::wiimote.getButtons()->A) {
|
|
| 348 |
if (wii_steering::wiimote.getButtons()->A || wii_steering::wiimote.getButtons()->B) {
|
|
| 331 | 349 |
// set kinematic relaive to maximum speeds |
| 332 |
kinematic.x = wii_steering::maxTranslation * wiimoteAcc[2]; |
|
| 333 |
kinematic.w_z = wii_steering::maxRotation * wiimoteAcc[0] * ((wiimoteAcc[2] < 0.0f) ? 1.0f : -1.0f); |
|
| 350 |
switch (wii_steering::principal_axis) {
|
|
| 351 |
case 1: |
|
| 352 |
if (wii_steering::axis_direction == -1) {
|
|
| 353 |
kinematic.x = wii_steering::maxTranslation * wiimoteAcc[2]; |
|
| 354 |
kinematic.w_z = wii_steering::maxRotation * wiimoteAcc[0] * ((wiimoteAcc[2] < 0.0f) ? 1.0f : -1.0f); |
|
| 355 |
break; |
|
| 356 |
} |
|
| 357 |
case 2: |
|
| 358 |
if (wii_steering::axis_direction == 1) {
|
|
| 359 |
kinematic.x = wii_steering::maxTranslation * wiimoteAcc[1]; |
|
| 360 |
kinematic.w_z = wii_steering::maxRotation * wiimoteAcc[0] * ((wiimoteAcc[1] < 0.0f) ? 1.0f : -1.0f); |
|
| 361 |
break; |
|
| 362 |
} |
|
| 363 |
default: |
|
| 364 |
kinematic = {0, 0, 0, 0, 0, 0};
|
|
| 365 |
break; |
|
| 366 |
} |
|
| 334 | 367 |
} else {
|
| 335 | 368 |
kinematic = {0, 0, 0, 0, 0, 0};
|
| 336 | 369 |
} |
Also available in: Unified diff