101 lines
3.5 KiB
C#
101 lines
3.5 KiB
C#
using Aurora;
|
|
using Aurora.Api;
|
|
using Aurora.Api.Devices;
|
|
using VisionCommon;
|
|
using System;
|
|
using System.Reactive.Linq;
|
|
|
|
namespace VisionDirector {
|
|
/// <summary>
|
|
/// 모터의 좌표계를 관리한다.
|
|
/// </summary>
|
|
public abstract class MotionDirector : IDisposable {
|
|
protected readonly object _lock = new object();
|
|
protected Logger _logger;
|
|
protected SubSysInfo _subSysInfo;
|
|
protected CalRecipe _calRecipe;
|
|
protected DirectorRecipe _directorRecipe;
|
|
private IDisposable _EventDisposable;
|
|
|
|
/// <summary>
|
|
/// 생성자
|
|
/// </summary>
|
|
/// <param name="subSysInfo">SubSysInfo</param>
|
|
protected MotionDirector(SubSysInfo subSysInfo) {
|
|
_subSysInfo = subSysInfo;
|
|
UpdateRecipe();
|
|
SetObservable();
|
|
}
|
|
|
|
private void SetObservable() {
|
|
lock (_lock) {
|
|
if (VisionEnv.AuroraEnv != null) {
|
|
if (_EventDisposable == null) {
|
|
var observable = VisionEventId.GetRecipeUpdateObservable();
|
|
_EventDisposable = observable.Subscribe(_ => UpdateRecipe());
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 레시피 업데이트
|
|
/// </summary>
|
|
protected abstract void UpdateRecipe();
|
|
|
|
/// <summary>
|
|
/// 자재 기준의 Position을 모터 기준 Position으로 변환
|
|
/// </summary>
|
|
/// <param name="position">자재 좌표계의 Position</param>
|
|
/// <returns>모터 좌표계의 Position</returns>
|
|
public Position ApplyDirection(Position position) {
|
|
lock (_lock) {
|
|
var motionPoint = ApplyDirection(new StaticPoint(position.X, position.Y));
|
|
return new Position() {
|
|
X = motionPoint.X,
|
|
Y = motionPoint.Y,
|
|
T = ApplyDirectionT(position.T),
|
|
Z = ApplyDirectionZ(position.Z)
|
|
};
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 자재 기준의 DoublePoint를 모터 기준 DoublePoint로 변환
|
|
/// </summary>
|
|
/// <param name="point">자재 좌표계의 DoublePoint</param>
|
|
/// <returns>모터 좌표계의 DoublePoint</returns>
|
|
public DoublePoint ApplyDirection(DoublePoint point) {
|
|
lock (_lock) {
|
|
return CoordSystem.ApplyDirection(new MotionPoint(point.X, point.Y), _calRecipe.CoordSystem, _directorRecipe.CoordSystem);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 자재 기준의 T축 방향을 모터 기준 T축 방향으로 변환
|
|
/// </summary>
|
|
/// <param name="deltaT">자재 좌표계의 T축 방향</param>
|
|
/// <returns>모터 좌표계의 T축 방향</returns>
|
|
public double ApplyDirectionT(double deltaT) {
|
|
lock (_lock) {
|
|
return CoordSystem.ApplyDirectionT(deltaT, _directorRecipe.CoordSystem);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 자재 기준의 Z축 방향을 모터 기준 Z축 방향으로 변환
|
|
/// </summary>
|
|
/// <param name="deltaZ">자재 좌표계의 Z축 방향</param>
|
|
/// <returns>모터 좌표계의 Z축 방향</returns>
|
|
public double ApplyDirectionZ(double deltaZ) {
|
|
lock (_lock) {
|
|
return CoordSystem.ApplyDirectionZ(deltaZ, _directorRecipe.CoordSystem);
|
|
}
|
|
}
|
|
|
|
public void Dispose() {
|
|
_EventDisposable?.Dispose();
|
|
}
|
|
}
|
|
}
|