// 실질적인 발키리 기체 제어 함수
void Thread1_UpdateObject(float rotation, float leftSpeed, float rightSpeed, float sideSpeed)
{
const int addToDelay = 1;
// SET SPEED
float speed = (leftSpeed + rightSpeed) * 7.0f;
if (Mathf.Abs(leftSpeed) == Mathf.Abs(rightSpeed))
{
speed *= 2.0f;
}
/// ANIMATION -> ROLL, PITCH
{
//움직임 관련, 앞으로 갈때 기우뚱
Vector2 angle = Thread1_WalkingAnimation(leftSpeed, rightSpeed, sideSpeed);
renderedRoll = AngleConverter.LerpAngle(renderedRoll, angle.x, 0.1f);
renderedPitch = AngleConverter.LerpAngle(renderedPitch, angle.y, 0.1f);
// WRITE
basic[0].Roll = AngleConverter.ToDeviceVer1(renderedRoll); // X
basic[0].Pitch = AngleConverter.ToDeviceVer1(renderedPitch); // Y
basic[0].Heave = AngleConverter.ToDeviceVer1(bodyAngle.sy * 100.0f); // 높낮이 수정
basic[0].Delay = 1;
}
/// YAW (Z)
{
// READ
double current = AngleConverter.ToTypicalVer2(output.Yaw);
renderedYaw = AngleConverter.LerpAngle(renderedYaw, (float)current, 0.1f);
// CONTROL 회전 수정
FAngleParam param = FAngleParam.MakeAngleParam(current, 0.1f * leftSpeed, -0.1f * rightSpeed, 0.02f, addToDelay, 0);
//회전관련
// WRITE
yawEx[0].Target = AngleConverter.ToDeviceVer2(param.TargetDegree);
yawEx[0].Delay = param.Delay;
}
}
void Thread2_UpdateDevice()
{
ExtendedOutput[] temp = new ExtendedOutput[1];
while (enableThread)
{
//uint inPitch = basic[0].Pitch;
//uint inYaw = yawEx[0].Target;
//uint yawDelay = yawEx[0].Delay;
//// Call Device API
//ExtendedUpdate(basic, rollEx, pitchEx, yawEx, devOutput, devInput, temp);
//uint outPitch = temp[0].Src.Pitch * 8;
//uint outYaw = temp[0].Src.YawEx;
//output.Pitch = 0.7 * Mathf.Abs((int)inPitch - (int)outPitch) / 20000 < 0.125 ? (inPitch + outPitch) / 2 : outPitch;
//float test = AngleConverter.ModifyOutputYaw(
// (float)AngleConverter.ToTypicalVer2(inYaw),
// (int)yawDelay,
// (float)AngleConverter.ToTypicalVer2(outYaw));
//output.Yaw = AngleConverter.ToDeviceVer2(test);
uint inPitch = basic[0].Pitch;
uint inYaw = yawEx[0].Target;
uint yawDelay = yawEx[0].Delay;
// Call Device API
ExtendedUpdate(basic, rollEx, pitchEx, yawEx, devOutput, devInput, temp);
uint outPitch = temp[0].Src.Pitch * 8;
uint outYaw = temp[0].Src.YawEx;
/// 이번에 목표지점 도달시 거리 수정
// Update Tick : 1/8 sec
// Pitch Max Speed : 20000 / 0.7 sec
// Yaw Max Speed : 36000 / 6.8 sec
output.Pitch = 0.7 * Mathf.Abs((int)inPitch - (int)outPitch) / 20000 < 0.125 ?
(inPitch + outPitch) / 2 :
outPitch;
/// 원래 버전
if (version == 1)
{
output.Yaw = 6.8 * Mathf.Abs((int)inYaw - (int)outYaw) / 36000 < 0.125 ?
(inYaw + outYaw) / 2 :
outYaw;
///Debug.Log(" before = " + outYaw + "\t next = " + output.Yaw + "\t delay = " + yawDelay + "\t dir = " + ((int)output.Yaw - (int)outYaw));
}
/// 미리 회전 버전
if (version == 2)
{
// Debug.Log("WHILE IN!!!!!!!!!!!!!!!");
float test = AngleConverter.ModifyOutputYaw(
(float)AngleConverter.ToTypicalVer2(inYaw),
(int)yawDelay,
(float)AngleConverter.ToTypicalVer2(outYaw));
output.Yaw = AngleConverter.ToDeviceVer2(test);
}
}
Debug.Log(" ---- END ---- ");
}