Exemple #1
0
        public void MoveTrc(MoveTrcPrm moveTrcPrm, IList <ICrdable> crds)
        {
            CommandMoveTrc command = new CommandMoveTrc(this.X, this.Y, moveTrcPrm, crds);

            this.Fire(command);
        }
Exemple #2
0
        public override Result FluidArc(PointD accStartPos, PointD arcStartPos, PointD arcEndPos, PointD decEndPos, PointD center, short clockwize, double vel, PointD[] points, double intervalSec, double acc)
        {
            if (Machine.Instance.Robot.IsSimulation)
            {
                return(Result.OK);
            }

            if (points == null)
            {
                return(Result.FAILED);
            }

            if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet || Machine.Instance.Valve1.RunMode == ValveRunMode.Dry)
            {
                accStartPos = accStartPos.ToNeedle(this.valve1.Key);
                arcStartPos = arcStartPos.ToNeedle(this.valve1.Key);
                arcEndPos   = arcEndPos.ToNeedle(this.valve1.Key);
                decEndPos   = decEndPos.ToNeedle(this.valve1.Key);
                center      = center.ToNeedle(this.valve1.Key);
                for (int i = 0; i < points.Length; i++)
                {
                    points[i] = points[i].ToNeedle(this.valve1.Key);
                }
            }

            double interval = 0;

            if (points.Length >= 2)
            {
                interval = points[0].DistanceTo(points[1]);
            }
            double accTime = this.CalcAccTime(accStartPos, arcStartPos, vel, acc * 1000);

            //连续差补
            List <ICrdable> crdList = new List <ICrdable>();
            //加速段-直线
            CrdLnXY crdAcc = new CrdLnXY()
            {
                EndPosX = arcStartPos.X,
                EndPosY = arcStartPos.Y,
                Vel     = vel,
                Acc     = acc,
                VelEnd  = vel
            };

            crdList.Add(crdAcc);
            //匀速段-圆
            for (int i = 1; i < points.Length; i++)
            {
                CrdArcXYC crdArc = new CrdArcXYC()
                {
                    EndPosX   = points[i].X,
                    EndPosY   = points[i].Y,
                    CenterX   = center.X - points[i - 1].X,
                    CenterY   = center.Y - points[i - 1].Y,
                    Clockwise = clockwize,
                    Vel       = vel,
                    Acc       = acc,
                    VelEnd    = vel
                };
                crdList.Add(crdArc);
            }
            //减速段-直线
            CrdLnXY crdDec = new CrdLnXY()
            {
                EndPosX = decEndPos.X,
                EndPosY = decEndPos.Y,
                Vel     = vel,
                Acc     = acc,
                VelEnd  = 0
            };

            crdList.Add(crdDec);
            Result rtn          = Result.OK;
            int    intervalTime = (int)(intervalSec * 1000000);

            Action timeCtrlAction = () =>
            {
                CommandMoveTrc command = new CommandMoveTrc(
                    Machine.Instance.Robot.AxisX,
                    Machine.Instance.Robot.AxisY,
                    Machine.Instance.Robot.TrcPrm,
                    crdList);
                command.Starting += () =>
                {
                    ValveSprayServer.Instance.StartSprayEvent.Set();
                };

                ValveSprayServer.Instance.SleepSpan = TimeSpan.FromSeconds(accTime);
                if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
                {
                    ValveSprayServer.Instance.SprayAction = () =>
                    {
                        this.SprayCycle((short)points.Count(), (short)(intervalTime - this.jtValve1.Prm.OnTime));
                    };
                }
                else
                {
                    ValveSprayServer.Instance.SprayAction = null;
                }

                Log.Dprint(string.Format("Fluid Line[time ctrl]-> accTime:{0}, intervalTime:{1}, onTime:{2}, offTime{3}.",
                                         accTime, intervalTime, this.jtValve1.Prm.OnTime, this.jtValve1.Prm.OffTime));
                Machine.Instance.Robot.Fire(command);
                rtn = Machine.Instance.Robot.WaitCommandReply(command);
            };

            Action cmp2dAction = () =>
            {
                if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
                {
                    SimulCmp2dStart((short)Cmp2dSrcType.编码器, (short)this.jtValve1.Prm.Cmp2dMaxErr, points);
                }
                CommandMoveTrc command = new CommandMoveTrc(
                    Machine.Instance.Robot.AxisX,
                    Machine.Instance.Robot.AxisY,
                    Machine.Instance.Robot.TrcPrm,
                    crdList);
                Log.Dprint(string.Format("Fluid Line[2d ctrl]-> maxErr:{0}, interval:{1}", this.jtValve1.Prm.Cmp2dMaxErr, interval));
                Machine.Instance.Robot.Fire(command);
                rtn = Machine.Instance.Robot.WaitCommandReply(command);
                //if (this.RunMode == ValveRunMode.Wet)
                //{
                //    Cmp2dStop();
                //}
            };

            if (this.jtValve1.Prm.FluidMode == ValveFluidMode.时间控制优先)
            {
                if (intervalTime - this.jtValve1.Prm.OnTime < short.MaxValue)
                {//判断脉宽是否short类型溢出,
                    timeCtrlAction();
                }
                else
                {//溢出:位置比较模式
                    cmp2dAction();
                }
            }
            else
            {
                if (interval * 1000 > this.jtValve1.Prm.Cmp2dMaxErr || points.Length < 2)
                {
                    cmp2dAction();
                }
                else
                {
                    timeCtrlAction();
                }
            }

            return(rtn);
        }
