Virgo_Mask/VirgoUI/Controls/Parts/ATMDualArmRobot.xaml.cs

using MECF.Framework.Common.CommonData;
using VirgoUI.Controls.Common;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Linq;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Animation;
using OpenSEMI.ClientBase;
using MECF.Framework.Common.Equipment;
using Aitex.Core.UI.MVVM;

namespace VirgoUI.Controls.Parts
{
    /// <summary>
    /// ATMDualArmRobot.xaml 的交互逻辑
    /// </summary>
    public partial class ATMDualArmRobot : UserControl, INotifyPropertyChanged
    {
        protected readonly int MoveTime = 300;
        private const int AnimationTimeout = 3000; // seconds

        public int TranslateX
        {
            get { return (int)GetValue(TranslateXProperty); }
            set { SetValue(TranslateXProperty, value); }
        }

        // Using a DependencyProperty as the backing store for RotateAngel.  This enables animation, styling, binding, etc...
        public static readonly DependencyProperty TranslateXProperty =
            DependencyProperty.Register("TranslateX", typeof(int), typeof(ATMDualArmRobot), new PropertyMetadata(0));

        public ModuleName Station
        {
            get { return (ModuleName)GetValue(StationProperty); }
            set { SetValue(StationProperty, value); }
        }

        // Using a DependencyProperty as the backing store for Station.  This enables animation, styling, binding, etc...
        public static readonly DependencyProperty StationProperty =
            DependencyProperty.Register("Station", typeof(ModuleName), typeof(ATMDualArmRobot), new PropertyMetadata(ModuleName.Robot));

        public ICommand Command
        {
            get { return (ICommand)GetValue(CommandProperty); }
            set { SetValue(CommandProperty, value); }
        }

        // Using a DependencyProperty as the backing store for Command.  This enables animation, styling, binding, etc...
        public static readonly DependencyProperty CommandProperty =
            DependencyProperty.Register("Command", typeof(ICommand), typeof(ATMDualArmRobot), new PropertyMetadata(null));



        public RobotMoveInfo RobotMoveInfo
        {
            get { return (RobotMoveInfo)GetValue(RobotMoveInfoProperty); }
            set { SetValue(RobotMoveInfoProperty, value); }
        }

        public static readonly DependencyProperty RobotMoveInfoProperty =
            DependencyProperty.Register("RobotMoveInfo", typeof(RobotMoveInfo), typeof(ATMDualArmRobot), new FrameworkPropertyMetadata(null, FrameworkPropertyMetadataOptions.AffectsRender));


        public VTMRobotPosition StationPosition
        {
            get { return (VTMRobotPosition)GetValue(StationPositionProperty); }
            set { SetValue(StationPositionProperty, value); }
        }

        // Using a DependencyProperty as the backing store for StationPosition.  This enables animation, styling, binding, etc...
        public static readonly DependencyProperty StationPositionProperty =
            DependencyProperty.Register("StationPosition", typeof(VTMRobotPosition), typeof(ATMDualArmRobot), new PropertyMetadata(null, PropertyChangedCallback));

        public WaferInfo[] RobotWafers
        {
            get { return (WaferInfo[])GetValue(RobotWafersProperty); }
            set { SetValue(RobotWafersProperty, value); }
        }

        // Using a DependencyProperty as the backing store for RobotWafers.  This enables animation, styling, binding, etc...
        public static readonly DependencyProperty RobotWafersProperty =
            DependencyProperty.Register("RobotWafers", typeof(WaferInfo[]), typeof(ATMDualArmRobot), new PropertyMetadata(null, PropertyChangedCallback));

        public WaferInfo Wafer1
        {
            get { return (WaferInfo)GetValue(Wafer1Property); }
            set { SetValue(Wafer1Property, value); }
        }

        // Using a DependencyProperty as the backing store for Wafer1.  This enables animation, styling, binding, etc...
        public static readonly DependencyProperty Wafer1Property =
            DependencyProperty.Register("Wafer1", typeof(WaferInfo), typeof(ATMDualArmRobot), new PropertyMetadata(null));

        public WaferInfo Wafer2
        {
            get { return (WaferInfo)GetValue(Wafer2Property); }
            set { SetValue(Wafer2Property, value); }
        }

        // Using a DependencyProperty as the backing store for Wafer2.  This enables animation, styling, binding, etc...
        public static readonly DependencyProperty Wafer2Property =
            DependencyProperty.Register("Wafer2", typeof(WaferInfo), typeof(ATMDualArmRobot), new PropertyMetadata(null));

        private string CurrentPosition;
        //private string CurrentArmA;
        //private string CurrentArmB;


        private RobotAction CurrentAction
        {
            get; set;
        }


