Display_D_HP/TeamAAS-VM/ViewModels/Product/TestProductPageViewModel.cs

using Cognex.VisionPro;
using Cognex.VisionPro.ToolBlock;
using MaterialDesignThemes.Wpf;
using MathNet.Numerics.LinearAlgebra;
using Prism.Commands;
using Prism.Events;
using Prism.Ioc;
using Prism.Mvvm;
using Prism.Regions;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Linq;
using System.Threading.Tasks;
using System.Windows;
using Team.FFFeederService;
using Team.FFFeederService.Interfaces;
using TeamAAS_VP.Controls;
using TeamAAS_VP.Core;
using TeamAAS_VP.Enums;
using TeamAAS_VP.Interfaces;
using TeamAAS_VP.Models;
using TeamAAS_VP.Models.Product;
using TeamAAS_VP.Models.Robot;
using TeamAAS_VP;
using TeamAAS_VP.Resources.Languages;

namespace TeamAAS_VP.ViewModels.Product
{
    public class TestProductPageViewModel : BindableBase
    {
        #region 字段
        IRegionManager _regionManager;
        IEventAggregator _eventAggregator;
        IContainerProvider _container;
        IFeederService _feederService;
        ICameraService _cameraService;
        IRobotService _robotService;
        IConfigService _configService;
        ICalibrationService _calibrationService;

        #endregion

        #region 属性
        private Management _management;

        public Management management
        {
            get { return _management; }
            set { SetProperty(ref _management,value); }
        }

        private ProductModel _SelectProduct;

        /// <summary>
        /// 选中的产品
        /// </summary>
        public ProductModel SelectProduct
        {
            get { return _SelectProduct; }
            set { SetProperty(ref _SelectProduct, value); }
        }

        //------------------------------------------------------------------------------------------------------------Feeder 取料--------------------------------------------------------------------
        private ProcedureModel _SelectProcedure;

        /// <summary>
        /// 选中的流程
        /// </summary>
        public ProcedureModel SelectProcedure
        {
            get { return _SelectProcedure; }
            set
            {
                SetProperty(ref _SelectProcedure, value);
                if (value != null)
                {
                    Robot = _robotService.GetRobot(value.RobotId);
                    Camera=_cameraService.GetCamera(value.CameraId);
                    Feeder= _feederService.GetFeeder(value.FeederId);
                }
            }
        }

        private ObservableCollection<ProcedureModel> _ProcedureModels;

        /// <summary>
        /// 流程集合
        /// </summary>
        public ObservableCollection<ProcedureModel> ProcedureModels
        {
            get { return _ProcedureModels; }
            set { SetProperty(ref _ProcedureModels, value); }
        }

        private RPoint _SelectPoint;

        public RPoint SelectPoint
        {
            get { return _SelectPoint; }
            set { SetProperty(ref _SelectPoint, value); }
        }

        private IRobot _Robot;

        public IRobot Robot
        {
            get { return _Robot; }
            set { SetProperty(ref _Robot, value); }
        }

        private ICamera _Camera;

        public ICamera Camera
        {
            get { return _Camera; }
            set { SetProperty(ref _Camera, value); }
        }

        public IVoiceCoilMotorFeeder Feeder { get; set; }

        private int _Inhal = -1;

        public int Inhal
        {
            get { return _Inhal; }
            set { SetProperty(ref _Inhal, value); }
        }

        private int _Blow = -1;

        public int Blow
        {
            get { return _Blow; }
            set { SetProperty(ref _Blow, value); }
        }

        private ICogImage _Image;

        public ICogImage Image
        {
            get { return _Image; }
            set { SetProperty(ref _Image, value); }
        }

        private Cognex.VisionPro.CogGraphicCollection _Graphic;

        public Cognex.VisionPro.CogGraphicCollection Graphic
        {
            get { return _Graphic; }
            set { SetProperty(ref _Graphic, value); }
        }

        //------------------------------------------------------------------------------------------------------------下相机定位--------------------------------------------------------------------
        private ProcedureModel _SelectProcedure1;

        /// <summary>
        /// 选中的流程
        /// </summary>
        public ProcedureModel SelectProcedure1
        {
            get { return _SelectProcedure1; }
            set
            {
                SetProperty(ref _SelectProcedure1, value);
                if (value != null)
                {
                    Robot1 = _robotService.GetRobot(value.RobotId);
                    Camera1 = _cameraService.GetCamera(value.CameraId);
                    Feeder1 = _feederService.GetFeeder(value.FeederId);
                }
            }
        }

