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); }