C++ (Cpp) CM730::SyncWrite Examples

Example #1

File: walk.cpp Project: jcramb/darwin

void stand_up(CM730 & cm730) {
  int n = 0;
  int param[JointData::NUMBER_OF_JOINTS * 5];
  int wGoalPos, wStartPos, wDistance;

  for (int id = JointData::ID_R_SHOULDER_PITCH; id < JointData::NUMBER_OF_JOINTS; id++) {
    wStartPos = MotionStatus::m_CurrentJoints.GetValue(id); 
    wGoalPos = Walking::GetInstance()->m_Joint.GetValue(id);
    wDistance = (wStartPos > wGoalPos) ? wStartPos - wGoalPos : wGoalPos - wStartPos;
    wDistance >>= 3;  

    if (wDistance < 8) {
      wDistance = 8;
    }

    param[n++] = id;
    param[n++] = CM730::GetLowByte(wGoalPos);
    param[n++] = CM730::GetHighByte(wGoalPos);
    param[n++] = CM730::GetLowByte(wDistance);
    param[n++] = CM730::GetHighByte(wDistance);
  } 
  cm730.SyncWrite(MX28::P_GOAL_POSITION_L, 5, JointData::NUMBER_OF_JOINTS - 1, param);
}