ArmDogControl/DogAndArmControl/Form1.cs


using DevExpress.Utils.Extensions;
using DevExpress.XtraPrinting.Preview;
using RoboticControlApp;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Diagnostics;
using System.Drawing;
using System.IO;
using System.Linq;
using System.Net.Sockets;
using System.Net.WebSockets;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Windows.Forms;
using Newtonsoft.Json;
using DevExpress.XtraPrinting.Native.WebClientUIControl;

namespace DogAndArmControl
{
    public partial class Form1 : DevExpress.XtraEditors.XtraForm
    {
        private TcpClient mergedVideoClient; // 用于接收合并视频流的客户端
        private NetworkStream mergedVideoStream; // 合并视频流
        private Task videoReceiveTask;
        // private ClientWebSocket webSocket; // 移除WebSocket
        private CancellationTokenSource videoCancellationTokenSource;
        private CancellationTokenSource cancellationTokenSource;

        private NetworkStream controlStream; // 新增：用于控制的NetworkStream
        private string currentControlMode = "DOG"; // 新增：当前控制模式，默认为DOG

        private VideoCaltulate vic = new VideoCaltulate();
        private bool Dogbutton = false;
        private bool Armbutton = false;
        private System.Windows.Forms.Timer keyRepeatTimer;
        private Keys currentKey;

        // 机械狗控制按钮
        private ModernButton wDogButton, sDogButton, aDogButton, dDogButton; // 移动按钮
        private ModernButton qDogButton, eDogButton, lDogButton, zDogButton, cDogButton, rDogButton, tDogButton, fDogButton; // 其他功能按钮

        public Form1()
        {
            InitializeComponent();
            Init();
        }

        private void Init()
        {
            IpEdit.Text = "192.168.123.18";
            PortEdit.Text = "8088";
            ArmIp.Text = "192.168.123.18"; // 机械臂IP，现在也用于TCP控制
            ArmPort.Text = "8000"; // 机械臂端口，现在也用于TCP控制

            DogVideo.SizeMode = PictureBoxSizeMode.Zoom;
            ArmVideo.SizeMode = PictureBoxSizeMode.Zoom;

            IpEdit.ReadOnly = true;
            PortEdit.ReadOnly = true;
            ArmIp.ReadOnly = true;
            ArmPort.ReadOnly = true;

            this.KeyDown += Form1_KeyDown;
            this.KeyUp += Form1_KeyUp;
            this.KeyPreview = true;

            UpdateModeDisplay(); // 初始化模式显示
            tabPane1.SelectedPageChanged += TabPane1_SelectedPageChanged; // 监听标签页切换事件
        }

        private async void button1_Click(object sender, EventArgs e)
        {
            try
            {
                if (!Dogbutton)
                {
                    IpEdit.ReadOnly = true;
                    PortEdit.ReadOnly = true;

                    // 只建立一个TCP连接用于接收合并视频流
                    mergedVideoClient = new TcpClient();
                    await mergedVideoClient.ConnectAsync(IpEdit.Text, Convert.ToInt32(PortEdit.Text));
                    mergedVideoStream = mergedVideoClient.GetStream();

                    // 新增：建立控制TCP连接
                    //controlClient = new TcpClient();
                    //await controlClient.ConnectAsync(ArmIp.Text, Convert.ToInt32(ArmPort.Text)); // 使用机械臂的IP和端口进行控制连接
                    controlStream = mergedVideoClient.GetStream();

                    // 自动向服务端发送"MERGED"表示已连接
                    byte[] mergedData = Encoding.ASCII.GetBytes("MERGED");
                    await controlStream.WriteAsync(mergedData, 0, mergedData.Length);

                    videoCancellationTokenSource?.Cancel();
                    videoCancellationTokenSource = new CancellationTokenSource();

                    // 启动接收合并视频流的任务
                    videoReceiveTask = Task.Run(() => vic.ReceiveMergedVideoFramesAsync(
                        videoCancellationTokenSource.Token,
                        mergedVideoStream,
                        ArmVideo,
                        DogVideo));

                    DogStatus.Text = "已连接";
                    DogStatus.ForeColor = Color.Green;
                    Dogbutton = true;
                    button1.Text = "断开";
                    button1.BackColor = Color.Red;
                    DogVideo.BackColor = Color.Black;
                    ArmVideo.BackColor = Color.Black;

                    // 初始模式设置为DOG，并发送CHANGE指令
                    currentControlMode = "DOG";
                    await SendChangeModeCommand("DOG");
                    UpdateModeDisplay();
                }
                else
                {
                    IpEdit.ReadOnly = false;
                    PortEdit.ReadOnly = false;
                    Dogbutton = false;
                    DogStatus.Text = "连接";
                    DogStatus.ForeColor = Color.Red;
                    button1.Text = "连接";
                    button1.BackColor = Color.Blue;

                    videoCancellationTokenSource?.Cancel();
                    mergedVideoClient?.Close();

                    Init();
                }
            }
            catch (Exception ee)
            {
                MessageBox.Show(ee.ToString());
            }
        }