        private List<MenuItem> menu;
        public List<MenuItem> Menu
        {
            get
            {
                return menu;
            }
            set
            {
                menu = value;
                PropertyChanged?.Invoke(this, new PropertyChangedEventArgs("Menu"));
            }
        }

        public ICommand MoveCommand
        {
            get
            {
                return new DelegateCommand<object>(MoveTo);
            }
        }

     //   private AnimationQueue queue;

        public event PropertyChangedEventHandler PropertyChanged;

        static void PropertyChangedCallback(DependencyObject d, DependencyPropertyChangedEventArgs e)
        {
            var self = (ATMDualArmRobot)d;
            switch (e.Property.Name)
            {
                case "StationPosition":

                    if (e.NewValue != null)
                    {
                        var positions = ((VTMRobotPosition)e.NewValue).Rotations;
                        var menus = new List<MenuItem>();
                        foreach (var position in positions)
                        {
                            var m = new MenuItem() { Header = position.Key };
                            Enum.TryParse(position.Key.Split('.')[0], out RobotArm arm);
                            m.Items.Add(new MenuItem() { Header = "Pick", Command = self.MoveCommand, CommandParameter = new RobotMoveInfo() { BladeTarget = position.Key, Action = RobotAction.Picking, ArmTarget = arm } });
                            m.Items.Add(new MenuItem() { Header = "Place", Command = self.MoveCommand, CommandParameter = new RobotMoveInfo() { BladeTarget = position.Key, Action = RobotAction.Placing, ArmTarget = arm } });
                            m.Items.Add(new MenuItem() { Header = "Move", Command = self.MoveCommand, CommandParameter = new RobotMoveInfo() { BladeTarget = position.Key, Action = RobotAction.Moving, ArmTarget = arm } });
                            menus.Add(m);
                        }
                        self.Menu = menus;
                        self.MoveTo(new RobotMoveInfo() { BladeTarget = positions.First().Key, Action = RobotAction.None });
                    }

                    break;
                case "RobotWafers":
                    //if (e.NewValue == null)
                    //{
                    //	self.Wafer1 = null;
                    //	self.Wafer2 = null;
                    //}
                    //else
                    //{
                    //	if (self.RobotWafers.Length > 1)
                    //	{
                    //		self.Wafer1 = self.RobotWafers[0];
                    //		self.Wafer2 = self.RobotWafers[1];
                    //	}
                    //}
                    break;
                default:
                    break;
            }
        }

        Dictionary<string, int> CanvasRotates = new Dictionary<string, int>
        {
            {"System",90},{"EfemRobot",90 },{"LP1",90 },{"LP2",90},{"Buffer",0},
            {"Flipper",180 },{"PMA",270},{"PMB",270},
            {"Aligner1",0 },{"Aligner2",0},{"Cooling1",180},{"Cooling2",180},
        };

        public ATMDualArmRobot()
        {
#if DEBUG
            System.Diagnostics.PresentationTraceSources.DataBindingSource.Switch.Level = System.Diagnostics.SourceLevels.Critical;
#endif
            InitializeComponent();
            root.DataContext = this;

            canvasRoot.Rotate(90);

            canvas1.Rotate(200);
            canvas2.Rotate(260);
            canvas3.Rotate(-100);


            canvas21.Rotate(140);
            canvas22.Rotate(100);
            canvas23.Rotate(120);

            StationPosition = new VTMRobotPosition()
            {
                Rotations = new Dictionary<string, int>()
                {
                    { "Both.System", -40 },
                    { "ArmA.System", -40 }

                    , { "ArmA.LP1", -147 }
                    , { "ArmA.LP2", 38 }
                    , { "ArmA.Flipper", -85 }
                    , { "ArmA.Buffer", 24 }
                    , { "ArmA.Aligner1", 74 }
                    , { "ArmA.Aligner2", 14 }
                    , { "ArmA.Cooling1", -90 }
                    , { "ArmA.Cooling2", -135 }
                    , { "ArmA.PMA", -145 }
                    , { "ArmA.PMB", 100 }
                    , { "ArmB.System", -68 }

                    , { "ArmB.LP1", -122 }
                    , { "ArmB.LP2", 60 }
                    , { "ArmB.Flipper", -85 }
                    , { "ArmB.Buffer", 44 }
                    , { "ArmB.Aligner1", 24 }
                    , { "ArmB.Aligner2", 44 }
                    , { "ArmB.Cooling1", -105 }
                    , { "ArmB.Cooling2", -105 }
                    , { "ArmB.PMA", -170 }
                    , { "ArmB.PMB", 75 }
                },
                Home = new int[8] { 0, 180, 180, 0, 0, 180, 180, 0 },

                //Arm1Extend = new int[8] { 140, 180, -50, 40, 100, 260, 50, -180 },
                //Arm2Extend = new int[8] { 220, 100, -50, -50, 180, 180, 50, -85 },
                //Flipper
                Arm1Extend = new int[8] { 4, 180, 180, 0, 4, 360, 0, 0 },
                Arm2Extend = new int[8] { -4, 360, 0, 0, -4, 180, 180, 0, },

                //Buffer
                Arm3Extend = new int[8] { -30, 210, 180, 0, -30, 390, 0, 0 },
                Arm31Extend = new int[8] { -60, 60, 0, 0, 0, 180, 180, 0 },
                //Aligner
                //Arm4Extend = new int[8] { 0, 180, 180, 0, 90, 270, 0, 0, },
                Arm4Extend = new int[8] { 44, 136, 180, 0, 90, 270, 0, 0, },
                Arm41Extend = new int[8] { 36, 324, 0, 0, 37, 143, 180, 0, },
                //LP,PM
                Arm5Extend = new int[8] { 0, 180, 180, 0, 0, 360, 0, 0 },
                Arm6Extend = new int[8] { 0, 360, 0, 0, 0, 180, 180, 0, },

                // Cooling1
                Arm7Extend = new int[8] { -35, 35, 180, 0, -30, 30, 0, 0 },
                Arm71Extend = new int[8] { -50, 50, 0, 0, -40, 220, 180, 0 },

                // Cooling2
                Arm8Extend = new int[8] { 80, 100, 180, 0, 80, -80, 0, 0 },
                Arm81Extend = new int[8] { 44, 316, 0, 0, 44, 140, 180, 0 },
            };

            CurrentPosition = ModuleName.System.ToString();
            //CurrentArmA = CurrentArmB = RobotConstant.RobotRetract;
        }

