edit proshivka

DroneClientCpp/Drone.cpp

@@ -5,7 +5,7 @@ namespace DroneClient {
     // Реализация статического метода GetBytes
     array<Byte>^ Drone::GetBytes(Object^ data)
     {
-        int size = Marshal::SizeOf(data);
+        int size = Marshal::SizeOf(data);   
         array<Byte>^ arr = gcnew array<Byte>(size);
 
         IntPtr ptr = IntPtr::Zero;
@@ -45,12 +45,14 @@ namespace DroneClient {
     // Реализация приватного метода SendDataMotor4
     array<Byte>^ Drone::SendDataMotor4()
     {
+
         DroneData::DataMotor4 mot4;
 
         mot4.Head.Size = Marshal::SizeOf(DroneData::DataMotor4::typeid);
         mot4.Head.Mode = DroneData::DataMode::Response;
         mot4.Head.Type = DroneData::DataType::DataMotor4;
-
+        updateData();
+        setMotors();
         mot4.UL = MotorUL;
         mot4.UR = MotorUR;
         mot4.DL = MotorDL;
@@ -158,6 +160,92 @@ namespace DroneClient {
         return zero;
     }
 
+
+
+    //void Drone::setMotors()
+    //{
+    //    array<UInt16>^ ch = joy->Channels;   // ссылка на тот же массив
+    //    UInt16 pow = (ch[2] - 1000) / 10;
+    //    float fpow = float(pow)/ 100.0f;
+
+    //   const float maxAngle = 20.0f;
+    //  
+
+    //   desPitch = (ch[1] - 1500) * maxAngle / 500;
+    //   desRoll = (ch[0] - 1499) * maxAngle / 500;
+
+
+    //   float errorPitch, errorRoll, forceRoll, forcePitch;
+    //    errorPitch = desPitch -pitch;
+    //    errorRoll = desRoll - roll;
+
+    //    static float PRegulator = 0.001f;
+ 
+    //    forcePitch = PRegulator * errorPitch;
+    //    forceRoll = PRegulator * errorRoll;
+
+
+    //   MotorUL = fpow-forcePitch + forceRoll;
+    //   MotorUR = fpow - forcePitch - forceRoll;
+    //   MotorDL = fpow + forcePitch + forceRoll;
+    //   MotorDR = fpow + forcePitch - forceRoll;
+    //}
+
+
+    /* ───────────── вспомогательная функция ───────────── */
+    static float Clamp01(float v)
+    {
+        if (v < 0.0f) return 0.0f;
+        if (v > 1.0f) return 1.0f;
+        return v;
+    }
+
+    /* ───────────── PD-регулятор и микширование ───────── */
+    void Drone::setMotors()
+    {
+        /* ---------- входные каналы --------------------- */
+        array<UInt16>^ ch = joy->Channels;
+        const float fpow = (ch[2] - 1000) * 0.001f;       // 0…1
+
+        /* ---------- желаемые углы ---------------------- */
+        const float maxAngle = 20.0f;
+         desPitch = (ch[1] - 1500) * maxAngle / 500.0f;
+         desRoll = (ch[0] - 1499) * maxAngle / 500.0f;
+
+         
+        /* ---------- PD-регулятор ----------------------- */
+        static float prevErrPitch = 0.0f, prevErrRoll = 0.0f;
+        const float errPitch = desPitch - pitch;
+        const float errRoll = desRoll - roll;
+
+        const float dt = 0.01f;                // период 10 мс (100 Гц)
+        const float dPitch = (errPitch - prevErrPitch) / dt;
+        const float dRoll = (errRoll - prevErrRoll) / dt;
+
+        const float Kp = 0.115f;               
+        const float Kd = 0.0f;
+
+        const float forcePitch = Kp * errPitch + Kd * dPitch;
+        const float forceRoll = Kp * errRoll + Kd * dRoll;
+
+        prevErrPitch = errPitch;
+        prevErrRoll = errRoll;
+
+        /* ---------- распределение на моторы ------------ */
+        MotorUL = fpow - forcePitch + forceRoll;
+        MotorUR = fpow - forcePitch - forceRoll;
+        MotorDL = fpow + forcePitch + forceRoll;
+        MotorDR = fpow + forcePitch - forceRoll;
+
+        //MotorUL = Clamp01(MotorUL);
+        //MotorUR = Clamp01(MotorUR);
+        //MotorDL = Clamp01(MotorDL);
+        //MotorDR = Clamp01(MotorDR);
+    }
+
+
+
+
     // Реализация конструктора
     Drone::Drone()
     {
@@ -166,6 +254,11 @@ namespace DroneClient {
         DroneStreamHead.Mode = DroneData::DataMode::None;
         DroneStreamHead.Size = 0;
         DroneStreamHead.Type = DroneData::DataType::None;
+
+        this->joy = gcnew Joypad();
+
+        this->joy->Start("COM7", 115200);
+          
     }
 
     // Реализация метода DataStream
@@ -252,4 +345,16 @@ namespace DroneClient {
 
       return send;
     }
+
+    void Drone::updateData(){
+        Vec3 acc{ this->AccX,this->AccY, this->AccZ };
+        Vec3 gyr{ this->GyrX,this->GyrY, this->GyrZ };
+        Vec3 mag{ 1,0, 0};
+        ORI result = WorkAccGyroMag(acc, gyr, mag, 0, 0.01);
+        this->pitch = result.Pitch;
+        this->roll = -result.Roll;
+        this->yaw = result.Yaw;
+
+
+    }
 }