        // 移除原有的button2_Click (WebSocket连接机械臂)
        //private void button2_Click(object sender, EventArgs e)
        //{
        //    try
        //    {
        //        if (!Armbutton)
        //        {
        //            ArmIp.ReadOnly = true;
        //            ArmPort.ReadOnly = true;
        //            Armbutton = true;
        //            string wsUrl = $"ws://{ArmIp.Text}:{ArmPort.Text}/ws/control";
        //            cancellationTokenSource = new CancellationTokenSource();
        //            webSocket = new ClientWebSocket();
        //            webSocket?.ConnectAsync(new Uri(wsUrl), cancellationTokenSource.Token);
        //            button2.BackColor = Color.Green;
        //            ArmStatus.Text = "已连接";
        //            ArmStatus.ForeColor = Color.Green;
        //            button2.Text = "断开";
        //            button2.BackColor = Color.Red;
        //        }
        //        else
        //        {
        //            ArmIp.ReadOnly = false;
        //            ArmPort.ReadOnly = false;
        //            Armbutton = false;
        //            ArmStatus.Text = "未连接";

        //            button2.Text = "断开";
        //            button2.BackColor = Color.Blue;

        //            ArmStatus.ForeColor = Color.Red;
        //            button2.BackColor = Color.Blue;
        //            webSocket?.CloseAsync(WebSocketCloseStatus.NormalClosure, "Closing", CancellationToken.None);
        //            webSocket?.Dispose();
        //        }
        //    }
        //    catch (Exception) { }
        //}

        private async void Form1_KeyDown(object sender, KeyEventArgs e)
        {
            try
            {
                if (keyRepeatTimer == null)
                {
                    currentKey = e.KeyCode;
                    keyRepeatTimer = new System.Windows.Forms.Timer();
                    keyRepeatTimer.Interval = 100;

                    keyRepeatTimer.Tick += async (s, args) => // 修改为async
                    {
                        string key = currentKey.ToString().ToLower();
                        if (e.KeyCode == Keys.Menu) return;

                        if (controlStream != null)
                        {
                            if (currentControlMode == "ARM") // 根据当前模式发送指令
                            {
                                string Armkey = MapKeyToCommand(e.KeyCode);
                                if (Armkey != null)
                                {
                                    await HandleArmKey("down", Armkey);  // 发送按下事件到机械臂
                                }
                            }
                            else if (currentControlMode == "DOG")
                            {
                                byte[] buffer = Encoding.UTF8.GetBytes(key.ToUpper());
                                await controlStream.WriteAsync(buffer, 0, buffer.Length); // 使用统一的controlStream发送
                                // Thread.Sleep(10);  // 确保发送间隔，对于TCP异步发送，这个可能不需要或需要更精细控制
                            }
                        }
                    };
                    keyRepeatTimer.Start();
                }
            }
            catch (IOException) { }
        }

