public void Update(Hardware hardware)
{
try
{
switch (m_Procedure)
{
case SettingProcedure.none:
m_Procedure = SettingProcedure.SetHeightVector;
break;
case SettingProcedure.SetHeightVector:
// 1. base to Upper point height vector 계산
SetBasetoHeightVector(hardware.Plate_Upper.Height);
m_Procedure = SettingProcedure.SetBaseJointToUpperPlate;
break;
case SettingProcedure.SetBaseJointToUpperPlate:
// 2. baseJoint to UpperPlate Vector 위치 벡터 계산
CalculateBaseToUpperPlateVector(hardware.Plate_Base.dicOfJointVector);
m_Procedure = SettingProcedure.SetBaseJointToUpperJointVector;
break;
case SettingProcedure.SetBaseJointToUpperJointVector:
// 3. base에서 바라본 Upperjoint vector 계산(회전 행렬)
CalculateBaseToUpperJoint_Rotation(hardware.Plate_Upper.Rotation, hardware.Plate_Upper.dicOfJointVector);
m_Procedure = SettingProcedure.SetBaseToUpperJoint_Rotation;
break;
case SettingProcedure.SetBaseToUpperJoint_Rotation:
// 4. base에서 바라본 BaseJoint와 UpperJoint 사이의 벡터 계산
CalculateBaseJointToUpperJointVector();
m_Procedure = SettingProcedure.SetCompensationVector;
break;
case SettingProcedure.SetCompensationVector:
// 5.1 보상할 병진 행렬 계산(목표 위치)
CalculateTargetPostionTranslation(hardware.Plate_Upper.Position);
// 5.2 보상할 병진 행렬 계산(ToolOffset) 위치
CompensateToolOffsetVector(hardware.Plate_Upper.Rotation, hardware.Plate_Upper.ToolOffset);
m_Procedure = SettingProcedure.CalculateActuatorLengths;
break;
case SettingProcedure.CalculateActuatorLengths:
// 6. 목표 엑추에이터 값 계산
CalculateActuatorLengths();
m_Procedure = SettingProcedure.Completed;
break;
case SettingProcedure.Completed:
break;
}
}
catch (Exception e)
{
Console.WriteLine(e.ToString());
}
}
public void InitializeState()
{
m_Procedure = SettingProcedure.none;
}
