BPASmartClient/BPASmartClient.Lebai/LebaiRobot.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using Lebai.SDK;
using Lebai.SDK.Dtos;
using Robotc;
using BPASmartClient.Message;
using BPASmartClient.Helper;
using TaskStatus = Lebai.SDK.Dtos.TaskStatus;
using BPASmartClient.Peripheral;
using BPASmartClient.EventBus;
using static BPASmartClient.EventBus.EventBus;
using BPASmartClient.Model;

namespace BPASmartClient.Lebai
{
    public class LebaiRobot: BasePeripheral
    {

        //public const int SENCE_自嗨 = 10008;
        //public const int SENCE_接冰淇淋1 = 10004;
        //public const int SENCE_接冰淇淋2 = 10003;
        //public const int SENCE_接冰淇淋3 = 10005;
        //public const int SENCE_咖啡 = 10006;
        //#region 新增场景
        //public const int SENCE_取咖啡杯 = 10013;
        //public const int SENCE_咖啡杯检测 = 10014;
        //public const int SENCE_再次取杯 = 10017;
        //public const int SENCE_接咖啡 = 10015;
        //public const int SENCE_送咖啡 = 10016; //无取走咖啡检测配合场景
        //public const int SENCE_New送咖啡 = 10021;//有取走咖啡检测配合场景
        //public const int SENCE_有无咖啡检测 = 10019;
        //public const int SENCE_再送咖啡 = 10020;
        //public const int SENCE_复位 = 10018;
        //#endregion
        //public const int SENCE_欢迎 = 10002;


        #region 且时且多设备
        //取咖啡杯 set：=1    get:=101	10031
        //取冰淇淋杯 set：=1    get:=101	10032
        //咖啡杯检测 set：=1    get:=101	10033
        //冰淇淋杯检测 set：=1    get:=101	10034
        //二次取咖啡杯 set：=1    get:=101	10035
        //二次取冰淇淋杯 set：=1    get:=101	10036
        //接咖啡 set：=1    get:=101	10037
        //接冰淇淋公共点 set：=1    get:=101	10038
        //接1号冰淇淋 set：=1    get:=101	10039
        //接2号冰淇淋 set：=1    get:=101	10040
        //接3号冰淇淋 set：=1    get:=101	10041
        //放咖啡位置 set：=1    get:=101	10042
        //放冰淇淋位置 set：=1    get:=101	10043

        public const int SENCE_取咖啡杯 = 10031;
        public const int SENCE_取冰淇淋杯 = 10032;
        public const int SENCE_咖啡杯检测 = 10033;
        public const int SENCE_冰淇淋杯检测 = 10034;
        public const int SENCE_二次取咖啡杯 = 10035;
        public const int SENCE_二次取冰淇淋杯 = 10036;
        public const int SENCE_接咖啡 = 10037;
        public const int SENCE_接冰淇淋公共点 = 10038;
        public const int SENCE_接1号冰淇淋 = 10039;
        public const int SENCE_接2号冰淇淋 = 10040;
        public const int SENCE_接3号冰淇淋 = 10041;
        public const int SENCE_放咖啡位置 = 10042;
        public const int SENCE_放冰淇淋位置 = 10043;

        //add 新加场景
        public const int SENCE_接咖啡后回原点 = 10051;
        public const int SENCE_咖啡杯回原点 = 10050;
        public const int SENCE_冰淇淋杯回原点 = 10049;
        public const int SENCE_取咖啡出餐 = 10052;
        #endregion

        /// <summary>
        /// IP地址
        /// </summary>
        public string IpAddress { get; set; }
        /// <summary>
        /// 抓手上传感器输入位索引
        /// </summary>
        public int HandSensor { get; set; }
        /// <summary>
        /// 输出信号量索引
        /// </summary>
        public int OutputSingalValue { get; set; }


        private volatile static LebaiRobot _Instance;
        public static LebaiRobot GetInstance => _Instance ?? (_Instance = new LebaiRobot());
        private LebaiRobot() { }
        private LebaiRobotClient client;
        private RobotData robotData;
        public bool IsIdle { get; set; } = false;
        public bool IsConnected { get { return null != robotData; } }

        public void Connect(string ip)
        {
            bool ErrorFlag = false;
            while (robotData == null)
            {
                try
                {
                    client = new LebaiRobotClient(ip);
                    robotData = client.GetRobotData().Result;
                }
                catch (Exception ex)
                {
                    if (!ErrorFlag)
                    {
                        MessageLog.GetInstance.ShowEx(ex.ToString());
                        ErrorFlag = true;
                    }
                    Thread.Sleep(3000);
                }
            }
            StartRobot();
            MessageLog.GetInstance.Show("乐百机器人连接成功！");
        }
        /// <summary>
        /// 机器人重连检测
        /// </summary>
        /// <param name="ip"></param>
        public void Reconnect(string ip)
        {
            if (client == null)
            {
                try
                {
                    client = new LebaiRobotClient(ip);
                }
                catch (Exception)
                {

                    // throw;
                }
            }
        check:
            while (robotData != null)
            {

                try
                {
                    robotData = client.GetRobotData().Result;
                }
                catch (Exception)
                {
                    robotData = null;
                    // throw;
                }
            }
            MessageLog.GetInstance.Show("乐白机器人断开连接，准备重连");
            int num = 0;
            while (num < 6 && robotData == null)
            {
                try
                {

                    robotData = client.GetRobotData().Result;
                }
                catch (Exception ex)
                {
                    if (num == 5)
                    {
                        MessageLog.GetInstance.ShowEx(ex.ToString());
                    }
                }

                if (num < 5 && robotData == null)
                {
                    Thread.Sleep(1000);
                    MessageLog.GetInstance.Show($"乐白机器人正在尝试第{num + 1}次重连....");
                }
                else if (num >= 5 && robotData == null)
                {
                    MessageLog.GetInstance.Show("乐白机器人重连失败，请检查硬件连接");
                    //return;
                }
                num++;
            }
            if (robotData != null)
            {
                StartRobot();
                MessageLog.GetInstance.Show("乐白机器人重连成功");
            }
            goto check;
        }