        private async void Form1_KeyUp(object sender, KeyEventArgs e)
        {
            if (controlStream != null)
            {
                if (currentControlMode == "DOG")
                {
                    byte[] buffer = Encoding.ASCII.GetBytes("L");
                    // if (mergedVideoStream != null) // 使用统一的controlStream
                    // {
                    // Task.Run(() => // 直接await
                    // {
                    for (int ii = 0; ii < 5; ii++)
                    {
                        await controlStream.WriteAsync(buffer, 0, buffer.Length);
                        // Thread.Sleep(10);
                    }
                    // });
                    // }
                }
                else if (currentControlMode == "ARM")
                {
                    string key = MapKeyToCommand(e.KeyCode);
                    if (key != null)
                    {
                        await HandleArmKey("up", key);  // 发送松开事件到机械臂
                    }
                }
            }
            if (keyRepeatTimer != null)
            {
                keyRepeatTimer.Stop();
                keyRepeatTimer.Dispose();
                keyRepeatTimer = null;
            }
        }

        // 映射按键到机械臂控制命令
        private string MapKeyToCommand(Keys keyCode)
        {
            switch (keyCode)
            {
                case Keys.W: return "W";  // 前进
                case Keys.S: return "S";  // 后退
                case Keys.A: return "A";  // 左移
                case Keys.D: return "D";  // 右移
                case Keys.I: return "I";  // 上升
                case Keys.K: return "K";  // 下降
                case Keys.M: return "M";  // Roll +（右转）
                case Keys.N: return "N";  // Roll -（左转）
                case Keys.L: return "L";  // Pitch +（前倾）
                case Keys.OemPeriod: return ".";  // Pitch -（后倾）
                case Keys.Oemcomma: return ",";  // Yaw +（顺时针）
                case Keys.OemQuestion: return "/";  // Yaw -（逆时针）
                case Keys.O: return "O";  // 夹爪打开
                case Keys.C: return "C";  // 夹爪关闭
                case Keys.U: return "U";  // 复位
                case Keys.J: return "J";  // 重力补偿
                case Keys.P: return "P";  // 退出
                case Keys.F: return "F";  //预设
                case Keys.G: return "g";  //自动丢入篮筐
                default: return null;  // 如果按下的键不在控制范围内，返回 null
            }
        }

        // 发送控制指令到机械臂 (现在通过TCP发送JSON)
        private async Task HandleArmKey(string type, string key)
        {
            if (controlStream != null )
            {
                //var msg = JsonConvert.SerializeObject(new { key_events = new[] { new { key, type } } });
                var buffer = Encoding.UTF8.GetBytes(key.ToUpper());  // 将控制命令转为字节数组
                await controlStream.WriteAsync(buffer, 0, buffer.Length);  // 通过TCP发送消息
            }
        }

        // 新增：更新模式显示
        private void UpdateModeDisplay()
        {
            // 假设你有一个Label控件用于显示模式，例如 modeLabel
            // 如果没有，你需要在WinForms设计器中添加一个Label控件，并命名为modeLabel
            // 例如：this.modeLabel.Text = $"mode:{currentControlMode}";
            // 这里暂时使用Debug.WriteLine代替，实际应用中请替换为UI控件
            Debug.WriteLine($"mode:{currentControlMode}");
            // 假设ModeLabel是你的Label控件
            if (ModeLabel != null)
            {
                ModeLabel.Text = $"mode:{currentControlMode}";
            }
        }

        // 新增：标签页切换事件处理
        private async void TabPane1_SelectedPageChanged(object sender, DevExpress.XtraBars.Navigation.SelectedPageChangedEventArgs e)
        {
            if (controlStream != null)
            {
                if (e.Page == tabNavigationPage1) // 切换到机械狗标签页
                {
                    currentControlMode = "DOG";
                    await SendChangeModeCommand("DOG");
                }
                else if (e.Page == tabNavigationPage2) // 切换到机械臂标签页
                {
                    currentControlMode = "ARM";
                    await SendChangeModeCommand("ARM");
                }
                UpdateModeDisplay();
            }
        }

