void MyController::setCameraPosition(double angle, int camID) {
double xDir = sin(DEG2RAD(angle));
double zDir = cos(DEG2RAD(angle));
m_my->setCamDir(Vector3d(xDir, 0.0, zDir), camID);
printf("camera Dir converted \n");
return;
}
void MyController::onInit(InitEvent &evt)
{
m_my = getRobotObj(myname());
// 初期位置取得
m_my->getPosition(m_inipos);
//v3の追加点
// ゴミがある方向にカメラを向ける
m_my->setCamDir(Vector3d(0.0, -1.0, 1.0),1);
m_my->setCamDir(Vector3d(0.0, -1.0, 1.0),2);
// 車輪の半径と車輪間隔距離
m_radius = 10.0;
m_distance = 10.0;
m_time = 0.0;
// 車輪の半径と車輪間距離設定
m_my->setWheel(m_radius, m_distance);
m_state = 0;
srand((unsigned)time( NULL ));
// 車輪の回転速度
m_vel = 1.0; //1.0;
m_rotateVel = 0.6; //1.0;
// 関節の回転速度
m_jvel = 0.6;
m_lookObjFlg = 0.0;
// grasp初期化
m_grasp = false;
m_srv = NULL;
m_sended = false;
m_executed = false;
}
