149 lines
3.6 KiB
C#
149 lines
3.6 KiB
C#
using System.Text;
|
|
using System.Numerics;
|
|
using System.Windows.Forms;
|
|
using static System.Net.Mime.MediaTypeNames;
|
|
using static System.Runtime.InteropServices.JavaScript.JSType;
|
|
using System.Security.Policy;
|
|
|
|
namespace DroneSimulator
|
|
{
|
|
public partial class Form_Main : Form
|
|
{
|
|
Screen2D screen2D = null;
|
|
|
|
NetServerClients netServerClient = new NetServerClients();
|
|
|
|
List<Drone> AllDrones = new List<Drone>();
|
|
|
|
public Form_Main()
|
|
{
|
|
InitializeComponent();
|
|
}
|
|
|
|
private void ConnectionCallback(object o)
|
|
{
|
|
NetServerClients.ConnectData data = (NetServerClients.ConnectData)o;
|
|
|
|
Invoke((MethodInvoker)delegate
|
|
{
|
|
label_Clients_Num.Text = data.Count.ToString();
|
|
});
|
|
|
|
if (data.Connect)
|
|
{
|
|
Drone drone = new Drone(data.ID);
|
|
drone.Create(0.5f, 10.0f);
|
|
|
|
screen2D.CreateDrone(Color.Red, data.ID);
|
|
|
|
AllDrones.Add(drone);
|
|
}
|
|
else
|
|
{
|
|
foreach (Drone drone in AllDrones)
|
|
{
|
|
if (drone.ID != data.ID) continue;
|
|
drone.Close();
|
|
|
|
screen2D.RemoveDrone(data.ID);
|
|
|
|
AllDrones.Remove(drone);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
private void ReceiveCallback(object o)
|
|
{
|
|
NetServerClients.ReceiveData data = (NetServerClients.ReceiveData)o;
|
|
|
|
Drone? drone = null;
|
|
|
|
foreach (Drone d in AllDrones)
|
|
{
|
|
if (d.ID != data.ID) continue;
|
|
drone = d;
|
|
break;
|
|
}
|
|
|
|
if (drone == null) return;
|
|
|
|
Drone.DataIn id = new Drone.DataIn();
|
|
|
|
id.fromBytes(data.Buffer);
|
|
|
|
drone.SetQadroPow(id.MotorUL, id.MotorUR, id.MotorDL, id.MotorDR);
|
|
|
|
Drone.DataOut od = new Drone.DataOut();
|
|
|
|
od.AccX= drone.Acc.X; od.AccY= drone.Acc.Y; od.AccZ= drone.Acc.Z;
|
|
od.GyrX = drone.Gyr.X; od.GyrY = drone.Gyr.Y; od.GyrZ = drone.Gyr.Z;
|
|
od.PosX= drone.PosXYZ.X; od.PosY = drone.PosXYZ.Y;
|
|
od.LaserRange = drone.LaserRange;
|
|
|
|
data.Client.Send(od.getBytes());
|
|
}
|
|
|
|
private void button_Client_Start_Click(object sender, EventArgs e)
|
|
{
|
|
var done = netServerClient.StartServer((int)numericUpDown_Clients_Port.Value, (int)numericUpDown_Clients_Limit.Value, ConnectionCallback, ReceiveCallback);
|
|
switch (done)
|
|
{
|
|
case NetServerClients.ServerState.Error:
|
|
{
|
|
MessageBox.Show("Error to start server");
|
|
break;
|
|
}
|
|
case NetServerClients.ServerState.Start:
|
|
{
|
|
button_Client_Start.Text = "Stop";
|
|
button_Client_Start.BackColor = Color.LimeGreen;
|
|
break;
|
|
}
|
|
case NetServerClients.ServerState.Stop:
|
|
{
|
|
button_Client_Start.Text = "Start";
|
|
button_Client_Start.BackColor = Color.Transparent;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (done != NetServerClients.ServerState.Start) return;
|
|
|
|
pictureBox_2D.Image = null;
|
|
|
|
screen2D = new Screen2D(DrawCallback);
|
|
}
|
|
|
|
private void exitToolStripMenuItem_Click(object sender, EventArgs e)
|
|
{
|
|
Close();
|
|
}
|
|
|
|
private void DrawCallback(Bitmap bmp)
|
|
{
|
|
Invoke((MethodInvoker)delegate
|
|
{
|
|
if (pictureBox_2D.Image == null) pictureBox_2D.Image = bmp;
|
|
pictureBox_2D.Refresh();
|
|
});
|
|
}
|
|
|
|
private void timer_Test_Tick(object sender, EventArgs e)
|
|
{
|
|
if (screen2D == null) return;
|
|
|
|
foreach (Drone d in AllDrones)
|
|
{
|
|
screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation());
|
|
}
|
|
|
|
screen2D.DrawScene();
|
|
}
|
|
private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
|
|
{
|
|
foreach (Drone d in AllDrones) d.Close();
|
|
}
|
|
}
|
|
}
|