        // 新增：发送切换模式命令
        private async Task SendChangeModeCommand(string mode)
        {
            if (controlStream != null)
            {
                byte[] changeCommand = Encoding.ASCII.GetBytes($"CHANGE:{mode}"); // 发送 "CHANGE:DOG" 或 "CHANGE:ARM"
                await controlStream.WriteAsync(changeCommand, 0, changeCommand.Length);
                Debug.WriteLine($"Sent CHANGE command: CHANGE:{mode}");
            }
        }

        //加载机械狗的控件
        private void tabNavigationPage1_Paint(object sender, PaintEventArgs e)
        {
            //移动控制按钮
            wDogButton = new ModernButton("W", "前进");
            wDogButton.Location = new Point(150, 80);
            wDogButton.Size = new Size(60, 60);
            tabNavigationPage1.AddControl(wDogButton);

            //移动控制按钮
            sDogButton = new ModernButton("S", "后退");
            sDogButton.Location = new Point(150, 150);
            sDogButton.Size = new Size(60, 60);
            tabNavigationPage1.AddControl(sDogButton);

            //移动控制按钮
            aDogButton = new ModernButton("A", "左走");
            aDogButton.Location = new Point(80, 150);
            aDogButton.Size = new Size(60, 60);
            tabNavigationPage1.AddControl(aDogButton);

            dDogButton = new ModernButton("D", "右走");
            dDogButton.Location = new Point(220, 150);
            dDogButton.Size = new Size(60, 60);
            tabNavigationPage1.AddControl(dDogButton);

            qDogButton = new ModernButton("Q", "左转");
            qDogButton.Location = new Point(80, 220);
            qDogButton.Size = new Size(60, 60);
            tabNavigationPage1.AddControl(qDogButton);

            eDogButton = new ModernButton("E", "右转");
            eDogButton.Location = new Point(220, 220);
            eDogButton.Size = new Size(60, 60);
            tabNavigationPage1.AddControl(eDogButton);

            zDogButton = new ModernButton("Z", "趴下");
            zDogButton.Location = new Point(80, 290);
            zDogButton.Size = new Size(60, 60);
            tabNavigationPage1.AddControl(zDogButton);

            tDogButton = new ModernButton("T", "站起");
            tDogButton.Location = new Point(220, 290);
            tDogButton.Size = new Size(60, 60);
            tabNavigationPage1.AddControl(tDogButton);

            lDogButton = new ModernButton("F", "解除锁定");
            lDogButton.Location = new Point(150, 340);
            lDogButton.Size = new Size(60, 60);
            tabNavigationPage1.AddControl(lDogButton);
        }

        //加载机械臂的控件
        private void tabNavigationPage2_Paint(object sender, PaintEventArgs e)
        {
            //移动控制按钮
            wDogButton = new ModernButton("W", "前进");
            wDogButton.Location = new Point(150, 80);
            wDogButton.Size = new Size(60, 60);
            tabNavigationPage2.AddControl(wDogButton);

            //移动控制按钮
            sDogButton = new ModernButton("S", "后退");
            sDogButton.Location = new Point(150, 150);
            sDogButton.Size = new Size(60, 60);
            tabNavigationPage2.AddControl(sDogButton);

            //移动控制按钮
            aDogButton = new ModernButton("A", "左走");
            aDogButton.Location = new Point(80, 150);
            aDogButton.Size = new Size(60, 60);
            tabNavigationPage2.AddControl(aDogButton);

            dDogButton = new ModernButton("D", "右走");
            dDogButton.Location = new Point(220, 150);
            dDogButton.Size = new Size(60, 60);
            tabNavigationPage2.AddControl(dDogButton);
        }
    }
}