        private ObservableCollection<ProcedureModel> _ProcedureModels1;

        /// <summary>
        /// 流程集合
        /// </summary>
        public ObservableCollection<ProcedureModel> ProcedureModels1
        {
            get { return _ProcedureModels1; }
            set { SetProperty(ref _ProcedureModels1, value); }
        }

        private RPoint _SelectPoint1;

        public RPoint SelectPoint1
        {
            get { return _SelectPoint1; }
            set { SetProperty(ref _SelectPoint1, value); }
        }

        private bool _IsCreateTool=true;

        public bool IsCreateTool
        {
            get { return _IsCreateTool; }
            set { SetProperty(ref _IsCreateTool, value); }
        }

        public IRobot Robot1 { get; set; }

        public ICamera Camera1 { get; set; }

        public IVoiceCoilMotorFeeder Feeder1 { get; set; }

        private int _Inhal1 = -1;

        public int Inhal1
        {
            get { return _Inhal1; }
            set { SetProperty(ref _Inhal1, value); }
        }

        private int _Blow1 = -1;

        public int Blow1
        {
            get { return _Blow1; }
            set { SetProperty(ref _Blow1, value); }
        }

        private ICogImage _Image1;

        public ICogImage Image1
        {
            get { return _Image1; }
            set { SetProperty(ref _Image1, value); }
        }

        private Cognex.VisionPro.CogGraphicCollection _Graphic1;

        public Cognex.VisionPro.CogGraphicCollection Graphic1
        {
            get { return _Graphic1; }
            set { SetProperty(ref _Graphic1, value); }
        }

        private bool _IsRunning = true;

        public bool IsRunning
        {
            get { return _IsRunning; }
            set { SetProperty(ref _IsRunning, value); }
        }

        

        #endregion

        #region 命令
        private DelegateCommand _LoadedCommand;
        public DelegateCommand LoadedCommand =>
            _LoadedCommand ?? (_LoadedCommand = new DelegateCommand(ExecuteLoadedCommand));

        private DelegateCommand _AutoPickCommand;
        public DelegateCommand AutoPickCommand =>
            _AutoPickCommand ?? (_AutoPickCommand = new DelegateCommand(ExecuteAutoPickCommand).ObservesCanExecute(() => IsRunning));

        private DelegateCommand _AutoPhotoCommand;
        public DelegateCommand AutoPhotoCommand =>
            _AutoPhotoCommand ?? (_AutoPhotoCommand = new DelegateCommand(ExecuteAutoPhotoCommand).ObservesCanExecute(() => IsRunning));

        private DelegateCommand<object> _BlowOrInhalControlCommand;
        public DelegateCommand<object> BlowOrInhalControlCommand =>
            _BlowOrInhalControlCommand ?? (_BlowOrInhalControlCommand = new DelegateCommand<object>(ExecuteBlowOrInhalControlCommand).ObservesCanExecute(() => IsRunning));

        private DelegateCommand<object> _BlowOrInhalControl1Command;
        public DelegateCommand<object> BlowOrInhalControl1Command =>
            _BlowOrInhalControl1Command ?? (_BlowOrInhalControl1Command = new DelegateCommand<object>(ExecuteBlowOrInhalControl1Command).ObservesCanExecute(() => IsRunning));
        #endregion

        #region 事件

        #endregion
        public TestProductPageViewModel(IRegionManager regionManager, IEventAggregator ea, IContainerProvider container,
            IFeederService feederService,ICameraService cameraService,IRobotService robotService, IConfigService configService,
            ICalibrationService calibrationService)
        {
            _regionManager = regionManager;
            _eventAggregator = ea;
            _container = container;
            _feederService = feederService;
            _cameraService = cameraService;
            _robotService = robotService;
            _configService = configService;
            _calibrationService = calibrationService;
            management = _container.Resolve<Management>();
        }

        #region 方法
        void ExecuteLoadedCommand()
        {
            management = _container.Resolve<Management>();
            var productsViewModel = _container.Resolve<ProductsViewModel>();
            //SelectProduct = productsViewModel.SelectProduct;
            ProcedureModels = new ObservableCollection<ProcedureModel>();
            foreach (var item in SelectProduct.CameraProcedures)
            {
                foreach (var item1 in item.ProcedureModels)
                {
                    if (item1.RobotId!=Guid.Empty && item1.FeederId!= Guid.Empty && item1.CameraId!= Guid.Empty)
                    {
                        ProcedureModels.Add(item1);
                    }
                    if (item1.RobotId != Guid.Empty && item1.FeederId == Guid.Empty && item1.CameraId != Guid.Empty)
                    {
                        ProcedureModels1.Add(item1);
                    }
                }
            }
        }

