private MotorInformation GetMotorInfo()
{
try
{
return(MotorUtil.GetMotorInformation(this.motorParaMap, this.GetOriginalMotorInfo()));
}
catch
{
throw new Exception("Error occurred when getting motor information!");
}
}
private MotorInfoMap <bool> GetAlarmInfo()
{
try
{
var bytes = Requestor.CreateGetMotorAlarmCmd();
bytes = this.tcpClient.Query(bytes, x => Protocol.Verify(x), out bool valid);
if (valid)
{
var data = Responder.GetMotorAlarmData(bytes);
return(MotorUtil.ParseAlarmInfo(data));
}
else
{
throw new Exception();
}
}
catch
{
throw new Exception("Error occurred when getting alarm information!");
}
}
public Task <MovementStatus> MoveAsync(List <LineSegment> segments, CancellationToken token, Action <MotorInfoMap <double>, MotorInfoMap <double> > motorInfoHandler)
{
return(Task <MovementStatus> .Factory.StartNew(() =>
{
if (token.IsCancellationRequested)
{
return MovementStatus.Cancel;
}
MovementStatus status = MovementStatus.Done;
segments = MotorUtil.SplitLines(this.motorParaMap, segments);
var items = MotorUtil.Split(segments.Count, PACKAGE_SIZE);
int index = 0;
Action reportHandler = () =>
{
var info = this.GetMotorInfo();
if (this.reportEnabled)
{
this.OnReportPositionInfo?.Invoke(info.PosInfo);
this.OnReportStatusInfo?.Invoke(info.StatusInfo);
this.OnReportSpeedInfo?.Invoke(info.SpeedInfo);
this.OnReportAlarmInfo?.Invoke(this.CurrentAlarmInfo);
}
motorInfoHandler?.Invoke(info.PosInfo, info.SpeedInfo);
};
//while (!token.WaitHandle.WaitOne(READ_REG_INTERVAL_MS))
while (true)
{
if (this.IsCacheEmpty)
{
if (index < items.Count)
{
int startIndex = index *PACKAGE_SIZE;
int len = items[index];
this.WriteData(segments.GetRange(startIndex, len));
index++;
}
else
{
if (this.IsMovementDone)
{
break;
}
}
}
if (!token.IsCancellationRequested)
{
reportHandler.Invoke();
}
else
{
break;
}
Thread.Sleep(READ_REG_INTERVAL_MS);
}
if (token.IsCancellationRequested)
{
this.Stop(true);
reportHandler.Invoke();
status = MovementStatus.Cancel;
//LogUtil.Instance.Info(string.Format("Stop time = {0}", DateTime.Now.ToString("HH:mm:ss.fff")));
}
return status;
}));
}
public static List <LineSegment> SplitLine(LineSegment line, int pulseStep)
{
var list = new List <LineSegment>();
uint tmp = GetPositive((uint)line.YDistance);
if (tmp <= pulseStep)
{
list.Add(line);
}
else
{
#region Step1: 获取每个轴距离的符号
var symbolList = new List <bool>();
symbolList.Add(IsNegative((uint)line.XDistance));
symbolList.Add(IsNegative((uint)line.YDistance));
symbolList.Add(IsNegative((uint)line.ZDistance));
symbolList.Add(IsNegative((uint)line.ADistance));
symbolList.Add(IsNegative((uint)line.BDistance));
symbolList.Add(IsNegative((uint)line.CDistance));
symbolList.Add(IsNegative((uint)line.UDistance));
symbolList.Add(IsNegative((uint)line.VDistance));
symbolList.Add(IsNegative((uint)line.WDistance));
#endregion
#region Step2: 获取轴距离的绝对值
var newLine = new LineSegment
{
XDistance = GetPositive((uint)line.XDistance),
XSpeed = line.XSpeed,
YDistance = GetPositive((uint)line.YDistance),
YSpeed = line.YSpeed,
ZDistance = GetPositive((uint)line.ZDistance),
ZSpeed = line.ZSpeed,
ADistance = GetPositive((uint)line.ADistance),
ASpeed = line.ASpeed,
BDistance = GetPositive((uint)line.BDistance),
BSpeed = line.BSpeed,
CDistance = GetPositive((uint)line.CDistance),
CSpeed = line.CSpeed,
UDistance = GetPositive((uint)line.UDistance),
USpeed = line.USpeed,
VDistance = GetPositive((uint)line.VDistance),
VSpeed = line.VSpeed,
WDistance = GetPositive((uint)line.WDistance),
WSpeed = line.WSpeed
};
#endregion
#region Step3: 分割线段
var stepList = MotorUtil.Split((int)newLine.YDistance, pulseStep);
int last = stepList.Last();
if (last < MIN_SPLIT_PULSE)
{
stepList.RemoveAt(stepList.Count - 1);
stepList[stepList.Count - 1] += last;
}
foreach (var m in stepList)
{
double ratio = m / newLine.YDistance;
var subLine = new LineSegment
{
XDistance = (uint)(newLine.XDistance * ratio),
XSpeed = newLine.XSpeed,
YDistance = m,
YSpeed = newLine.YSpeed,
ZDistance = (uint)(newLine.ZDistance * ratio),
ZSpeed = newLine.ZSpeed,
ADistance = (uint)(newLine.ADistance * ratio),
ASpeed = newLine.ASpeed,
BDistance = (uint)(newLine.BDistance * ratio),
BSpeed = newLine.BSpeed,
CDistance = (uint)(newLine.CDistance * ratio),
CSpeed = newLine.CSpeed,
UDistance = (uint)(newLine.UDistance * ratio),
USpeed = newLine.USpeed,
VDistance = (uint)(newLine.VDistance * ratio),
VSpeed = newLine.VSpeed,
WDistance = (uint)(newLine.WDistance * ratio),
WSpeed = newLine.WSpeed,
};
list.Add(subLine);
}
#endregion
#region Step4: 补偿误差
var tmp1 = new LineSegment();
foreach (var m in list)
{
tmp1.XDistance += m.XDistance;
tmp1.YDistance += m.YDistance;
tmp1.ZDistance += m.ZDistance;
tmp1.ADistance += m.ADistance;
tmp1.BDistance += m.BDistance;
tmp1.CDistance += m.CDistance;
tmp1.UDistance += m.UDistance;
tmp1.VDistance += m.VDistance;
tmp1.WDistance += m.WDistance;
}
var lastLine = list.Last();
lastLine.XDistance += newLine.XDistance - tmp1.XDistance;
lastLine.YDistance += newLine.YDistance - tmp1.YDistance;
lastLine.ZDistance += newLine.ZDistance - tmp1.ZDistance;
lastLine.ADistance += newLine.ADistance - tmp1.ADistance;
lastLine.BDistance += newLine.BDistance - tmp1.BDistance;
lastLine.CDistance += newLine.CDistance - tmp1.CDistance;
lastLine.UDistance += newLine.UDistance - tmp1.UDistance;
lastLine.VDistance += newLine.VDistance - tmp1.VDistance;
lastLine.WDistance += newLine.WDistance - tmp1.WDistance;
#endregion
#region Step5: 更新符号
foreach (var m in list)
{
if (symbolList[0])
{
m.XDistance = ToNegative((uint)m.XDistance);
}
if (symbolList[1])
{
m.YDistance = ToNegative((uint)m.YDistance);
}
if (symbolList[2])
{
m.ZDistance = ToNegative((uint)m.ZDistance);
}
if (symbolList[3])
{
m.ADistance = ToNegative((uint)m.ADistance);
}
if (symbolList[4])
{
m.BDistance = ToNegative((uint)m.BDistance);
}
if (symbolList[5])
{
m.CDistance = ToNegative((uint)m.CDistance);
}
if (symbolList[6])
{
m.UDistance = ToNegative((uint)m.UDistance);
}
if (symbolList[7])
{
m.VDistance = ToNegative((uint)m.VDistance);
}
if (symbolList[8])
{
m.WDistance = ToNegative((uint)m.WDistance);
}
}
#endregion
}
return(list);
}
