238 lines
11 KiB
C#
238 lines
11 KiB
C#
/*
|
|
* Copyright (c) 2025 LG Electronics Inc.
|
|
* SPDX-License-Identifier: LicenseRef-LGE-Proprietary
|
|
*/
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using VisionCommon;
|
|
|
|
namespace VisionSequence.Setup {
|
|
/// <summary>
|
|
/// 캘리브레이션 플랜
|
|
/// </summary>
|
|
public class CalPlan {
|
|
private const int FIRST_STEP = 0;
|
|
private const int SECOND_STEP = 1;
|
|
private const int BACKLASH_STEP = 2;
|
|
|
|
/// <summary>X, Y축 수평 이동 횟수</summary>
|
|
public int _MoveCalStep { get; private set; }
|
|
/// <summary>T축 회전 이동 횟수</summary>
|
|
public int _MoveThetaStep { get; private set; }
|
|
/// <summary>현재까지 이동 횟수</summary>
|
|
public int _CurrentStep { get; private set; }
|
|
/// <summary>X, Y축으로 1회 이동 거리</summary>
|
|
public double _AxisCalLength { get; private set; }
|
|
/// <summary>T축으로 1회 회전 각도(Degree)</summary>
|
|
public double _ThetaCalDegree { get; private set; }
|
|
|
|
private int _CalXaxisStep { get; set; }
|
|
private int _CalYaxisStep { get; set; }
|
|
private int _CalTaxisStep { get; set; }
|
|
private Dictionary<string, object> Plan = new Dictionary<string, object>();
|
|
|
|
/// <summary>
|
|
/// 생성자
|
|
/// </summary>
|
|
/// <param name="moveStep">X, Y축 수평 이동 횟수</param>
|
|
/// <param name="axisLength">X, Y축으로 1회 이동 거리</param>
|
|
/// <param name="thetaStep">T축 회전 이동 횟수</param>
|
|
/// <param name="thetaDegree">T축으로 1회 회전 각도(Degree)</param>
|
|
public CalPlan(int moveStep, double axisLength, int thetaStep, double thetaDegree) {
|
|
_MoveCalStep = moveStep;
|
|
_MoveThetaStep = thetaStep;
|
|
_AxisCalLength = axisLength;
|
|
_ThetaCalDegree = thetaDegree;
|
|
_CalXaxisStep = moveStep + BACKLASH_STEP;
|
|
_CalYaxisStep = moveStep + BACKLASH_STEP;
|
|
_CalTaxisStep = thetaStep + BACKLASH_STEP;
|
|
_CurrentStep = 1;
|
|
}
|
|
|
|
private void MakeXaxisCalPlan(string axis, string actionType) {
|
|
if (_CalXaxisStep == BACKLASH_STEP) {
|
|
return;
|
|
}
|
|
else {
|
|
MakeAxisCalPlan(_CalXaxisStep, _AxisCalLength, axis, actionType);
|
|
}
|
|
}
|
|
|
|
private void MakeYaxisCalPlan(string axis, string actionType) {
|
|
if (_CalYaxisStep == BACKLASH_STEP) {
|
|
return;
|
|
}
|
|
else {
|
|
MakeAxisCalPlan(_CalYaxisStep, _AxisCalLength, axis, actionType);
|
|
}
|
|
}
|
|
|
|
private void MakeTaxisCalPlan(string axis, string actionType) {
|
|
if (_CalTaxisStep == BACKLASH_STEP) {
|
|
return;
|
|
}
|
|
|
|
MakeAxisCalPlan(_CalTaxisStep, _ThetaCalDegree, axis, actionType);
|
|
}
|
|
|
|
private void MakeAxisCalPlan(int calAxisStep, double calAxisLength, string moveAxis, string actionType) {
|
|
string MoveAxis = moveAxis.Remove(1);
|
|
double CalAxisLength = calAxisLength;
|
|
double BackLash = -calAxisLength;
|
|
|
|
try {
|
|
for (int i = 0; i <= calAxisStep; i++) {
|
|
Dictionary<string, object> CalDistance = new Dictionary<string, object>();
|
|
Dictionary<string, object> CalMove = new Dictionary<string, object>();
|
|
Dictionary<string, object> CalAction = new Dictionary<string, object>();
|
|
Dictionary<string, object> CalAxis = new Dictionary<string, object>();
|
|
if (i == FIRST_STEP) {
|
|
if (MoveAxis == Constants.X) {
|
|
double startPosition = CalculateStartPosition(_MoveCalStep, CalAxisLength) + BackLash;
|
|
CalDistance.Add(Constants.X, startPosition.ToString());
|
|
CalDistance.Add(Constants.Y, "0");
|
|
CalDistance.Add(Constants.T, "0");
|
|
}
|
|
else if (MoveAxis == Constants.Y) {
|
|
double startPosition = CalculateStartPosition(_MoveCalStep, CalAxisLength) + BackLash;
|
|
CalDistance.Add(Constants.X, "0");
|
|
CalDistance.Add(Constants.Y, startPosition.ToString());
|
|
CalDistance.Add(Constants.T, "0");
|
|
}
|
|
else if (MoveAxis == Constants.T) {
|
|
double startPosition = CalculateStartPosition(_MoveThetaStep, CalAxisLength) + BackLash;
|
|
CalDistance.Add(Constants.X, "0");
|
|
CalDistance.Add(Constants.Y, "0");
|
|
CalDistance.Add(Constants.T, startPosition.ToString());
|
|
}
|
|
else {
|
|
Console.WriteLine("Axis Error");
|
|
}
|
|
CalMove.Add(Constants.BACKLASH, CalDistance);
|
|
CalAxis.Add(moveAxis, CalMove);
|
|
CalAction.Add(actionType, CalAxis);
|
|
Plan.Add(_CurrentStep.ToString(), CalAction);
|
|
}
|
|
else if (i == SECOND_STEP) {
|
|
if (MoveAxis == Constants.X) {
|
|
CalDistance.Add(Constants.X, (-BackLash).ToString());
|
|
CalDistance.Add(Constants.Y, "0");
|
|
CalDistance.Add(Constants.T, "0");
|
|
}
|
|
else if (MoveAxis == Constants.Y) {
|
|
CalDistance.Add(Constants.X, "0");
|
|
CalDistance.Add(Constants.Y, (-BackLash).ToString());
|
|
CalDistance.Add(Constants.T, "0");
|
|
}
|
|
else if (MoveAxis == Constants.T) {
|
|
CalDistance.Add(Constants.X, "0");
|
|
CalDistance.Add(Constants.Y, "0");
|
|
CalDistance.Add(Constants.T, (-BackLash).ToString());
|
|
}
|
|
else {
|
|
Console.WriteLine("Axis Error");
|
|
}
|
|
CalMove.Add(Constants.MOVE, CalDistance);
|
|
CalAxis.Add(moveAxis, CalMove);
|
|
CalAction.Add(actionType, CalAxis);
|
|
Plan.Add(_CurrentStep.ToString(), CalAction);
|
|
}
|
|
else if (i == calAxisStep) {
|
|
if (MoveAxis == Constants.X) {
|
|
double measurePosition = CalculateMeasurePosition(_MoveCalStep, CalAxisLength);
|
|
CalDistance.Add(Constants.X, measurePosition.ToString());
|
|
CalDistance.Add(Constants.Y, "0");
|
|
CalDistance.Add(Constants.T, "0");
|
|
}
|
|
else if (MoveAxis == Constants.Y) {
|
|
double measurePosition = CalculateMeasurePosition(_MoveCalStep, CalAxisLength);
|
|
CalDistance.Add(Constants.X, "0");
|
|
CalDistance.Add(Constants.Y, measurePosition.ToString());
|
|
CalDistance.Add(Constants.T, "0");
|
|
}
|
|
else if (MoveAxis == Constants.T) {
|
|
double measurePosition = CalculateMeasurePosition(_MoveThetaStep, CalAxisLength);
|
|
CalDistance.Add(Constants.X, "0");
|
|
CalDistance.Add(Constants.Y, "0");
|
|
CalDistance.Add(Constants.T, measurePosition.ToString());
|
|
}
|
|
else {
|
|
Console.WriteLine("Axis Error");
|
|
}
|
|
CalMove.Add(Constants.CALCULATION, CalDistance);
|
|
CalAxis.Add(moveAxis, CalMove);
|
|
CalAction.Add(actionType, CalAxis);
|
|
Plan.Add(_CurrentStep.ToString(), CalAction);
|
|
}
|
|
else {
|
|
if (MoveAxis == Constants.X) {
|
|
CalDistance.Add(Constants.X, CalAxisLength.ToString());
|
|
CalDistance.Add(Constants.Y, "0");
|
|
CalDistance.Add(Constants.T, "0");
|
|
}
|
|
else if (MoveAxis == Constants.Y) {
|
|
CalDistance.Add(Constants.X, "0");
|
|
CalDistance.Add(Constants.Y, CalAxisLength.ToString());
|
|
CalDistance.Add(Constants.T, "0");
|
|
}
|
|
else if (MoveAxis == Constants.T) {
|
|
CalDistance.Add(Constants.X, "0");
|
|
CalDistance.Add(Constants.Y, "0");
|
|
CalDistance.Add(Constants.T, CalAxisLength.ToString());
|
|
}
|
|
else {
|
|
Console.WriteLine("Axis Error");
|
|
}
|
|
|
|
CalMove.Add(Constants.MOVE, CalDistance);
|
|
CalAxis.Add(moveAxis, CalMove);
|
|
CalAction.Add(actionType, CalAxis);
|
|
Plan.Add(_CurrentStep.ToString(), CalAction);
|
|
}
|
|
_CurrentStep++;
|
|
}
|
|
}
|
|
catch (Exception e) {
|
|
Console.WriteLine(e.ToString());
|
|
}
|
|
}
|
|
|
|
private double CalculateStartPosition(int calAxisStep, double calAxisLength) {
|
|
int stepsToStartPos = (calAxisStep + 1) / 2;
|
|
int directionToStartPos = -1;
|
|
|
|
return (calAxisLength * stepsToStartPos * directionToStartPos);
|
|
}
|
|
|
|
private double CalculateMeasurePosition(int calAxisStep, double calAxisLength) {
|
|
int stepsToMeasurePos = calAxisStep / 2;
|
|
return (stepsToMeasurePos * calAxisLength * (-1));
|
|
}
|
|
|
|
/// <summary>
|
|
/// 캘리브레이션 플랜을 생성한다.
|
|
/// </summary>
|
|
/// <param name="isCalPixelExist">픽셀 사이즈 측정 플래그</param>
|
|
/// <param name="isX">X축 캘리브레이션 사용 플래그</param>
|
|
/// <param name="isY">Y축 캘리브레이션 사용 플래그</param>
|
|
/// <param name="isT">T축 캘리브레이션 사용 플래그</param>
|
|
/// <returns></returns>
|
|
public Dictionary<string, object> MakeCalPlan(bool isCalPixelExist, bool isX, bool isY, bool isT) {
|
|
if (isCalPixelExist) {
|
|
if (isX) {
|
|
MakeXaxisCalPlan(Constants.X_PLUS, Constants.X_CAL);
|
|
}
|
|
if (isY) {
|
|
MakeYaxisCalPlan(Constants.Y_PLUS, Constants.Y_CAL);
|
|
}
|
|
}
|
|
if (isT) {
|
|
MakeTaxisCalPlan(Constants.T_PLUS, Constants.THETA_CAL);
|
|
}
|
|
|
|
return Plan;
|
|
}
|
|
}
|
|
}
|