424 lines
17 KiB
C#
424 lines
17 KiB
C#
|
|
|||
|
|
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 TcpClient controlClient; // 新增：用于控制的TCP客户端
|
|||
|
|
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.0.123";
|
|||
|
|
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.ReceiveVideoFramesAsync(
|
|||
|
|
videoCancellationTokenSource.Token,
|
|||
|
|
mergedVideoStream,
|
|||
|
|
ArmVideo,
|
|||
|
|
false));
|
|||
|
|
|
|||
|
|
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();
|
|||
|
|
controlClient?.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 && controlClient.Connected)
|
|||
|
|
{
|
|||
|
|
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 && controlClient.Connected)
|
|||
|
|
{
|
|||
|
|
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 && controlClient.Connected)
|
|||
|
|
{
|
|||
|
|
//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 && controlClient.Connected)
|
|||
|
|
{
|
|||
|
|
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 && controlClient.Connected)
|
|||
|
|
{
|
|||
|
|
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);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|