/* ------------------------
* シミュレーションループ
------------------------ */
static void simLoop(int pause)
{
/* ローカル変数の定義 */
double vx, vy, vx1, vy1, vx2, vy2, x, y;
double sup_x[2], sup_y[2];
t2 = clock();
dSpaceCollide(space, 0, &nearCallback);
GetOmniPotision(&x, &y, 0);
printf("Position(x, y) = (%lf, %lf) \r", x, y);
VectorRoute(x, y, routeX, routeY, lineRoute-2, &vx1, &vy1);
VectorObst(x, y, obstX, obstY, lineObst-1, 1.0, 1.0, 0.0, &vx2, &vy2);
vx = vx1+vx2;
vy = vy1+vy2;
/* 停留点にはまった場合(障害物回避ベクトルを切る) */
if(abs(vx)<0.2 || abs(vy)<0.2)
{
vx = vx1*2;
vy = vy1*2;
}
//ControlOmni(vx, vy, 0);
dWorldStep(world, 0.01);
dJointGroupEmpty(contactgroup);
DrawOmni();
/* 経路の表示 */
//DrawLine(routeX, routeY, 0.01, lineRoute, 1.0, 0.2, 0.2);
DrawBox();
//SetCamera(x, y-0.15, 0.45, 90.0, 0.0, 0.0);
dt = t2-t1;
/* 1000[ms]毎にロボットが移動した経路を更新 */
if(dt >1000)
{
t1 = clock();
omniX[line] = x;
omniY[line] = y;
line++;
/* ロボットの移動経路をtxtファイルに記録 */
SaveTxt("omni.txt", omniX, omniY, omniZ, line);
}
/* 自律制御による移動経路の表示 */
//DrawLine(omniX, omniY, 0.01, line, 0.2, 0.8, 0.2);
/* 補助線の表示 */
sup_x[0] = x;
sup_x[1] = x+vx1;
sup_y[0] = y;
sup_y[1] = y+vy1;
DrawLine(sup_x, sup_y, 0.4, 3, 1.0, 0.1, 0.0);
}
TStr TExpVal::GetStr() const {
PSOut SOut=TMOut::New(); TMOut& MOut=*(TMOut*)SOut();
SaveTxt(SOut);
TStr ExpValStr=TStr::LoadTxt(MOut.GetSIn());
return ExpValStr;
}
void TExpVal::SaveTxt(const PSOut& SOut) const {
TOLx Lx(SOut, TFSet()|oloCmtAlw|oloSigNum|oloCsSens);
SaveTxt(Lx);
}
void TSch::SaveTxt(const TStr& FNm) const {
PSOut SOut=PSOut(new TFOut(FNm));
TOLx Lx(SOut, TFSet()|oloFrcEoln|oloSigNum|oloCsSens|oloVarIndent);
SaveTxt(Lx);
}