        async void ExecuteBlowOrInhalControlCommand(object isInhal)
        {
            try
            {
                if (Robot == null || !Robot.IsConnected)
                {
                    return;
                }
                IsRunning = false;
                Robot.Timeout = 10000;
                //发送校准的参数
                await Robot.CalibParameAsync(new PickInfo() { Blow= Blow , Inhal= Inhal, PickPlaceModel= PickPlaceModel.Inhal}, 20, 50, false, 300, 300);
                await Robot.CalibOutIOAsync(!(bool)isInhal);
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError(Lang.示教取料点时出错, ex);
                MessageBox.Show(ex.Message);
            }
            finally
            {
                IsRunning = true;
            }

        }

        async void ExecuteBlowOrInhalControl1Command(object isInhal)
        {
            try
            {
                if (Robot == null || !Robot.IsConnected)
                {
                    return;
                }
                IsRunning = false;
                Robot.Timeout = 10000;
                //发送校准的参数
                await Robot.CalibParameAsync(new PickInfo() { Blow = Blow1, Inhal = Inhal1, PickPlaceModel = PickPlaceModel.Inhal }, 20, 50, false, 300, 300);
                await Robot.CalibOutIOAsync(!(bool)isInhal);
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError(Lang.示教取料点时出错, ex);
                MessageBox.Show(ex.Message);
            }
            finally
            {
                IsRunning = true;
            }

        }

        /// <summary>
        /// 自动取料
        /// </summary>
        async void ExecuteAutoPickCommand()
        {
            if (SelectPoint==null || SelectProcedure==null)
            {
                return;
            }

            if (!(Robot!=null && Robot.IsConnected))
            {
                return;
            }

            try
            {
                if (Robot == null || !Robot.IsConnected)
                {
                    return;
                }
                IsRunning = false;
                Robot.Timeout = 1000000;
                await Robot.SLockAsync();
                //发送校准的参数
                await Robot.CalibParameAsync(new PickInfo() { Blow = Blow, Inhal = Inhal, PickPlaceModel = PickPlaceModel.Inhal }, 20, 50, false, 300, 300);

                var pos = SelectPoint.Clone();
                //pos.Z = 0;
                //移动机器人至待机位置
                await Robot.CalibMotionAsync(pos, 0);
                //打开光源
                await Feeder.OpenLightAsync();
                await Feeder.SetLightValueAsync((ushort)SelectProcedure.LightValue);

                var calib = _calibrationService.GetAllCalibrations().FirstOrDefault(cal => cal.Id == SelectProcedure.CalibrationId);
                if (calib == null)
                {
                    throw new Exception(Lang.没有可用的校准);
                }
                //关闭吸气
                await Robot.CalibOutIOAsync(false);
                await Task.Delay(300);

                //移动机器人至待机位置
                await Robot.CalibMotionAsync(pos, 0);

                var campos = await Photo(SelectProcedure, Camera);

                double angle = campos.Item3;
                if (calib.CameraMount == CameraMount.FixedDown || calib.CameraMount == CameraMount.MobileJ2 || calib.CameraMount == CameraMount.MobileJ4)
                {
                    angle *= -1;
                }

                //得到校准矩阵
                Matrix<double> matrix = Matrix<double>.Build.DenseOfArray(calib.AffineTransformationMaterial);
                //将像素坐标转换成机器人坐标
                var rpos = matrix * Vector<double>.Build.Dense(new double[] { campos.Item1, campos.Item2, 1 });

                RPoint Target = pos.Clone();
                Target.X = (float)rpos[0];
                Target.Y = (float)rpos[1];
                Target.Z = (float)SelectProcedure.ToolInfo[0].OffsetZ;
                Target.U = (float)(angle + SelectProcedure.ToolInfo[0].OffsetU);
                Target.Tool = 2;

                //取料
                await Robot.CalibMotionAsync(Target, 0);
                await Task.Delay(200);
                await Robot.CalibOutIOAsync(true);

                //移动机器人至待机位置
                await Robot.CalibMotionAsync(pos, 0);

                //打开光源
                await Feeder.CloseLightAsync();
                MessageBox.Show(Lang.完成);
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.ToString());
            }
            finally
            {
                IsRunning = true;
            }

        }

