422 lines
17 KiB
C#
422 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 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);
|
||
}
|
||
}
|
||
}
|
||
|