コード例 #1
0
ファイル: wiimote_manager.cpp プロジェクト: jieah/viewer
void
WiimoteManager::handle_event(CWiimote& wm)
{
    std::lock_guard<std::mutex> lock(m_mutex);

    char prefixString[64];
    sprintf(prefixString, "Controller [%i]: ", wm.GetID());

    if(wm.Buttons.isJustPressed(CButtons::BUTTON_UP))
    {
        //g_orientation = glm::vec3();
    }

    if(wm.Buttons.isJustPressed(CButtons::BUTTON_RIGHT))
    {
        log_info("activating gyro");
        wm.EnableMotionPlus(CWiimote::ON);
        wm.ExpansionDevice.MotionPlus.Gyroscope.SetGyroThreshold(1);
    }

    if(wm.Buttons.isJustPressed(CButtons::BUTTON_LEFT))
    {
        wm.EnableMotionPlus(CWiimote::OFF);
    }

    if(wm.Buttons.isJustPressed(CButtons::BUTTON_A))
    {
        m_accumulated = glm::vec3(0.0f, 0.0f, 0.0f);
        m_gyro_orientation = glm::quat();
        m_gyro_pitch = 0.0f;
        m_gyro_yaw   = 0.0f;
        m_gyro_roll  = 0.0f;
    }

    if(wm.isUsingACC())
    {
        // We are not using WiiCs orientation calculation as it doesn't
        // work properly when roll and ptich are in effect at the same
        // time
        glm::vec3 gravity;
        wm.Accelerometer.GetGravityVector(gravity.x, gravity.y, gravity.z);

        // convert WiiC gravity vector into OpenGL coordinate system
        std::swap(gravity.y, gravity.z);
        gravity.z = -gravity.z;

        float alpha = 0.05f;
        m_smoothed_gravity = alpha * gravity + (1.0f - alpha) * m_smoothed_gravity;
        gravity = m_smoothed_gravity;

        // calculate the roll
        float roll  = atan2f(gravity.y, gravity.x) - glm::half_pi<float>();
        glm::quat roll_rot = glm::quat(glm::vec3(0.0f, 0.0f, roll));

        // remove the roll from the gravity vector, so that pitch can be calculated properly
        glm::quat rot_undo = glm::normalize(glm::quat(glm::vec3(0.0f, 0.0f, -roll)));
        gravity = rot_undo * gravity;

        // calculate pitch
        float pitch = atan2f(gravity.z, gravity.y);
        glm::quat pitch_rot = glm::quat(glm::vec3(pitch, 0.0f, 0.0f));

        m_accel_orientation = pitch_rot * roll_rot;

        m_accel_roll  = roll;
        m_accel_pitch = pitch;

        m_yaw   = glm::yaw(m_gyro_orientation);
        m_pitch = glm::pitch(m_gyro_orientation);
        m_roll  = glm::roll(m_gyro_orientation);

        //m_accel_orientation = glm::quat(glm::vec3(0.0f, glm::radians(glm::yaw(m_gyro_orientation)), 0.0f)) * m_accel_orientation;

        //std::cout << m_yaw << std::endl;
        //printf("%8.2f %8.2f   %8.2f %8.2f %8.2f\n", glm::degrees(pitch), glm::degrees(roll), gravity.x, gravity.y, gravity.z);
    }

    // if the Motion Plus is turned on then print angles
    if(wm.isUsingMotionPlus())
    {
        float pitch, yaw, roll;
        wm.ExpansionDevice.MotionPlus.Gyroscope.GetRates(roll, pitch, yaw);

        // convert into radians
        roll  = glm::radians(roll  * 0.01f);
        pitch = glm::radians(pitch * -0.01f);
        yaw   = glm::radians(yaw   * 0.01f);

        //glm::vec3 rot_x = m_gyro_orientation * glm::vec3(1.0f, 0.0f, 0.0f);
        glm::vec3 rot = m_gyro_orientation * glm::vec3(0.0f, 0.0f, 1.0f);
        //glm::vec3 rot_z = m_gyro_orientation * glm::vec3(0.0f, 0.0f, 1.0f);

        m_gyro_pitch = m_accel_pitch;
        m_gyro_yaw   = atan2(rot.x, rot.z);
        m_gyro_roll  = m_accel_roll;

        //glm::quat pitch(glm::vec3(m_gyro_pitch, 0.0f, 0.0f));
        //glm::quat yaw(glm::vec3(0.0f, m_gyro_yaw, 0.0f));
        //glm::quat roll(glm::vec3(0.0f, 0.0f, m_gyro_roll));
        //return yaw * pitch * roll;

        m_gyro_orientation = m_gyro_orientation * glm::quat(glm::vec3(pitch, yaw, roll));

        //glm::quat qpitch(glm::vec3(m_accel_pitch, 0.0f, 0.0f));
        //glm::quat qyaw(glm::vec3(0.0f, m_gyro_yaw, 0.0f));
        //glm::quat qroll(glm::vec3(0.0f, 0.0f, m_accel_roll));
        //m_orientation = /*qyaw */ qpitch * qroll;

        //glm::vec3 angles = glm::eulerAngles(g_wiimote.get_orientation());
        //glm::vec3 accum = g_wiimote.get_accumulated();
        //printf("wiimote: %8.2f %8.2f %8.2f  --  ", glm::degrees(angles.x), glm::degrees(angles.y), glm::degrees(angles.z));
        //printf("%8.2f %8.2f %8.2f\n", glm::degrees(accum.x), glm::degrees(accum.y), glm::degrees(accum.z));
    }

    if(wm.isUsingIR())
    {
        std::vector<CIRDot>::iterator i;
        int x, y;
        int index;

        printf("%s Num IR Dots: %i\n", prefixString, wm.IR.GetNumDots());
        printf("%s IR State: %u\n", prefixString, wm.IR.GetState());

        std::vector<CIRDot>& dots = wm.IR.GetDots();

        for(index = 0, i = dots.begin(); i != dots.end(); ++index, ++i)
        {
            if((*i).isVisible())
            {
                (*i).GetCoordinate(x, y);
                printf("%s IR source %i: (%i, %i)\n", prefixString, index, x, y);

                wm.IR.GetCursorPosition(x, y);
                printf("%s IR cursor: (%i, %i)\n", prefixString, x, y);
                printf("%s IR z distance: %f\n", prefixString, wm.IR.GetDistance());
            }
        }
    }

    if (false)
    {
        printf("a.p: %8.2f a.r: %8.2f -- g.p: %8.2f g.y: %8.2f g.r: %8.2f\n",
               glm::degrees(m_accel_pitch), glm::degrees(m_accel_roll),
               glm::degrees(m_gyro_pitch), glm::degrees(m_gyro_yaw), glm::degrees(m_gyro_roll));
    }
}
コード例 #2
0
ファイル: wiimote_test.cpp プロジェクト: Airstriker/viewer
void HandleEvent(CWiimote &wm)
{
  char prefixString[64];

  if(wm.Buttons.isJustPressed(CButtons::BUTTON_MINUS))
  {
    wm.SetMotionSensingMode(CWiimote::OFF);
  }

  if(wm.Buttons.isJustPressed(CButtons::BUTTON_PLUS))
  {
    wm.SetMotionSensingMode(CWiimote::ON);
  }

  if(wm.Buttons.isJustPressed(CButtons::BUTTON_DOWN))
  {
    wm.IR.SetMode(CIR::OFF);
  }

  if(wm.Buttons.isJustPressed(CButtons::BUTTON_UP))
  {
    wm.IR.SetMode(CIR::ON);
  }

  if(wm.Buttons.isJustPressed(CButtons::BUTTON_RIGHT))
  {
    wm.EnableMotionPlus(CWiimote::ON);
  }

  if(wm.Buttons.isJustPressed(CButtons::BUTTON_LEFT))
  {
    wm.EnableMotionPlus(CWiimote::OFF);
  }

  if(wm.Buttons.isJustPressed(CButtons::BUTTON_B))
  {
    wm.ToggleRumble();
  }

  sprintf(prefixString, "Controller [%i]: ", wm.GetID());

  if(wm.Buttons.isPressed(CButtons::BUTTON_A))
  {
    printf("%s A pressed\n", prefixString);
  }

  if(wm.Buttons.isPressed(CButtons::BUTTON_B))
  {
    printf("%s B pressed\n", prefixString);
  }

  if(wm.Buttons.isPressed(CButtons::BUTTON_UP))
  {
    printf("%s Up pressed\n", prefixString);
  }

  if(wm.Buttons.isPressed(CButtons::BUTTON_DOWN))
  {
    printf("%s Down pressed\n", prefixString);
  }

  if(wm.Buttons.isPressed(CButtons::BUTTON_LEFT))
  {
    printf("%s Left pressed\n", prefixString);
  }

  if(wm.Buttons.isPressed(CButtons::BUTTON_RIGHT))
  {
    printf("%s Right pressed\n", prefixString);
  }

  if(wm.Buttons.isPressed(CButtons::BUTTON_MINUS))
  {
    printf("%s Minus pressed\n", prefixString);
  }

  if(wm.Buttons.isPressed(CButtons::BUTTON_PLUS))
  {
    printf("%s Plus pressed\n", prefixString);
  }

  if(wm.Buttons.isPressed(CButtons::BUTTON_ONE))
  {
    printf("%s One pressed\n", prefixString);
  }

  if(wm.Buttons.isPressed(CButtons::BUTTON_TWO))
  {
    printf("%s Two pressed\n", prefixString);
  }

  if(wm.Buttons.isPressed(CButtons::BUTTON_HOME))
  {
    printf("%s Home pressed\n", prefixString);
  }

  // if the accelerometer is turned on then print angles
  if(wm.isUsingACC())
  {
    float pitch, roll, yaw;
    wm.Accelerometer.GetOrientation(pitch, roll, yaw);
    printf("%s wiimote roll  = %f\n", prefixString, roll);
    printf("%s wiimote pitch = %f\n", prefixString, pitch);
    printf("%s wiimote yaw   = %f\n", prefixString, yaw);
  }

  // if the Motion Plus is turned on then print angles
  if(wm.isUsingMotionPlus()) {
    float roll_rate, pitch_rate, yaw_rate;
    wm.ExpansionDevice.MotionPlus.Gyroscope.GetRates(roll_rate,pitch_rate,yaw_rate);

    printf("%s motion plus roll rate  = %f\n", prefixString,roll_rate);
    printf("%s motion plus pitch rate = %f\n", prefixString,pitch_rate);
    printf("%s motion plus yaw rate   = %f\n", prefixString,yaw_rate);
  }

  // if(IR tracking is on then print the coordinates
  if(wm.isUsingIR())
  {
    std::vector<CIRDot>::iterator i;
    int x, y;
    int index;

    printf("%s Num IR Dots: %i\n", prefixString, wm.IR.GetNumDots());
    printf("%s IR State: %u\n", prefixString, wm.IR.GetState());

    std::vector<CIRDot>& dots = wm.IR.GetDots();

    for(index = 0, i = dots.begin(); i != dots.end(); ++index, ++i)
    {
      if((*i).isVisible())
      {
        (*i).GetCoordinate(x, y);
        printf("%s IR source %i: (%i, %i)\n", prefixString, index, x, y);

        wm.IR.GetCursorPosition(x, y);
        printf("%s IR cursor: (%i, %i)\n", prefixString, x, y);
        printf("%s IR z distance: %f\n", prefixString, wm.IR.GetDistance());
      }
    }
  }

  int exType = wm.ExpansionDevice.GetType();
  if(exType == wm.ExpansionDevice.TYPE_NUNCHUK)
  {
    float pitch, roll, yaw, a_pitch, a_roll;
    float angle, magnitude;

    CNunchuk &nc = wm.ExpansionDevice.Nunchuk;

    sprintf(prefixString, "Nunchuk [%i]: ", wm.GetID());

    if(nc.Buttons.isPressed(CNunchukButtons::BUTTON_C))
    {
      printf("%s C pressed\n", prefixString);
    }

    if(nc.Buttons.isPressed(CNunchukButtons::BUTTON_Z))
    {
      printf("%s Z pressed\n", prefixString);
    }

    nc.Accelerometer.GetOrientation(pitch, roll, yaw);
    printf("%s roll = %f\n", prefixString, roll);
    printf("%s pitch = %f\n", prefixString, pitch);
    printf("%s yaw = %f\n", prefixString, yaw);

    nc.Joystick.GetPosition(angle, magnitude);
    printf("%s joystick angle = %f\n", prefixString, angle);
    printf("%s joystick magnitude = %f\n", prefixString, magnitude);
  }
  else if(exType == wm.ExpansionDevice.TYPE_CLASSIC)
  {
    float angle, magnitude;

    CClassic &cc = wm.ExpansionDevice.Classic;

    sprintf(prefixString, "Classic [%i]: ", wm.GetID());

    if(cc.Buttons.isPressed(CClassicButtons::BUTTON_A))
    {
      printf("%s A pressed\n", prefixString);
    }

    if(cc.Buttons.isPressed(CClassicButtons::BUTTON_B))
    {
      printf("%s B pressed\n", prefixString);
    }

    if(cc.Buttons.isPressed(CClassicButtons::BUTTON_X))
    {
      printf("%s X pressed\n", prefixString);
    }

    if(cc.Buttons.isPressed(CClassicButtons::BUTTON_Y))
    {
      printf("%s Y pressed\n", prefixString);
    }

    if(cc.Buttons.isPressed(CClassicButtons::BUTTON_LEFT))
    {
      printf("%s Left pressed\n", prefixString);
    }

    if(cc.Buttons.isPressed(CClassicButtons::BUTTON_UP))
    {
      printf("%s Up pressed\n", prefixString);
    }

    if(cc.Buttons.isPressed(CClassicButtons::BUTTON_RIGHT))
    {
      printf("%s Right pressed\n", prefixString);
    }

    if(cc.Buttons.isPressed(CClassicButtons::BUTTON_DOWN))
    {
      printf("%s Down pressed\n", prefixString);
    }

    if(cc.Buttons.isPressed(CClassicButtons::BUTTON_PLUS))
    {
      printf("%s Plus pressed\n", prefixString);
    }

    if(cc.Buttons.isPressed(CClassicButtons::BUTTON_MINUS))
    {
      printf("%s Minus pressed\n", prefixString);
    }

    if(cc.Buttons.isPressed(CClassicButtons::BUTTON_HOME))
    {
      printf("%s Home pressed\n", prefixString);
    }

    if(cc.Buttons.isPressed(CClassicButtons::BUTTON_ZL))
    {
      printf("%s ZL pressed\n", prefixString);
    }

    if(cc.Buttons.isPressed(CClassicButtons::BUTTON_FULL_L))
    {
      printf("%s ZR pressed\n", prefixString);
    }

    if(cc.Buttons.isPressed(CClassicButtons::BUTTON_FULL_R))
    {
      printf("%s ZR pressed\n", prefixString);
    }

    printf("%s L button pressed = %f\n", prefixString, cc.GetLShoulderButton());
    printf("%s R button pressed = %f\n", prefixString, cc.GetRShoulderButton());

    cc.LeftJoystick.GetPosition(angle, magnitude);
    printf("%s left joystick angle = %f\n", prefixString, angle);
    printf("%s left joystick magnitude = %f\n", prefixString, magnitude);

    cc.RightJoystick.GetPosition(angle, magnitude);
    printf("%s right joystick angle = %f\n", prefixString, angle);
    printf("%s right joystick magnitude = %f\n", prefixString, magnitude);
  }
  else if(exType == wm.ExpansionDevice.TYPE_GUITAR_HERO_3)
  {
    float angle, magnitude;

    CGuitarHero3 &gh = wm.ExpansionDevice.GuitarHero3;

    sprintf(prefixString, "Guitar [%i]: ", wm.GetID());

    if(gh.Buttons.isPressed(CGH3Buttons::BUTTON_STRUM_UP))
    {
      printf("%s Strum Up pressed\n", prefixString);
    }

    if(gh.Buttons.isPressed(CGH3Buttons::BUTTON_STRUM_DOWN))
    {
      printf("%s Strum Down pressed\n", prefixString);
    }

    if(gh.Buttons.isPressed(CGH3Buttons::BUTTON_YELLOW))
    {
      printf("%s Yellow pressed\n", prefixString);
    }

    if(gh.Buttons.isPressed(CGH3Buttons::BUTTON_GREEN))
    {
      printf("%s Green pressed\n", prefixString);
    }

    if(gh.Buttons.isPressed(CGH3Buttons::BUTTON_BLUE))
    {
      printf("%s Blue pressed\n", prefixString);
    }

    if(gh.Buttons.isPressed(CGH3Buttons::BUTTON_RED))
    {
      printf("%s Red pressed\n", prefixString);
    }

    if(gh.Buttons.isPressed(CGH3Buttons::BUTTON_ORANGE))
    {
      printf("%s Orange pressed\n", prefixString);
    }

    if(gh.Buttons.isPressed(CGH3Buttons::BUTTON_PLUS))
    {
      printf("%s Plus pressed\n", prefixString);
    }

    if(gh.Buttons.isPressed(CGH3Buttons::BUTTON_MINUS))
    {
      printf("%s Minus pressed\n", prefixString);
    }

    printf("%s whammy bar = %f\n", prefixString, gh.GetWhammyBar());

    gh.Joystick.GetPosition(angle, magnitude);
    printf("%s joystick angle = %f\n", prefixString, angle);
    printf("%s joystick magnitude = %f\n", prefixString, magnitude);
  }
  else if(exType == wm.ExpansionDevice.TYPE_BALANCE_BOARD) 
  {
    CBalanceBoard &bb = wm.ExpansionDevice.BalanceBoard;
    float total, topLeft, topRight, bottomLeft, bottomRight;
		
    bb.WeightSensor.GetWeight(total, topLeft, topRight, bottomLeft, bottomRight);
    printf("balance board top left weight: %f\n", topLeft);
    printf("balance board top right weight: %f\n", topRight);
    printf("balance board bottom left weight: %f\n", bottomLeft);
    printf("balance board bottom right weight: %f\n", bottomRight);
    printf("balance board total weight: %f\n", total);
  }
}