        /// <summary>
        /// 自动下相机拍照
        /// </summary>
        async void ExecuteAutoPhotoCommand()
        {
            if (SelectPoint1 == null || SelectProcedure1 == null)
            {
                return;
            }

            if (!(Robot1 != null && Robot1.IsConnected))
            {
                return;
            }

            try
            {
                if (Robot1 == null || !Robot1.IsConnected)
                {
                    return;
                }
                IsRunning = false;
                Robot1.Timeout = 1000000;
                await Robot1.SLockAsync();
                //发送校准的参数
                await Robot1.CalibParameAsync(new PickInfo() { Blow = Blow1, Inhal = Inhal1, PickPlaceModel = PickPlaceModel.Inhal }, 20, 50, false, 300, 300);

                var pos = SelectPoint1.Clone();
                //pos.Z = 0;
                //移动机器人至待机位置
                await Robot1.CalibMotionAsync(pos, 0);

                var calib = _calibrationService.GetAllCalibrations().FirstOrDefault(cal => cal.Id == SelectProcedure1.CalibrationId);
                if (calib == null)
                {
                    throw new Exception(Lang.没有可用的校准);
                }

                var campos = await Photo(SelectProcedure1, Camera1);

                double angle = campos.Item3;
                if (calib.CameraMount == CameraMount.FixedDown || calib.CameraMount == CameraMount.MobileJ2 || calib.CameraMount == CameraMount.MobileJ4)
                {
                    angle *= -1;
                }

                //得到校准矩阵
                Matrix<double> matrix = Matrix<double>.Build.DenseOfArray(calib.AffineTransformationMaterial);
                //将像素坐标转换成机器人坐标
                var rpos = matrix * Vector<double>.Build.Dense(new double[] { campos.Item1, campos.Item2, 1 });

                await Robot1.SelectToolAsync(1);
                var curpos=await Robot1.GetRobotPosAsync();

                //根据机器人当前的点位计算工具坐标
                ToolCoord tool = new ToolCoord();
                tool.ComputeTool(curpos.X, curpos.Y, curpos.U, rpos[0], rpos[1]);
                rpos = Vector<double>.Build.Dense(new double[] { tool.X, tool.Y });
                await Robot1.SetToolAsync(3, rpos[0], rpos[1]);
                MessageBox.Show(Lang.完成);
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.ToString());
            }
            finally
            {
                IsRunning = true;
            }
        }

        /// <summary>
        /// 相机拍照
        /// </summary>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        private async Task<Tuple<double, double, double>> Photo(ProcedureModel SelectProcedure, ICamera Camera)
        {
            if (Feeder != null)
            {
                await Feeder.OpenLightAsync();
                await Feeder.SetLightValueAsync((ushort)SelectProcedure.LightValue);
                await Task.Delay(200);
            }

            //采集图像
            if (Camera != null)
            {
                SelectProcedure.ToolBlock.Inputs["InputImage"].Value = Camera.Grab();
                Image = (ICogImage)SelectProcedure.ToolBlock.Inputs["InputImage"].Value;
            }
            Graphic = null;
            SelectProcedure.ToolBlock.Run();

            if (Feeder != null)
            {
                await Feeder.CloseLightAsync();
            }
            CogToolBlockTerminalCollection outputCollection = SelectProcedure.ToolBlock.Outputs;
            Graphic = outputCollection["Graphic"].Value as CogGraphicCollection;
            bool Found = (bool)(outputCollection["Found"].Value);
            if (Found)
            {
                var strpoints = outputCollection["Point"].Value.ToString();
                string[] items = strpoints.Split(',');
                var calib = _calibrationService.GetAllCalibrations().FirstOrDefault(cal => cal.Id == SelectProcedure.CalibrationId);
                if (calib == null)
                {
                    throw new Exception(Lang.没有可用的校准);
                }
                Matrix<double> matrix = Matrix<double>.Build.DenseOfArray(calib.AffineTransformationMaterial);
                double x = double.Parse(items[0]);
                double y = double.Parse(items[1]);
                double angle = double.Parse(items[2]);
                var rpos = matrix * Vector<double>.Build.Dense(new double[] { x, y, 1 });
                return new Tuple<double, double, double>(rpos[0], rpos[1], angle);
            }
            else
            {
                throw new Exception(Lang.拍照失败); 
            }

        }
        #endregion
    }
}