int TestPtToPtAndCoordStageMotion()
{
int iModule;
int iError;
double dX, dY, dZ;
Path vPath;
iError=InitNMCModule3Axis (iModule, vPath);
g_stage.Rotate(180.0);
g_stage.SetVel(3.0, 3.0, 3.0);
g_stage.SetAccel(1.0, 1.0, 1.0);
// g_stage.EnableAmp();
// g_stage.ResetPos();
// g_stage.SetPathStatus();
// g_stage.EnableAmp();
// g_stage.ResetPos();
// g_stage.SetPathStatus();
vPath.SetPathParams(P_60HZ, //path frequency = 30 Hz
75, //Store a minimum of 45 points in the path point buffer
15000.0, //X scale - 20000.0 counts per inch
15000.0, //Y scale - 20000.0 counts per inch
15000.0, //Z scale - 1.0 counts per inch - not used
0.175); //acceleration = 1.0 inch/second/second
// InitServoPathParams(P_30HZ,1, 2, 3);
vPath.SetOrigin(0.0, 0.0, 0.0); //set the origin to X = 0, Y = 0, Z = 0
vPath.SetFeedrate(0.175); //feedrate = 1.0 inches/second
vPath.SetTangentTolerance(10.0); //continuous path tangent tolerence = 10 degrees
g_stage.InitPathMode(vPath);
BOOL bMotion;
iError=g_stage.MoveTo(0.0, 0.000, -0.5, true);
g_stage.GetPos( dX, dY, dZ );
CreatePath1A_XOffset( dX, dY, dZ, vPath);
ExecutePath3Axis();
iError=g_stage.MoveTo(1.0, 0.000, 0.000, true);
iError=g_stage.MoveTo(1.0, 0.000, -0.5, true);
g_stage.GetPos( dX, dY, dZ );
CreatePath1A_XOffset( dX, dY, dZ, vPath );
ExecutePath3Axis();
iError=g_stage.MoveTo(-1.0, 1.000, 0.0, true);
iError=g_stage.MoveTo(-1.0, 1.000, -0.5, true);
g_stage.GetPos( dX, dY, dZ );
CreatePath1A_XOffset( dX, dY, dZ, vPath );
ExecutePath3Axis();
iError=g_stage.MoveTo(0.0, 0.000, 0.000, true);
return 0;
};
int TestCoordMotionCircleAtOrigin()
{
int iModule;
int iError;
double dX=0.0, dY=0.0, dZ=0.0;
Path vPath;
iError=InitNMCModule3Axis (iModule, vPath);
g_stage.Rotate(180.0);
g_stage.SetVel(3.0, 3.0, 3.0);
g_stage.SetAccel(1.0, 1.0, 1.0);
vPath.SetPathParams(P_60HZ, //path frequency = 30 Hz
75, //Store a minimum of 45 points in the path point buffer
15000.0, //X scale - 20000.0 counts per inch
15000.0, //Y scale - 20000.0 counts per inch
15000.0, //Z scale - 1.0 counts per inch - not used
0.175); //acceleration = 1.0 inch/second/second
vPath.SetOrigin(0.0, 0.0, 0.0); //set the origin to X = 0, Y = 0, Z = 0
vPath.SetFeedrate(0.175); //feedrate = 1.0 inches/second
vPath.SetTangentTolerance(10.0); //continuous path tangent tolerence = 10 degrees
g_stage.InitPathMode(vPath);
BOOL bMotion;
iError=g_stage.MoveTo(2.5, 0.0, 0.0, true);
g_stage.GetPos( dX, dY, dZ );
vPath.ClearSegListA(2.5, 0.0, 0.0); //Clear the segment list and set the
// starting point for the path
// at X = 0, Y = 1, Z = 0
//
//Add line and arc segments to the path module's segment list for first move
//
int res;
res = vPath.AddArcSegA( 0.0, 2.5, 0.0, //end point of arc: x=1, y=3, z=0
0.0, 0.0, 0.0, //center point of arc: x=1, y=2, z = 0
0.0, 0.0, 1.0 ); //normal vector to arc plane: x = 0, y = 0, z = -1
if (res<0) return -1;
g_stage.ExecuteCoordMotion(vPath);
vPath.ClearSegListA(0.0, 2.5, 0.0); //Clear the segment list and set the
// starting point for the path
// at X = 0, Y = 1, Z = 0
//
//Add line and arc segments to the path module's segment list for first move
//
res = vPath.AddArcSegA( -2.5, 0.0, 0.0, //end point of arc: x=1, y=3, z=0
0.0, 0.0, 0.0, //center point of arc: x=1, y=2, z = 0
0.0, 0.0, 1.0 ); //normal vector to arc plane: x = 0, y = 0, z = -1
if (res<0) return -1;
g_stage.ExecuteCoordMotion(vPath);
// ExecutePath3Axis();
// iError=g_stage.MoveTo(-1.0, 1.000, 0.0, true);
// iError=g_stage.MoveTo(-1.0, 1.000, -0.5, true);
// g_stage.GetPos( dX, dY, dZ );
// CreatePath1A_XOffset( dX, dY, dZ );
// ExecutePath3Axis();
iError=g_stage.MoveTo(0.0, 0.000, 0.000, true);
return 0;
};
int TestCoordMotionShortPath()
{
int iError;
int res;
int iModule;
Path vPath;
iError=InitNMCModule3Axis (iModule, vPath);
if (iModule < 3)
return -1;
g_stage.Rotate(180.0);
g_stage.SetVel(3.0, 3.0, 3.0);
g_stage.SetAccel(1.0, 1.0, 1.0);
vPath.SetPathParams(P_60HZ, //path frequency = 30 Hz
75, //Store a minimum of 45 points in the path point buffer
6684.57447, //X scale - 20000.0 counts per inch
6684.57447, //Y scale - 20000.0 counts per inch
2*6684.57447, //Z scale - 1.0 counts per inch - not used
1.0); //acceleration = 1.0 inch/second/second
vPath.SetOrigin(0.0, 0.0, 0.0); //set the origin to X = 0, Y = 0, Z = 0
vPath.SetFeedrate(1.0); //feedrate = 1.0 inches/second
vPath.SetTangentTolerance(10.0); //continuous path tangent tolerence = 10 degrees
g_stage.InitPathMode(vPath);
double x,y,z;
// g_stage.GetPos(x,y,z);
// InitPathParam(P_60HZ, 60,
// 6684.57447, 6684.57447, 2*6684.57447,
// 1.0,
// 1.0);
g_stage.GetPos(x,y,z);
g_stage.ResetPos();
//Clear the segment list and set the starting point for the path
// at X = 0, Y = 1, Z = 0
vPath.ClearSegListA(0,0,0);
//Add a segment to move to x=0, y=2, z=0
res = vPath.AddLineSegA(0.0, 1.0, 0.0);
if (res<0) return -1;
res = vPath.AddArcSegA( 1.0, 2.0, 0.0, //end point of arc: x=1, y=2, z=0
1.0, 1.0, 0.0, //center point of arc: x=1, y=1, z = 0
0.0, 0.0, -1.0 ); //normal vector to arc plane: x = 0, y = 0, z = -1
if (res<0) return -1;
g_stage.ExecuteCoordMotion(vPath);
vPath.ClearSegListA(1.0, 2.0, 0.0);
g_stage.GetPos(x,y,z);
res = vPath.AddLineSegA(2.0, 2.0, 0.0); //line segment endpoint: x=4, y=3, z=0
if (res<0) return -1;
res = vPath.AddArcSegA( 3.0, 1.0, 0.0, //end point of arc: x=5, y=2, z=0
2.0, 1.0, 0.0, //center point of arc: x=4, y=2, z = 0
0.0, 0.0, -1.0 ); //normal vector to arc plane: x = 0, y = 0, z = -1
if (res<0) return -1;
//line segment endpoint: x=5, y=0, z=0
res = vPath.AddLineSegA(3.0, 0.0, 0.0);
if (res<0) return -1;
//line segment endpoint: x=5, y=0, z=0
g_stage.ExecuteCoordMotion(vPath);
vPath.ClearSegListA(3.0, 0.0, 0.0);
if (res<0) return -1;
//line segment endpoint: x=5, y=0, z=0
res = vPath.AddLineSegA(0.0, 0.0, 0.0);
if (res<0) return -1;
g_stage.ExecuteCoordMotion(vPath);
ResetModules(iModule);
return 0;
};
int TestPtToPtMotionShort()
{
// NMCSERVO Servo;
int iModules;
int iError;
long lPosition;
double XPos,YPos,ZPos;
double CmXPos,CmdYPos,CmdZPos;
double dCmdXSpeed,dCmdYSpeed,dCmdZSpeed;
double dCmdXAccel,dCmdYAccel,dCmdZAccel;
double dVelocity;
double dXHome, dYHome, dZHome;
byte byAD;
iModules=g_stage.Initialize("COM4:");
SetScaling_3Axis();
SetPIDGain_3Axis();
g_stage.SetVel(3.0, 3.0, 3.0);
g_stage.SetAccel(6.0, 6.0, 6.0);
g_stage.EnableAmp();
g_stage.ResetPos();
g_stage.GetHome( dXHome, dYHome, dZHome);
BOOL bMotion;
iError=g_stage.MoveRel(0.00, 0.00, 0.15, true);
iError=g_stage.MoveRel(0.00, 0.00, 0.15, true);
iError=g_stage.MoveRel(0.00, 0.00, 0.15, true);
iError=g_stage.MoveRel(0.15, 0.00, 0.00, true);
iError=g_stage.MoveRel(0.15, 0.00, 0.00, true);
iError=g_stage.MoveRel(0.15, 0.00, 0.00, true);
iError=g_stage.MoveRel(0.00, 0.15, 0.00, true);
iError=g_stage.MoveRel(0.00, 0.15, 0.00, true);
iError=g_stage.MoveRel(0.00, 0.15, 0.00, true);
iError=g_stage.MoveTo(1.00, 1.000, 1.000, true);
g_stage.GetPos(XPos, YPos, ZPos );
if ( !((XPos < 1.02) && (XPos > 0.98)) &&
!((YPos < 1.02) && (YPos > 0.98)) &&
!((ZPos < 1.02) && (ZPos > 0.98)) )
MessageBox(NULL, "Error with Position","Error Dialog", 1);
g_stage.GetHome( dXHome, dYHome, dZHome);
g_stage.GetCmdAccel(dCmdXSpeed, dCmdYSpeed, dCmdZSpeed);
g_stage.GetCmdAccel(dCmdXAccel, dCmdYAccel, dCmdZAccel );
iError=g_stage.MoveTo(0.0, 0.0, 0.0, true);
g_stage.GetPos(XPos, YPos, ZPos );
if ( !((XPos < 0.02) && (XPos > -0.02)) &&
!((YPos < 0.02) && (YPos > -0.02)) &&
!((ZPos < 0.02) && (ZPos > -0.02)) )
MessageBox(NULL, "Error with Position","Error Dialog", 1);
iError=g_stage.MoveTo(0.000, 0.000, 0.000, true);
g_stage.GetPos(XPos, YPos, ZPos );
g_stage.GetHome( dXHome, dYHome, dZHome);
return 0;
}
int TestPtToPtMotionInPCSWithStage()
{
// NMCSERVO Servo;
int iModules;
int iError;
long lPosition;
double dXPosition,dYPosition,dZPosition;
double dCmdXPosition,dCmdYPosition,dCmdZPosition;
double dCmdXSpeed,dCmdYSpeed,dCmdZSpeed;
double dCmdXAccel,dCmdYAccel,dCmdZAccel;
double dVelocity;
double dXHome, dYHome, dZHome;
byte byAD;
iModules=g_stage.Initialize("COM3:");
SetScaling_3Axis();
SetPIDGain_3Axis();
g_stage.Rotate(180.0);
g_stage.SetVel(3.0, 3.0, 3.0);
g_stage.SetAccel(1.0, 1.0, 1.0);
g_stage.EnableAmp();
g_stage.ResetPos();
g_stage.GetCmdVel(dCmdXSpeed, dCmdYSpeed, dCmdZSpeed);
g_stage.GetSpeed(dCmdXSpeed, dCmdYSpeed, dCmdZSpeed);
g_stage.GetCmdAccel(dCmdXAccel, dCmdYAccel, dCmdZAccel );
BOOL bMotion;
iError=g_stage.MoveTo(0.0, 0.0, 0.0, true);
g_stage.GetPos(dXPosition, dYPosition, dZPosition );
double x,y,z;
x=0.5;y= 0.0;z= 0.0;
iError=g_stage.MoveTo(x,y,z, true);
g_stage.GetPos(dXPosition, dYPosition, dZPosition );
x=0.0;y= 0.5;z= 0.0;
iError=g_stage.MoveTo(x,y,z, true);
g_stage.GetPos(dXPosition, dYPosition, dZPosition );
x=-0.5;y= 0.0;z= 0.0;
iError=g_stage.MoveTo(x,y,z, true);
g_stage.GetPos(dXPosition, dYPosition, dZPosition );
x=0.0;y= -0.5;z= 0.0;
iError=g_stage.MoveTo(x,y,z, true);
g_stage.GetPos(dXPosition, dYPosition, dZPosition );
x=0.5;y= 0.5;z= 0.0;
iError=g_stage.MoveTo(x,y,z, true);
g_stage.GetPos(dXPosition, dYPosition, dZPosition );
x=-0.5;y= -0.5;z= 0.0;
iError=g_stage.MoveTo(x,y,z, true);
g_stage.GetPos(dXPosition, dYPosition, dZPosition );
x=0.5;y= -0.5;z= 0.0;
iError=g_stage.MoveTo(x,y,z, true);
g_stage.GetPos(dXPosition, dYPosition, dZPosition );
x=-0.5;y= 0.5;z= 0.0;
iError=g_stage.MoveTo(x,y,z, true);
g_stage.GetPos(dXPosition, dYPosition, dZPosition );
// iError=Servo.move(-8000,2);
// do{
// bMotion=Servo.IsInMotion();
// }
// while(bMotion);
// g_stage.GetHome( dXHome, dYHome, dZHome);
return 0;
}
//playerのHit関数
//引数:UnitBase型のポインタ
//(UnitBaseクラスを継承した子クラスの情報が扱える)
//戻り値:HRESULT型-成功かどうか
HRESULT Player::Hit(UnitBase* pTarget)
{
//ステージとの当たり判定
if (typeid(*pTarget) == typeid(Stage))
{
//pTargetをステージと判断し、stage型にキャスト
Stage* stage = (Stage*)pTarget;
//RayCastData型の変数を作る
RayCastData ray;
//レイが出る位置
ray.start = position;
ray.start.y = position.y+10;
//レイを向ける方向
ray.dir = D3DXVECTOR3(0, -1, 0);
//レイの情報を格納したアドレスを渡す
stage->GetModel()->RayCast(&ray);
//Rayが当たっているかつ
//ステージの位置Yがプレイヤー位置Y-1より大きかったら
if (ray.hit && ray.pos.y >= (position.y - 1) && ground_flg == FALSE)
{
jump_flg = TRUE;
jump_Count = 0;
ground_flg = TRUE;
position = ray.pos + D3DXVECTOR3(0, 0.5, 0);
}
else if (ground_flg == TRUE)
{
position = ray.pos + D3DXVECTOR3(0,0.5,0);
}
//目の前にステージがあるかの判定
ray.start = position;
D3DXMATRIX matR;
D3DXMatrixRotationY(&matR, D3DXToRadian(dir));
D3DXVECTOR3 rayVec = D3DXVECTOR3(0, 0, 1);
D3DXVec3TransformCoord(&rayVec, &rayVec, &matR);
ray.dir = rayVec;
stage->GetModel()->RayCast(&ray);
if (ray.hit && ray.dist < 2)
{
isWallHit = TRUE;
wallNormal = ray.normal;
}
else
{
isWallHit = FALSE;
}
ray.start = position;
ray.dir = D3DXVECTOR3(0, 0, -1);
stage->GetModel()->RayCast(&ray);
D3DXVECTOR3 v = stage->GetPos() - position;
//プレイヤーとカメラの間にステージがあるか
D3DXVECTOR3 pToCdist = camVec-position;
if (D3DXVec3Length(&v) < D3DXVec3Length(&pToCdist))
{
camVec = D3DXVECTOR3(position.x, 5, position.z);
}
}
//敵との当たり判定
if (typeid(*pTarget) == typeid(Enemy))
{
static int count = 0;
count--;
Enemy* enemy = (Enemy*)pTarget; //UnitBase型をEnemy型にキャスト
BOOL flg = item->Hit(enemy->GetPos()); //アイテムが当たったかどうか
if (flg)
{
enemy->KnockBack();
count = 100;
}
D3DXVECTOR3 v = enemy->GetPos() - position; //プレイヤーから敵に向かうベクトルを求める
if (D3DXVec3Length(&v) < 2) //ベクトルの長さが2以下だったら
{
if (count >= 0)
{
enemy->Kill();
g_gameScene = SC_CLEAR;
}
else
{
g_gameScene = SC_GAME_OVER;
}
}
}
if (typeid(*pTarget) == typeid(Invisible))
{
Invisible* invisible = (Invisible*)pTarget;
D3DXVECTOR3 inTopVec = invisible->GetPos() - position;
if (D3DXVec3Length(&inTopVec) < 1)
{
isInvisible = TRUE;
}
else
{
isInvisible = FALSE;
}
}
return S_OK;
}