        public void GetRobotModeStatus()
        {

            if (robotData == null) return;
            int mode = robotData.RobotMode.Mode;
            IsIdle = mode == 5;
            for (int i = 0; i < 14; i++)
            {
                if (RTrig.GetInstance(((ELebaiRModel)i).ToString()).Start(mode == i))
                    MessageLog.GetInstance.Show(((ELebaiRModel)i).ToString());
            }
        }
        public void GetSpeed()
        {
            //client.StartSys().GetAwaiter();
            var aa = client.GetActualTcpSpeed().GetAwaiter();
            //robotData.RobotMode.Mode
        }

        /// <summary>
        /// 启动机器人
        /// </summary>
        public async void StartRobot()
        {
            if (robotData != null)
            {
                await client.StartSys();
                await client.Sync();
                MessageLog.GetInstance.Show("机器人启动成功");
            }
        }

        /// <summary>
        /// 启动示教
        /// </summary>
        public async void StartTeachMode()
        {
            try
            {
                if (robotData != null)
                {
                    await client.TeachMode();
                    MessageLog.GetInstance.Show("机器人切换为示教模式.");
                }
            }
            catch (Exception ex)
            {
                MessageLog.GetInstance.ShowEx(ex.ToString());
            }
        }

        /// <summary>
        /// 结束示教
        /// </summary>
        public async void EndtTeachMode()
        {
            try
            {
                if (robotData != null)
                {
                    await client.EndTeachMode();
                    MessageLog.GetInstance.Show("机器人切换为停止示教模式.");
                }
            }
            catch (Exception ex)
            {
                MessageLog.GetInstance.ShowEx(ex.ToString());
            }
        }

        /// <summary>
        /// 机器人急停
        /// </summary>
        public async void EStopRobot()
        {
            try
            {
                if (robotData != null)
                {
                    await client.EStop();
                    MessageLog.GetInstance.Show("机器人急停");
                }
            }
            catch (Exception ex)
            {
                MessageLog.GetInstance.ShowEx(ex.ToString());
            }
        }

        /// <summary>
        /// 获取抓手重量
        /// </summary>
        /// <returns></returns>
        public double GetClawWdight()
        {
            if (robotData != null)
            {
                return client.GetClawWeight().Result.Weight_;
            }
            return 0;
        }

        /// <summary>
        /// 获取信号量
        /// </summary>
        /// <param name="index"></param>
        /// <returns></returns>
        public SignalResult? GetValueAsync(int index = 0)
        {
            if (robotData == null) return default(SignalResult);
            SignalValue signalValue = new SignalValue();
            signalValue.Index = index;
            return client?.GetSignal(signalValue).Result;
        }

        /// <summary>
        /// 设置信号量
        /// </summary>
        /// <param name="index"></param>
        /// <param name="value"></param>
        /// <returns></returns>
        public SignalResult? SetValue(int value, int index = 0)
        {
            if (robotData == null) return default;
            SignalValue signalValue = new SignalValue();
            signalValue.Index = index;
            signalValue.Value = value;
            return client.SetSignal(signalValue).Result;
        }

        /// <summary>
        /// 获取输入DI状态
        /// </summary>
        /// <param name="pin"></param>
        /// <returns></returns>
        public bool GetInput(int pin = 0)
        {
            if (client == null) return false;
            var res = client.GetDIO(new IOPin() { Pin = pin }).Result;
            if (res != null)
            {
                return res.Value == 1 ? true : false;
            }
            return false;
        }

        /// <summary>
        /// 运行指定的场景
        /// </summary>
        /// <param name="id"></param>
        public async void Scene(int id)
        {
            CancellationToken cancellationToken = default(CancellationToken);
            var result = await client.GetTasks(new GetTasksInput { PageIndex = 1, PageSize = 1 }, cancellationToken);
            var first = result?.Items?.FirstOrDefault();
            var r = first == null || first.Status != TaskStatus.Running && first.Status != TaskStatus.Pause;
            while (!r)
            {
                Thread.Sleep(5);
                result = await client.GetTasks(new GetTasksInput { PageIndex = 1, PageSize = 1 }, cancellationToken);
                first = result?.Items?.FirstOrDefault();
                r = first == null || first.Status != TaskStatus.Running && first.Status != TaskStatus.Pause;
            }
            while (GetValueAsync().Value != 0)
            {
                Thread.Sleep(500);
            }
            Thread.Sleep(1000);
            if (robotData == null || client == null) return;
            //if (!client.GetIsCanRunTask().Result) return;
            await client?.RunScene(id);
            MessageLog.GetInstance.Show($"调用场景：{id}");
        }

        protected override void InitStatus()
        {
            status["Connected"] = false;
            status["HandSensor"] = 0;
            status["OutputSingalValue"] = 0;
        }

        public override void Start()
        {
            Connect(IpAddress);
            
        }

        public override void Stop()
        {
        }

        public override void Init()
        {
            EventBus.EventBus.GetInstance().Subscribe<Demo_MakeCoffeeEvent>(DeviceId, delegate (IEvent @event, EventCallBackHandle callBack )
            {

            });
            InitStatus();
        }
    }
}