485 lines
14 KiB
C#
485 lines
14 KiB
C#
using Aurora.Api;
|
|
using Aurora.Api.Devices;
|
|
using Hermes;
|
|
using PropertyTools.DataAnnotations;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Threading;
|
|
|
|
namespace AuroraMotor
|
|
{
|
|
public enum eHomingType
|
|
{
|
|
mode37 = 37,
|
|
}
|
|
|
|
[Obsolete("고정부 Motor 클래스 사용중단. AuroraMotor 상속해서 쓰세요")]
|
|
public class MotorMock : Motor
|
|
{
|
|
protected Logger LOG = new Logger("MotorMock");
|
|
|
|
public class Config : BaseConfig
|
|
{
|
|
// 왜 상수지? 음수 범위는 어떻게 ??
|
|
public const double AXIS_VEL_RANGE_FROM = 1;
|
|
public const double AXIS_VEL_RANGE_TO = 1000;
|
|
|
|
|
|
[SelectorStyle(SelectorStyle.ComboBox)]
|
|
[Category("Homing mode")]
|
|
public eHomingType HomingMode { get; set; } = eHomingType.mode37;
|
|
|
|
// 단위가 뭘까? 단위 표기가 없네...
|
|
[Category("Working Move Velocity")]
|
|
public double AXIS_MOVE_VEL { get; set; } = 300;
|
|
public double AXIS_MOVE_ACC { get; set; } = 200;
|
|
// % 단위인가 ??
|
|
[Category("Jog Speed")]
|
|
public double JOG_HIGH_VEL { get; set; } = 100;
|
|
public double JOG_MID_VEL { get; set; } = 50;
|
|
public double JOG_LOW_VEL { get; set; } = 5;
|
|
// [Category("Motion Pos Limit")]
|
|
//public double AXIS_POS_LIMIT { get; set; } = 900;
|
|
//public double AXIS_NEG_LIMIT { get; set; } = 0;
|
|
[Category("Axis Overload")]
|
|
public double AXIS_OVERLOAD { get; set; } = 100;
|
|
//---------------------------------------------------------
|
|
// 위치 Inposition Gap
|
|
//---------------------------------------------------------
|
|
public double INPOS_GAP { get; set; } = 0.01;
|
|
public Range<double> AXIS_VEL_RANGE { get; set; } = (-1000, 1000);
|
|
public Range<double> AXIS_ACC_RANGE { get; set; } = (-500, 500);
|
|
|
|
//public Range<double> MoveRange { get; set; } = (-1000, 1000);
|
|
|
|
}
|
|
|
|
|
|
public static MotorMock.Config GetMotorConfig()
|
|
{
|
|
var config = new MotorMock.Config();
|
|
|
|
/*
|
|
config.AXIS_VEL_RANGE = new Range<double>();
|
|
config.AXIS_VEL_RANGE.Max = UmacMotor.Config.AXIS_VEL_RANGE_TO;
|
|
config.AXIS_VEL_RANGE.Min = AXIS_VEL_RANGE_FROM;
|
|
|
|
config.AXIS_ACC_RANGE = new Range<double>();
|
|
config.AXIS_ACC_RANGE.Max = UmacMotor.AXIS_ACC_RANGE_TO;
|
|
config.AXIS_ACC_RANGE.Max = UmacMotor.AXIS_ACC_RANGE_FROM;
|
|
|
|
config.MoveRange = new Range<double>();
|
|
config.MoveRange.Max = 900; //AXIS_POS_LIMIT
|
|
config.MoveRange.Min = 0;
|
|
*/
|
|
config.MoveRange = (-1000, 1000);
|
|
config.VelocityRange = (0, 10000);
|
|
|
|
return config;
|
|
}
|
|
/*
|
|
protected bool _isSimMode;
|
|
public bool IsSimMode {
|
|
get { return _isSimMode; }
|
|
}
|
|
*/
|
|
|
|
public eHomingType HomingMode { get; private set; }
|
|
|
|
bool mServo = false;
|
|
double mCurrentPos;
|
|
|
|
List<string> mAlarmList = new List<string>();
|
|
bool mIsHoming;
|
|
bool mIsMoving;
|
|
bool mIsJogging;
|
|
internal bool mIsHomeCompleted = false;
|
|
|
|
double mMoveIncrement;
|
|
double mStartPos;
|
|
DateTime mMoveStartTime;
|
|
private object LockObj = new object();
|
|
public int MotorDirection = 1;
|
|
public double CurrentAccel = 0;
|
|
public double TargetPos = 0;
|
|
// 무슨 용도지 ??
|
|
//public double HomeProgress = 0.5;
|
|
|
|
|
|
public Config _Config;
|
|
protected double Velocity;
|
|
public double CurPosition;
|
|
public bool IsAxisMoving = false;
|
|
public bool IsInit;
|
|
public double Overload = 0;
|
|
|
|
public MotorMock(string deviceId, bool isSimul) : base(deviceId)
|
|
{
|
|
CommType = CommunicationType.IPAndStationNo;
|
|
|
|
IsSimul = isSimul;
|
|
|
|
mIsHomeCompleted = false;
|
|
|
|
Random r = new Random();
|
|
mCurrentPos = r.Next(100) / 1.2342;
|
|
}
|
|
|
|
protected override int DoActivate(IDeviceConfig config)
|
|
{
|
|
_Config = config as Config;
|
|
|
|
Velocity = _Config.AXIS_MOVE_VEL;
|
|
CurrentAccel = _Config.AXIS_MOVE_ACC;
|
|
|
|
HomingMode = _Config.HomingMode;
|
|
|
|
return E1.OK;
|
|
}
|
|
|
|
protected override int DoDeactivate()
|
|
{
|
|
IsInit = false;
|
|
IsAxisMoving = false;
|
|
|
|
//HomeProgress = 0;
|
|
|
|
return E1.OK;
|
|
}
|
|
|
|
bool IsReversed()
|
|
{
|
|
return MotorDirection < 0;
|
|
}
|
|
|
|
public override double GetCurrentPosition()
|
|
{
|
|
if (IsMoving()) {
|
|
CalculateCurrentPos();
|
|
}
|
|
return mCurrentPos;
|
|
}
|
|
|
|
/*
|
|
public double GetAcutalPosition()
|
|
{
|
|
return mCurrentPos;
|
|
}
|
|
*/
|
|
|
|
public void UpdateCurrentPosition()
|
|
{
|
|
CurPosition = GetCurrentPosition();
|
|
}
|
|
|
|
public void UpdateMovingStatus()
|
|
{
|
|
IsAxisMoving = mIsHoming || mIsMoving || mIsJogging;
|
|
}
|
|
|
|
public void UpdateInitComp()
|
|
{
|
|
IsInit = IsHomeCompleted();
|
|
}
|
|
|
|
|
|
public int Move(double pos, double velocity, double acceleration, TargetPosType targetPosType = TargetPosType.ABS)
|
|
{
|
|
SetVelocity((uint)velocity);
|
|
SetAcceleration(acceleration);
|
|
SetTargetPosition(pos, targetPosType);
|
|
|
|
return Move();
|
|
}
|
|
|
|
public override int SetTargetPosition(double targetPos, TargetPosType targetPosType = TargetPosType.ABS)
|
|
{
|
|
if (targetPosType == TargetPosType.ABS) {
|
|
if (!CheckInsidePos(targetPos)) {
|
|
LOG.E($"[{DeviceId}] 축 설정 위치가 상하한 범위를 벗어납니다.");
|
|
return E1.ARGUMENT;
|
|
}
|
|
|
|
TargetPos = targetPos;
|
|
return E1.OK;
|
|
} else {
|
|
double dCurPos = GetCurrentPosition();
|
|
return SetTargetPosition(dCurPos + targetPos);
|
|
}
|
|
}
|
|
|
|
protected bool CheckInsidePos(double value)
|
|
{
|
|
return ((_Config.MoveRange.Min - _Config.INPOS_GAP) < value) && (value < (_Config.MoveRange.Max + _Config.INPOS_GAP));
|
|
}
|
|
|
|
public override int SetVelocity(uint level)
|
|
{
|
|
if (level < _Config.AXIS_VEL_RANGE.Min || level > _Config.AXIS_VEL_RANGE.Max) {
|
|
LOG.E($"[{DeviceId}] 축 속도설정 범위를 벗어납니다.");
|
|
return E1.ARGUMENT;
|
|
}
|
|
|
|
Velocity = level;
|
|
return E1.OK;
|
|
}
|
|
|
|
public int SetAcceleration(double value)
|
|
{
|
|
CurrentAccel = value;
|
|
return E1.OK;
|
|
}
|
|
|
|
public override int Move()
|
|
{
|
|
mStartPos = mCurrentPos;
|
|
mMoveIncrement = TargetPos - mCurrentPos;
|
|
MotorDirection = mMoveIncrement > 0 ? 1 : -1;
|
|
Velocity = GetVelocity();
|
|
mIsMoving = true;
|
|
mMoveStartTime = DateTime.Now;
|
|
IsAxisMoving = true;
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
public bool IsInposition()
|
|
{
|
|
return !IsMoving();
|
|
}
|
|
|
|
public bool CheckAbsMoveComp()
|
|
{
|
|
return !IsMoving();
|
|
}
|
|
|
|
public void UpdateMotorOverload()
|
|
{
|
|
Overload = new Random().Next(30);
|
|
}
|
|
|
|
public override int ServoOn(bool onOff)
|
|
{
|
|
mServo = onOff;
|
|
//if (!mServo)
|
|
// mIsHomeCompleted = false;
|
|
return 0;
|
|
}
|
|
|
|
public override bool IsServoOn()
|
|
{
|
|
return mServo;
|
|
}
|
|
|
|
public bool SetDBOff()
|
|
{
|
|
return true;
|
|
}
|
|
|
|
public int JogStop()
|
|
{
|
|
return Stop();
|
|
}
|
|
|
|
public override int Stop()
|
|
{
|
|
if (mIsMoving || mIsJogging) {
|
|
CalculateCurrentPos();
|
|
}
|
|
|
|
mIsHoming = false;
|
|
mIsMoving = false;
|
|
mIsJogging = false;
|
|
IsAxisMoving = false;
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
public override bool IsAlarm()
|
|
{
|
|
return mAlarmList.Count() != 0;
|
|
}
|
|
|
|
public override int ClearAlarm()
|
|
{
|
|
mAlarmList.Clear();
|
|
return 0;
|
|
}
|
|
|
|
public override IList<string> GetAlarm()
|
|
{
|
|
return mAlarmList;
|
|
}
|
|
|
|
public override Range<double> GetMoveRange()
|
|
{
|
|
return _Config.MoveRange;
|
|
}
|
|
|
|
public override Range<uint> GetVelocityRange()
|
|
{
|
|
return _Config.VelocityRange;
|
|
}
|
|
|
|
public override uint GetVelocity()
|
|
{
|
|
return (uint)Velocity;
|
|
}
|
|
|
|
public override double GetTargetPosition()
|
|
{
|
|
return TargetPos;
|
|
}
|
|
|
|
public override bool IsMoving()
|
|
{
|
|
return IsAxisMoving;
|
|
}
|
|
|
|
|
|
#region 홈밍 관련
|
|
|
|
/*
|
|
public double GetHomeProgress()
|
|
{
|
|
return HomeProgress;
|
|
}
|
|
*/
|
|
|
|
|
|
public override bool IsHomeCompleted()
|
|
{
|
|
if (mIsHoming) {
|
|
CalculateCurrentPos();
|
|
}
|
|
|
|
return mIsHomeCompleted;
|
|
}
|
|
|
|
public override int Home()
|
|
{
|
|
mStartPos = mCurrentPos;
|
|
mIsHomeCompleted = false;
|
|
MotorDirection = -1;
|
|
Velocity = GetVelocity();
|
|
mIsHoming = true;
|
|
mMoveStartTime = DateTime.Now;
|
|
return 0;
|
|
}
|
|
|
|
|
|
#endregion
|
|
|
|
|
|
#region 조깅 관련
|
|
public override int Jog(int direction, double velocity)
|
|
{
|
|
mStartPos = mCurrentPos;
|
|
MotorDirection = direction;
|
|
Velocity = velocity;
|
|
mIsJogging = true;
|
|
IsAxisMoving = true;
|
|
mMoveStartTime = DateTime.Now;
|
|
return 0;
|
|
}
|
|
|
|
public override int JogIncrement(double increment, double velocity)
|
|
{
|
|
mStartPos = mCurrentPos;
|
|
mMoveIncrement = increment;
|
|
MotorDirection = mMoveIncrement > 0 ? 1 : -1;
|
|
Velocity = velocity;
|
|
mIsMoving = true;
|
|
IsAxisMoving = true; // Lukas 추가 : 이걸 추가해야 이동되는 것이 UI에 나타난다.
|
|
mMoveStartTime = DateTime.Now;
|
|
return 0;
|
|
}
|
|
|
|
|
|
#endregion
|
|
|
|
|
|
#region 모션 업데이트
|
|
private void CalculateCurrentPos()
|
|
{
|
|
Monitor.Enter(LockObj);
|
|
|
|
var diff = (DateTime.Now - mMoveStartTime).TotalSeconds;
|
|
double distance = MotorDirection * Velocity * diff;
|
|
double position = IsMoving() ? mStartPos : mCurrentPos;
|
|
|
|
if (mIsHoming) { // 홈밍 모드..
|
|
if (IsSimul && HomingMode == eHomingType.mode37) {
|
|
// 현재 위치를 0 으로 한다.
|
|
position = 0;
|
|
mIsHomeCompleted = true;
|
|
|
|
// 시뮬이니까, 사용상 편의용
|
|
// 서보 오프한다.
|
|
//ServoOn(false);
|
|
// 홈밍 종료
|
|
mIsHoming = false;
|
|
} else { // 기존 코드 보존...
|
|
if (diff > 3) {
|
|
mIsHoming = false;
|
|
mIsHomeCompleted = true;
|
|
//HomeProgress = 10;
|
|
position = 0;
|
|
} else if (diff > 1) {
|
|
//HomeProgress += 0.05;
|
|
position = (mStartPos + MotorDirection * Velocity * 2) // go to NOT
|
|
- (MotorDirection * (diff - 2) * Velocity); // go to reverse dir until C-phase signal
|
|
} else {
|
|
//HomeProgress += 0.05;
|
|
position += distance;
|
|
}
|
|
}
|
|
} else if (mIsJogging) {
|
|
position += distance;
|
|
if (!mIsHomeCompleted && diff > 10) {
|
|
mAlarmList.Add(
|
|
(MotorDirection > 0) ? "POT: (+) over travel"
|
|
: "NOT: (-) over travel");
|
|
mIsJogging = false;
|
|
}
|
|
} else if (mIsMoving) {
|
|
if (Math.Abs(mMoveIncrement) > Math.Abs(distance)) {
|
|
position += distance;
|
|
} else {
|
|
position += mMoveIncrement;
|
|
mIsMoving = false;
|
|
IsAxisMoving = false;
|
|
}
|
|
}
|
|
|
|
// check limit
|
|
//
|
|
if (mIsHomeCompleted) {
|
|
//var motorDirection = IsReversed() ? -1 : 1;
|
|
//var moveRange = GetMoveRange();
|
|
//double convertedPos = motorDirection * position;
|
|
//if (convertedPos < moveRange.Min
|
|
// && convertedPos != GetMoveRange().Min) {
|
|
// position = motorDirection * GetMoveRange().Min;
|
|
// mAlarmList.Add("NOT: (-) over travel");
|
|
// mIsMoving = false;
|
|
//} else if (convertedPos > GetMoveRange().Max
|
|
// && convertedPos != GetMoveRange().Max) {
|
|
// position = motorDirection * GetMoveRange().Max;
|
|
// mAlarmList.Add("POT: (+) over travel");
|
|
// mIsMoving = false;
|
|
//}
|
|
}
|
|
|
|
mCurrentPos = position;
|
|
Monitor.Exit(LockObj);
|
|
}
|
|
|
|
#endregion
|
|
|
|
}
|
|
}
|