Esempio n. 1
0
/* ------------------------
* シミュレーションループ
------------------------ */
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);
}
Esempio n. 2
0
File: exp.cpp Progetto: Accio/snap
TStr TExpVal::GetStr() const {
  PSOut SOut=TMOut::New(); TMOut& MOut=*(TMOut*)SOut();
  SaveTxt(SOut);
  TStr ExpValStr=TStr::LoadTxt(MOut.GetSIn());
  return ExpValStr;
}
Esempio n. 3
0
File: exp.cpp Progetto: Accio/snap
void TExpVal::SaveTxt(const PSOut& SOut) const {
  TOLx Lx(SOut, TFSet()|oloCmtAlw|oloSigNum|oloCsSens);
  SaveTxt(Lx);
}
Esempio n. 4
0
void TSch::SaveTxt(const TStr& FNm) const {
  PSOut SOut=PSOut(new TFOut(FNm));
  TOLx Lx(SOut, TFSet()|oloFrcEoln|oloSigNum|oloCsSens|oloVarIndent);
  SaveTxt(Lx);
}