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); }