        protected override void OnRender(DrawingContext drawingContext)
        {
            base.OnRender(drawingContext);

            if (DesignerProperties.GetIsInDesignMode(this))
            {
                return;
            }
            if (RobotMoveInfo != null)
            {
                var needMove = CurrentPosition != RobotMoveInfo.BladeTarget || CurrentAction != RobotMoveInfo.Action;

                if (needMove)
                {
                    //LogMsg($" RobotMoveInfo, action:{RobotMoveInfo.Action}  armTarget:{RobotMoveInfo.ArmTarget} bladeTarget:{RobotMoveInfo.BladeTarget}");

                    Invoke(() => MoveRobot(RobotMoveInfo));

                    CurrentAction = RobotMoveInfo.Action;
                    CurrentPosition = RobotMoveInfo.BladeTarget;
                }
            }
        }


        private void UpdateWafer(RobotMoveInfo moveInfo)
        {
            /*
            bool waferPresent = false;
            switch (moveInfo.Action)
            {
                case RobotAction.None:
                case RobotAction.Moving:
                    return;
                case RobotAction.Picking:
                    waferPresent = true;
                    break;
                case RobotAction.Placing:
                    waferPresent = false;
                    break;
                default:
                    break;
            }*/

            switch (moveInfo.ArmTarget)
            {
                case RobotArm.ArmA:
                    //	WaferPresentA = waferPresent;
                    break;
                case RobotArm.ArmB:
                    //	WaferPresentB = waferPresent;
                    break;
                case RobotArm.Both:
                    //	WaferPresentA = waferPresent;
                    //	WaferPresentB = waferPresent;
                    break;
                default:
                    break;
            }
        }


        private void Action(int[] angles, Action onComplete = null)
        {
            var storyboard = new Storyboard();
            storyboard.Completed += (s, e) => onComplete?.Invoke();
            var needRotate = new List<bool>
            {
                canvas1.Rotate(storyboard, angles[0], true, MoveTime),
                canvas2.Rotate(storyboard,angles[1], true, MoveTime),
                canvas3.Rotate(storyboard, angles[2], true, MoveTime),
                canvas21.Rotate(storyboard, angles[4], true, MoveTime),
                canvas22.Rotate(storyboard, angles[5], true, MoveTime),
                canvas23.Rotate(storyboard, angles[6], true, MoveTime),
            };

            if (needRotate.Any(x => x))
            {
                storyboard.Begin();
            }
            else
            {
                onComplete?.Invoke();
            }
        }

        private void Translate(string station, Action onComplete = null)
        {
            if (station == null)
                return;
            if (CanvasRotates.Keys.Any(s => s.Contains(station.Split('.')[1])))
            {
                canvasRoot.Rotate(CanvasRotates[station.Split('.')[1]]);
            }
            var translateX = StationPosition.Rotations[station];
            TranslateX = translateX;
            TranslateTo(onComplete);
        }

        private void TranslateTo(Action onComplete)
        {
            if (translate.X != TranslateX)
            {
                var animation = new DoubleAnimation(translate.X, TranslateX, new Duration(TimeSpan.FromMilliseconds(MoveTime)));
                animation.Completed += (s, e) => onComplete?.Invoke();
                translate.BeginAnimation(TranslateTransform.XProperty, animation);
            }
            else
            {
                onComplete?.Invoke();
            }
        }

