From 46e296b7c21d71458bdf1766670caf3bd4e76378 Mon Sep 17 00:00:00 2001 From: Sergey Sklyarov Date: Fri, 4 Apr 2025 02:12:55 +0300 Subject: [PATCH] =?UTF-8?q?=D0=94=D0=BE=D1=84=D0=B8=D0=B3=D0=B0=20=D0=B8?= =?UTF-8?q?=D1=81=D0=BF=D1=80=D0=B0=D0=B2=D0=BB=D0=B5=D0=BD=D0=B8=D0=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- DroneClient/FormMain.cs | 26 +++++++++++--------- DroneSimulator/Drone.cs | 50 ++++++++++++++++++++++++++------------ DroneSimulator/FormMain.cs | 9 ++----- DroneSimulator/Screen2D.cs | 6 +++++ 4 files changed, 57 insertions(+), 34 deletions(-) diff --git a/DroneClient/FormMain.cs b/DroneClient/FormMain.cs index d479e1a..dee166e 100644 --- a/DroneClient/FormMain.cs +++ b/DroneClient/FormMain.cs @@ -112,37 +112,39 @@ namespace DroneSimulator private void button_UU_MouseDown(object sender, MouseEventArgs e) { + const float pow = 0.1f; + if (sender == button_UU) { - sendDrone.MotorUL -= 0.1f; sendDrone.MotorUR -= 0.1f; - sendDrone.MotorDL += 0.1f; sendDrone.MotorDR += 0.1f; + sendDrone.MotorUL -= pow; sendDrone.MotorUR -= pow; + sendDrone.MotorDL += pow; sendDrone.MotorDR += pow; } if (sender == button_DD) { - sendDrone.MotorUL += 0.1f; sendDrone.MotorUR += 0.1f; - sendDrone.MotorDL -= 0.1f; sendDrone.MotorDR -= 0.1f; + sendDrone.MotorUL += pow; sendDrone.MotorUR += pow; + sendDrone.MotorDL -= pow; sendDrone.MotorDR -= pow; } if (sender == button_LL) { - sendDrone.MotorUL -= 0.1f; sendDrone.MotorUR += 0.1f; - sendDrone.MotorDL -= 0.1f; sendDrone.MotorDR += 0.1f; + sendDrone.MotorUL -= pow; sendDrone.MotorUR += pow; + sendDrone.MotorDL -= pow; sendDrone.MotorDR += pow; } if (sender == button_RR) { - sendDrone.MotorUL += 0.1f; sendDrone.MotorUR -= 0.1f; - sendDrone.MotorDL += 0.1f; sendDrone.MotorDR -= 0.1f; + sendDrone.MotorUL += pow; sendDrone.MotorUR -= pow; + sendDrone.MotorDL += pow; sendDrone.MotorDR -= pow; } if (sender == button_ML) { - sendDrone.MotorUL -= 0.1f; sendDrone.MotorUR += 0.1f; - sendDrone.MotorDL += 0.1f; sendDrone.MotorDR -= 0.1f; + sendDrone.MotorUL -= pow; sendDrone.MotorUR += pow; + sendDrone.MotorDL += pow; sendDrone.MotorDR -= pow; } if (sender == button_MR) { - sendDrone.MotorUL += 0.1f; sendDrone.MotorUR -= 0.1f; - sendDrone.MotorDL -= 0.1f; sendDrone.MotorDR += 0.1f; + sendDrone.MotorUL += pow; sendDrone.MotorUR -= pow; + sendDrone.MotorDL -= pow; sendDrone.MotorDR += pow; } } diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs index 0fba4f6..fd0aaa7 100644 --- a/DroneSimulator/Drone.cs +++ b/DroneSimulator/Drone.cs @@ -9,6 +9,7 @@ namespace DroneSimulator public float Mass; // Масса public bool Active; // Живой? public float Length; // Длинна лучей + public float Dynamic; // Физика поведения public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение public Quaternion Quat; // Основной кватернион public float Power = 0; // Тяга всех двигателей @@ -76,6 +77,20 @@ namespace DroneSimulator public float LaserRange; } + public DataOut GetDataOut() + { + DataOut data = new DataOut(); + + data.AccX = Acc.X; data.AccY = Acc.Y; data.AccZ = Acc.Z; + data.GyrX = Gyr.X; data.GyrY = Gyr.Y; data.GyrZ = Gyr.Z; + + data.PosX = PosXYZ.X; data.PosY = PosXYZ.Y; + + data.LaserRange = LaserRange; + + return data; + } + public struct DataIn { public float MotorUL, MotorUR, MotorDL, MotorDR; @@ -99,7 +114,7 @@ namespace DroneSimulator { float time = Environment.TickCount - Timer; Timer = Environment.TickCount; - Action(time / 100); + Action(time / 1000); Thread.Sleep(1); } } @@ -108,7 +123,7 @@ namespace DroneSimulator { ID = id; Active = false; - PosXYZ = new Vector3(2000, 1000, 0); + PosXYZ = Vector3.Zero; SpdXYZ = Vector3.Zero; AccXYZ = Vector3.Zero; Quat = Quaternion.Identity; @@ -118,9 +133,10 @@ namespace DroneSimulator DroneThread.Start(); } - public int Create(float mass, float len) + public int Create(float mass, float len, float dynamic) { Mass = Range(mass); + Dynamic = dynamic * 100; Length = len; Active = true; @@ -180,19 +196,23 @@ namespace DroneSimulator float flow = Power; - if (PosXYZ.Z < 100) + if (PosXYZ.Z < 0.3f) { flow += flow * 0.1f; // Воздушная подушка } + SpdPRY += AccPRY * ((time * Dynamic) / (Mass * Length)); + + float dyn = Dynamic / 10; + Quaternion pow = Quaternion.Inverse(Quat) * new Quaternion(0, 0, flow, 0) * Quat; - AccXYZ = new Vector3(pow.X, pow.Y, pow.Z) / Mass; - - SpdPRY += AccPRY * time / (Mass * Length); - - SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity)) * time; + AccXYZ = new Vector3(pow.X, pow.Y, pow.Z) * (dyn / Mass); + + SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity * dyn)) * time; PosXYZ += SpdXYZ * time; + AccXYZ /= dyn; + if (PosXYZ.Z < 0) { SpdPRY = Vector3.Zero; @@ -200,7 +220,7 @@ namespace DroneSimulator SpdXYZ.Y = 0; Quat = Quaternion.Identity; } - else Rotate(SpdPRY.X, SpdPRY.Y, SpdPRY.Z); + else Rotate(SpdPRY.X * time, SpdPRY.Y * time, SpdPRY.Z * time); Vector4 ori = GetOrientation(); @@ -213,7 +233,7 @@ namespace DroneSimulator Active = false; // Сильно ударился о землю }*/ - /*if (MathF.Abs(ori.X) > 45 || MathF.Abs(ori.Y) > 45) + /*if (MathF.Abs(ori.X) > 20 || MathF.Abs(ori.Y) > 20) { Active = false; // Повредил винты при посадке }*/ @@ -226,13 +246,13 @@ namespace DroneSimulator } else { - /*if (ori.W < 0) + if (ori.W < 0) { - Active = false; // Перевернулся вверх ногами - }*/ + //Active = false; // Перевернулся вверх ногами + } Quaternion grav = new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0); - grav = (Quat * grav) * Quaternion.Inverse(Quat); + //grav = (Quat * grav) * Quaternion.Inverse(Quat); // Инерциальный акселерометр (тест) Acc = new Vector3(grav.X, grav.Y, grav.Z); Gyr = SpdPRY; diff --git a/DroneSimulator/FormMain.cs b/DroneSimulator/FormMain.cs index 41e4719..d291450 100644 --- a/DroneSimulator/FormMain.cs +++ b/DroneSimulator/FormMain.cs @@ -33,7 +33,7 @@ namespace DroneSimulator if (data.Connect) { Drone drone = new Drone(data.ID); - drone.Create(0.5f, 10.0f); + drone.Create(0.5f, 1.0f, 1.0f); screen2D.CreateDrone(Color.Red, data.ID); @@ -73,12 +73,7 @@ namespace DroneSimulator 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; + Drone.DataOut od = drone.GetDataOut(); try { data.Client.Send(Drone.getBytes(od)); } catch { } diff --git a/DroneSimulator/Screen2D.cs b/DroneSimulator/Screen2D.cs index 36a4a72..c967b1c 100644 --- a/DroneSimulator/Screen2D.cs +++ b/DroneSimulator/Screen2D.cs @@ -24,6 +24,7 @@ namespace DroneSimulator public int Azimuth = 0; } + private float Scale = 100; private DrawCallback drawCallback; private Bitmap MainArea = new Bitmap(4096, 2560); private List DroneList = new List(); @@ -161,6 +162,11 @@ namespace DroneSimulator const float TO_GRAD = 180 / MathF.PI; const float TO_RADI = MathF.PI / 180; + pos *= Scale; + + pos.X += MainArea.Width / 2; + pos.Y += MainArea.Height / 2; + foreach (var d in DroneList) { if (d.ID != id) continue;