Exemple #3
0
        public override Result FluidSymbolLines(List <CrdSymbolLine> symbolLinesCrdData, GearValveFluidSymbolLinesPrm fluidPrm, double acc, double offsetX = 0, double offsetY = 0)
        {
            Result ret = Result.OK;

            //如果不是CV模式,则要把坐标都转换到阀上
            if (this.RunMode != ValveRunMode.Look)
            {
                PointD offset = new PointD(offsetX, offsetY);
                foreach (var item in symbolLinesCrdData)
                {
                    if (item.Type == 0)
                    {
                        item.Points[0] = item.Points[0].ToNeedle(this.ValveType) + offset;
                        item.Points[1] = item.Points[1].ToNeedle(this.ValveType) + offset;
                    }
                    else
                    {
                        item.Points[0] = item.Points[0].ToNeedle(this.ValveType) + offset;
                        item.Points[1] = item.Points[1].ToNeedle(this.ValveType) + offset;
                        item.Points[2] = item.Points[2].ToNeedle(this.ValveType) + offset;
                    }
                }
            }


            //解析复合线段为对应的插补指令
            List <ICrdable> crdList = new List <ICrdable>();

            for (int i = 0; i < symbolLinesCrdData.Count; i++)
            {
                //如果是直线,则先要判断高度是否一致,一致的话添加一个两轴直线插补,不一致的话要先添加一个BufGear插补
                if (symbolLinesCrdData[i].Type == 0)
                {
                    if (Machine.Instance.Robot.AxisZ.Pos != symbolLinesCrdData[i].EndZ)
                    {
                        CrdXYGear crdXYGear = new CrdXYGear()
                        {
                            GearAxis   = Machine.Instance.Robot.AxisZ,
                            DeltaPulse = Machine.Instance.Robot.AxisZ.ConvertPos2Card(symbolLinesCrdData[i].EndZ)
                        };
                        crdList.Add(crdXYGear);
                    }

                    CrdLnXY crd = new CrdLnXY()
                    {
                        EndPosX = symbolLinesCrdData[i].Points[1].X,
                        EndPosY = symbolLinesCrdData[i].Points[1].Y,
                        Vel     = fluidPrm.Vel,
                        Acc     = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
                        VelEnd  = (i != symbolLinesCrdData.Count - 1 ? 0 : fluidPrm.Vel),
                    };
                    crdList.Add(crd);
                }
                //如果是圆弧,则要先添加一个BufGear插补,再添加一个圆弧插补
                else if (symbolLinesCrdData[i].Type == 1)
                {
                    CrdXYGear crdXYGear = new CrdXYGear()
                    {
                        GearAxis   = Machine.Instance.Robot.AxisR,
                        DeltaPulse = Machine.Instance.Robot.AxisR.ConvertPos2Card(symbolLinesCrdData[i].TrackSweep)
                    };
                    crdList.Add(crdXYGear);
                    double r = Math.Sqrt(Math.Pow(symbolLinesCrdData[i].Points[0].X - symbolLinesCrdData[i].Points[1].X, 2)
                                         + Math.Pow(symbolLinesCrdData[i].Points[0].Y - symbolLinesCrdData[i].Points[1].Y, 2));
                    CrdArcXYR crd = new CrdArcXYR
                    {
                        EndPosX   = symbolLinesCrdData[i].Points[2].X,
                        EndPosY   = symbolLinesCrdData[i].Points[2].Y,
                        R         = r,
                        Clockwise = (short)symbolLinesCrdData[i].Clockwise,
                        Vel       = fluidPrm.ArcSpeed,
                        Acc       = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
                        VelEnd    = (i != symbolLinesCrdData.Count - 1 ? 0 : fluidPrm.Vel),
                    };
                    if (Math.Abs(symbolLinesCrdData[i].EndAngle) > 180)
                    {
                        crd.R = crd.R * -1;
                    }
                    crdList.Add(crd);
                }
            }
            //计算关胶位置
            PointD stopSprayPos = this.CalculateStopSprayPos(symbolLinesCrdData, fluidPrm.StopSprayDistance);

            //终点位置
            PointD endPos = new PointD();

            //如果是Wet模式
            if (this.RunMode == ValveRunMode.Wet)
            {
                //得到终点位置
                if (symbolLinesCrdData.Last().Type == 0)
                {
                    endPos = symbolLinesCrdData.Last().Points[1];
                }
                else
                {
                    endPos = symbolLinesCrdData.Last().Points[2];
                }
                endPos.X = Math.Round(endPos.X, 3);
                endPos.Y = Math.Round(endPos.Y, 3);
                //如果线的终点和关胶点不一致,则可以启用二维比较
                if (endPos != stopSprayPos)
                {
                    this.CmpStop();
                    this.Cmp2dStart(stopSprayPos);
                }
                else
                {
                    this.Spraying();
                }
            }

            //移动前的出胶延时
            Thread.Sleep(TimeSpan.FromSeconds(fluidPrm.StartSprayDelay));

            //开始移动
            CommandMoveTrc command = new CommandMoveTrc(
                Machine.Instance.Robot.AxisX,
                Machine.Instance.Robot.AxisY,
                Machine.Instance.Robot.TrcPrm,
                crdList)
            {
                EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
            };

            Log.Dprint(string.Format("Fluid Line[2d ctrl]-> maxErr:{0}", this.Prm.Cmp2dMaxErr));
            Machine.Instance.Robot.Fire(command);
            ret = Machine.Instance.Robot.WaitCommandReply(command);
            if (!ret.IsOk)
            {
                return(ret);
            }

            if (this.RunMode == ValveRunMode.Wet)
            {
                //如果提前关胶距离是0,就直接关胶
                if (fluidPrm.StopSprayDistance == 0)
                {
                    this.SprayOff();
                }
            }

            //在终点延时
            Thread.Sleep(TimeSpan.FromSeconds(fluidPrm.EndPosDelay));

            //if (this.RunMode != ValveRunMode.Look)
            //{
            //    //下压
            //    ret = Machine.Instance.Robot.MoveIncZAndReply(-fluidPrm.PressDistance, fluidPrm.PressVel, fluidPrm.PressAcc);
            //    if (!ret.IsOk)
            //        return ret;

            //    //下压保持时间
            //    Thread.Sleep(TimeSpan.FromSeconds(fluidPrm.PressTime));

            //    //抬起
            //    ret = Machine.Instance.Robot.MoveIncZAndReply(fluidPrm.RaiseDistance, fluidPrm.RaiseVel, fluidPrm.RaiseAcc);
            //}

            //开始回走
            //如果是Wet模式或者Dry模式
            if (fluidPrm.BacktrackDistance == 0 || fluidPrm.BacktrackDistance.Equals(0))
            {
            }
            else
            {
                if (this.RunMode == ValveRunMode.Wet || this.RunMode == ValveRunMode.Dry)
                {
                    //回抬
                    ret = Machine.Instance.Robot.MoveIncZAndReply(fluidPrm.BacktrackGap, fluidPrm.BacktrackSpeed, acc);

                    //计算回走终点位置
                    PointD backTrackPoint = CalculateBackTrackPos(symbolLinesCrdData, fluidPrm.BacktrackDistance);

                    //执行回走
                    List <ICrdable> crdXYZList = new List <ICrdable>();
                    if (symbolLinesCrdData.Last().Type == 0) //直线
                    {
                        CrdXYGear crdXYGear = new CrdXYGear()
                        {
                            GearAxis   = Machine.Instance.Robot.AxisZ,
                            DeltaPulse = Machine.Instance.Robot.AxisZ.ConvertPos2Card(fluidPrm.BacktrackEndGap - fluidPrm.BacktrackGap)
                        };
                        crdXYZList.Add(crdXYGear);
                        CrdLnXY crd = new CrdLnXY()
                        {
                            EndPosX = backTrackPoint.X,
                            EndPosY = backTrackPoint.Y,
                            Vel     = fluidPrm.BacktrackSpeed,
                            Acc     = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
                            VelEnd  = 0,
                        };
                        crdXYZList.Add(crd);
                    }
                    else      //圆弧
                    {
                        CrdXYGear crdXYZGear = new CrdXYGear()
                        {
                            GearAxis   = Machine.Instance.Robot.AxisZ,
                            DeltaPulse = Machine.Instance.Robot.AxisZ.ConvertPos2Card(fluidPrm.BacktrackEndGap - fluidPrm.BacktrackGap)
                        };
                        crdXYZList.Add(crdXYZGear);
                        CrdXYGear crdXYRGear = new CrdXYGear()
                        {
                            GearAxis   = Machine.Instance.Robot.AxisR,
                            DeltaPulse = Machine.Instance.Robot.AxisR.ConvertPos2Card(-symbolLinesCrdData.Last().TrackSweep *fluidPrm.BacktrackDistance / 100)
                        };
                        crdXYZList.Add(crdXYRGear);
                        double r = Math.Sqrt(Math.Pow(symbolLinesCrdData.Last().Points[0].X - symbolLinesCrdData.Last().Points[1].X, 2)
                                             + Math.Pow(symbolLinesCrdData.Last().Points[0].Y - symbolLinesCrdData.Last().Points[1].Y, 2));
                        CrdArcXYR crd = new CrdArcXYR
                        {
                            EndPosX   = backTrackPoint.X,
                            EndPosY   = backTrackPoint.Y,
                            R         = r,
                            Clockwise = (short)symbolLinesCrdData.Last().Clockwise == (short)0 ? (short)1 : (short)0,
                            Vel       = fluidPrm.BacktrackSpeed,
                            Acc       = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
                            VelEnd    = 0,
                        };
                        if (Math.Abs(symbolLinesCrdData.Last().EndAngle *fluidPrm.BacktrackDistance / 100) > 180)
                        {
                            crd.R = crd.R * -1;
                        }
                        crdXYZList.Add(crd);
                    }
                    CommandMoveTrc command2 = new CommandMoveTrc(
                        Machine.Instance.Robot.AxisX,
                        Machine.Instance.Robot.AxisY,
                        Machine.Instance.Robot.TrcPrm,
                        crdXYZList)
                    {
                        EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
                    };
                    Machine.Instance.Robot.Fire(command2);
                    ret = Machine.Instance.Robot.WaitCommandReply(command2);
                }
            }
            //结束回走

            if (this.RunMode == ValveRunMode.Wet || this.RunMode == ValveRunMode.Dry)
            {
                //抬起到回位高度
                ret = Machine.Instance.Robot.MoveIncZAndReply(fluidPrm.BackGap, fluidPrm.Vel, acc);
            }

            return(ret);
        }