        private void MoveRobot(RobotMoveInfo moveInfo)
        {
            var updateWafer = new Action(() => UpdateWafer(moveInfo));

            canvas1.Stop();
            canvas2.Stop();
            canvas3.Stop();
            canvas2.Stop();
            canvas21.Stop();
            canvas22.Stop();
            canvas23.Stop();

            var target = moveInfo.BladeTarget;
            var arm = moveInfo.ArmTarget;

            Action(StationPosition.Home, () => Translate(moveInfo.BladeTarget, () =>
            {
                var moved = false;
                string modulename = moveInfo.BladeTarget.Split('.')[1];
                if (moveInfo.ArmTarget == RobotArm.ArmA)
                {
                    switch (moveInfo.Action)
                    {
                        case RobotAction.None:
                            break;
                        case RobotAction.Picking:
                        case RobotAction.Placing:
                            if (modulename == "Flipper")
                            {
                                Action(StationPosition.Arm1Extend, updateWafer);
                            }
                            else if (modulename == "Buffer" || modulename == "Aligner2")
                            {
                                Action(StationPosition.Arm3Extend, updateWafer);
                            }
                            else if ( modulename == "Aligner1")
                            {
                                Action(StationPosition.Arm4Extend, updateWafer);
                            }
                            else if(modulename == "Cooling1")
                            {
                                Action(StationPosition.Arm7Extend, updateWafer);
                            }
                            else if(modulename == "Cooling2")
                            {
                                Action(StationPosition.Arm8Extend, updateWafer);
                            }
                            else
                            {
                                Action(StationPosition.Arm5Extend, updateWafer);
                            }
                            
                            moved = true;
                            break;
                        case RobotAction.Moving:
                            break;
                        default:
                            break;
                    }
                }
                else if (moveInfo.ArmTarget == RobotArm.ArmB)
                {
                    switch (moveInfo.Action)
                    {
                        case RobotAction.None:
                            break;
                        case RobotAction.Picking:
                        case RobotAction.Placing:
                            if (modulename == "Flipper")
                            {
                                Action(StationPosition.Arm2Extend, updateWafer);
                            }
                            else if (modulename == "Buffer" || modulename == "Aligner2")
                            {
                                Action(StationPosition.Arm31Extend, updateWafer);
                            }
                            else if (modulename == "Aligner1" )
                            {
                                Action(StationPosition.Arm41Extend, updateWafer);
                            }
                            else if (modulename == "Cooling1")
                            {
                                Action(StationPosition.Arm71Extend, updateWafer);
                            }
                            else if (modulename == "Cooling2")
                            {
                                Action(StationPosition.Arm81Extend, updateWafer);
                            }
                            else
                            {
                                Action(StationPosition.Arm6Extend, updateWafer);
                            }
                            
                            moved = true;
                            break;
                        case RobotAction.Moving:
                            break;
                        default:
                            break;
                    }
                }

                if (!moved && updateWafer != null)
                {
                    updateWafer();
                }
            }));
        }




        private void MoveTo(object obj)
        {
            MoveRobot((RobotMoveInfo)obj);
        }

        private void Invoke(Action action)
        {
            Dispatcher.Invoke(action);
        }

        private void LogMsg(string msg)
        {
            var source = "ATMRobot";
            Console.WriteLine("{0} {1}", source, msg);
        }
    }

    //public class RobotPosition
    //{
    //    public int? X;
    //    public int Root;
    //    public int Arm;
    //    public int Hand;
    //}

    //public class StationPosition
    //{
    //    public RobotPosition StartPosition;
    //    public RobotPosition EndPosition;
    //}
    public class VTMRobotPosition
    {
        public Dictionary<string, int> Rotations;
        public int[] Arm1Extend;
        public int[] Arm2Extend;
        public int[] Home;

        public int[] Arm3Extend;
        public int[] Arm4Extend;
        public int[] Arm5Extend;
        public int[] Arm6Extend;
        internal int[] Arm31Extend;
        internal int[] Arm41Extend;

        public int[] Arm7Extend;
        public int[] Arm71Extend;
        public int[] Arm8Extend;
        public int[] Arm81Extend;
    }

    public class RobotPosition
    {
        public int X;
        public int Z;
        public int Root;
        public int Arm;
        public int Hand;
    }

    public class StationPosition
    {
        public RobotPosition StartPosition;
        public RobotPosition EndPosition;
    }

    public class RobotConstant
    {
        public const string RobotRetract = "0";
        public const string RobotExtend = "1";
        public const string UnknownTarget = "Unknown";
    }
}