Exemple #4
0
        public override Result FluidLine(PointD accStartPos, PointD lineStartPos, PointD lineEndPos, PointD decEndPos, double vel, PointD[] points, double intervalSec, double acc)
        {
            if (Machine.Instance.Robot.IsSimulation)
            {
                return(Result.OK);
            }

            if (points == null)
            {
                return(Result.FAILED);
            }

            if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet || Machine.Instance.Valve1.RunMode == ValveRunMode.Dry)
            {
                accStartPos  = accStartPos.ToNeedle(this.valve1.Key);
                lineStartPos = lineStartPos.ToNeedle(this.valve1.Key);
                lineEndPos   = lineEndPos.ToNeedle(this.valve1.Key);
                decEndPos    = decEndPos.ToNeedle(this.valve1.Key);
                for (int i = 0; i < points.Length; i++)
                {
                    points[i] = points[i].ToNeedle(this.valve1.Key);
                }
            }

            double interval = 0;

            if (points.Length >= 2)
            {
                interval = points[0].DistanceTo(points[1]);
            }

            //计算加速段时间,用于启动时间控制打胶
            double accTime = this.CalcAccTime(accStartPos, lineStartPos, vel, acc * 1000);

            //连续插补
            List <ICrdable> crdList = new List <ICrdable>();
            //加速段
            CrdLnXY crdAcc = new CrdLnXY()
            {
                EndPosX = lineStartPos.X,
                EndPosY = lineStartPos.Y,
                Vel     = vel,
                Acc     = acc,
                VelEnd  = vel
            };

            crdList.Add(crdAcc);
            //匀速段
            for (int i = 1; i < points.Length; i++)
            {
                CrdLnXY crdLn = new CrdLnXY()
                {
                    EndPosX = points[i].X,
                    EndPosY = points[i].Y,
                    Vel     = vel,
                    Acc     = acc,
                    VelEnd  = vel
                };
                crdList.Add(crdLn);
            }
            //减速段
            CrdLnXY crdDec = new CrdLnXY()
            {
                EndPosX = decEndPos.X,
                EndPosY = decEndPos.Y,
                Vel     = vel,
                Acc     = acc,
                VelEnd  = 0
            };

            crdList.Add(crdDec);

            DateTime t1           = DateTime.Now;
            DateTime t2           = DateTime.Now;
            Result   rtn          = Result.OK;
            int      intervalTime = (int)(intervalSec * 1000000);

            //时间控制函数
            Action timeCtrlAction = () =>
            {
                ICommandable command = null;
                if (this.jtValve1.Prm.MoveMode == ValveMoveMode.单次插补)
                {//单次插补
                    CrdLnXY crd = new CrdLnXY()
                    {
                        EndPosX = decEndPos.X,
                        EndPosY = decEndPos.Y,
                        Vel     = vel,
                        Acc     = acc,
                        VelEnd  = vel
                    };
                    command = new CommandMoveTrc(
                        Machine.Instance.Robot.AxisX,
                        Machine.Instance.Robot.AxisY,
                        Machine.Instance.Robot.TrcPrm,
                        crd,
                        () =>
                    {
                        t1 = DateTime.Now;
                        ValveSprayServer.Instance.StartSprayEvent.Set();
                    })
                    {
                        EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
                    };
                }
                else
                {//多次插补
                    command = new CommandMoveTrc(
                        Machine.Instance.Robot.AxisX,
                        Machine.Instance.Robot.AxisY,
                        Machine.Instance.Robot.TrcPrm,
                        crdList,
                        () =>
                    {
                        t1 = DateTime.Now;
                        ValveSprayServer.Instance.StartSprayEvent.Set();
                    })
                    {
                        EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
                    };
                }

                //设置打胶线程启动时间
                ValveSprayServer.Instance.SleepSpan = TimeSpan.FromSeconds(accTime);
                if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
                {
                    ValveSprayServer.Instance.SprayAction = () =>
                    {
                        t2 = DateTime.Now;
                        this.SprayCycle((short)points.Count(), (short)(intervalTime - this.jtValve1.Prm.OnTime));
                    };
                }
                else
                {
                    ValveSprayServer.Instance.SprayAction = null;
                }

                Log.Dprint(string.Format("Fluid Line[time ctrl]-> accTime:{0}, intervalTime:{1}, onTime:{2}, offTime{3}.",
                                         accTime, intervalTime, this.jtValve1.Prm.OnTime, this.jtValve1.Prm.OffTime));
                Machine.Instance.Robot.Fire(command);
                rtn = Machine.Instance.Robot.WaitCommandReply(command);

                Debug.WriteLine(t1.ToString("HH::mm::ss::fff"));
                Debug.WriteLine(t2.ToString("HH::mm::ss::fff"));
            };

            //2d比较控制函数
            Action cmp2dAction = () =>
            {
                if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
                {
                    CmpStop();
                    SimulCmp2dStart((short)Cmp2dSrcType.编码器, (short)this.jtValve1.Prm.Cmp2dMaxErr, points);
                }

                if (this.jtValve1.Prm.MoveMode == ValveMoveMode.单次插补)
                {
                    rtn = Machine.Instance.Robot.MovePosXYAndReply(decEndPos, vel, acc);
                }
                else
                {
                    CommandMoveTrc command = new CommandMoveTrc(
                        Machine.Instance.Robot.AxisX,
                        Machine.Instance.Robot.AxisY,
                        Machine.Instance.Robot.TrcPrm,
                        crdList)
                    {
                        EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
                    };
                    Log.Dprint(string.Format("Fluid Line[2d ctrl]-> maxErr:{0}, interval:{1}", this.jtValve1.Prm.Cmp2dMaxErr, interval));
                    Machine.Instance.Robot.Fire(command);
                    rtn = Machine.Instance.Robot.WaitCommandReply(command);
                }

                if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
                {
                    //Cmp2dStop();
                }
            };

            //一维比较控制函数
            Action cmp1dAction = () =>
            {
                if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
                {
                    SimulCmp2dStop();
                    CmpStart((short)Cmp2dSrcType.编码器, points);
                }

                if (this.jtValve1.Prm.MoveMode == ValveMoveMode.单次插补)
                {
                    rtn = Machine.Instance.Robot.MovePosXYAndReply(decEndPos, vel, acc);
                }
                else
                {
                    CommandMoveTrc command = new CommandMoveTrc(
                        Machine.Instance.Robot.AxisX,
                        Machine.Instance.Robot.AxisY,
                        Machine.Instance.Robot.TrcPrm,
                        crdList)
                    {
                        EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
                    };
                    Log.Dprint(string.Format("Fluid Line[2d ctrl]-> maxErr:{0}, interval:{1}", this.jtValve1.Prm.Cmp2dMaxErr, interval));
                    Machine.Instance.Robot.Fire(command);
                    rtn = Machine.Instance.Robot.WaitCommandReply(command);
                }

                if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
                {
                    //CmpStop();
                }
            };

            switch (this.jtValve1.Prm.FluidMode)
            {
            case ValveFluidMode.一维比较优先:
                cmp1dAction();
                break;

            case ValveFluidMode.二维比较优先:
                if (interval * 1000 > this.jtValve1.Prm.Cmp2dMaxErr || points.Length < 2)
                {
                    cmp2dAction();
                }
                else
                {
                    //timeCtrlAction();
                    cmp1dAction();
                }
                break;

            case ValveFluidMode.时间控制优先:
                if (intervalTime - this.jtValve1.Prm.OnTime < short.MaxValue)
                {    //判断脉宽是否short类型溢出,
                    timeCtrlAction();
                }
                else
                {    //溢出:位置比较模式
                    //cmp2dAction();
                    cmp1dAction();
                }
                break;
            }


            return(rtn);
        }