تطوير hexapod من الصفر (الجزء 4) - مسارات الرياضيات وتسلسلات

مرحبا بالجميع! وقد ذهب تطوير Hexapod خطوة أخرى إلى الأمام. هذه المرة ، يتم تنفيذ مسارات حركة الأطراف واختبارها - الجزء التالي من رياضيات الحركة. في هذه المقالة سأتحدث عن هذه المسارات والتسلسلات الأساسية للحركة. آمل أن تكون مثيرة للاهتمام.

مراحل التنمية:
الجزء 1 - التصميم
الجزء 2 - التجمع
الجزء 3 - الحركية
الجزء 4 - مسارات الرياضيات وتسلسل
الجزء 5 - الالكترونيات
الجزء 6 - الانتقال إلى الطباعة ثلاثية الأبعاد
الجزء 7 - برامج الإسكان والتطبيق الجديدة وبروتوكولات الاتصال

مسارات

جوهر هذه الآلية هو أنه عند تعيين نقطتين ، يمكنك اختيار مسار الطرف. عند الانتقال من نقطة إلى أخرى ، ستتغير الإحداثيات وفقًا لمعادلات حدية معينة. اتضح أن الآلية قوية جدًا وتتيح لك الحصول على منحنيات مثيرة للحركة. كما أنها تقوم بتنفيذ تجانس الحركات من خلال تغيير خطوة المعامل t - كلما كانت الخطوة أصغر ، كلما زادت النقاط الوسيطة ، على التوالي ، سرعة أقل وسلاسة أعلى للحركة.

إجراء تحديد معالم المسار في بعض الأماكن غير مفهوم إلى حد ما ، ويمكن أن تتشوش فيه. تكمن الصعوبة في حقيقة أنه عند تعيين إحداثيات نقاط البداية والنهاية ، فإن إحداثيات النقاط الحقيقية في الفضاء لا يتم ضبطها دائمًا ، أي بعض الإحداثيات تعيين المعلمات مسار. اضطررت إلى كتابة برنامج يعرض مسارًا محددًا وفي الوقت نفسه يتحقق من إمكانية الوصول إلى كل نقطة من المسار.

يدعم برنامج التشغيل مسارات الحركة التالية:

1. XYZ_LINAR هو أبسط المسارات. يتم استخدام المسار عند التحرك للأمام ، للخلف ، للأعلى وللأسفل. جميع الإحداثيات تتغير خطيا وتحسب على النحو التالي:
   
   

   x = t * (x1 - x0) / 180.0f + x0; y = t * (y1 - y0) / 180.0f + y0; z = t * (z1 - z0) / 180.0f + z0; 

   
   هنا نفهم أنه لا توجد مشاكل. تحدد الإحداثيات الزوايا المتوازية وتتزامن مع الإحداثيات الحقيقية. تحدث الحركة على طول المربع القطري.
   
   
   التصور المسار

   
   
   
   
   
   
2. YZ_ARC_Y_LINEAR - يسمح لك هذا المسار بتنفيذ الحركة على طول القوس. يتم استخدام المسار عند الدوران ، عندما تحتاج إلى تحريك طرف على الأرض. يتم حساب الإحداثيات على النحو التالي:
   
   

    float R = sqrt(x0 * x0 + z0 * z0); float atan0 = RAD_TO_DEG(atan2(x0, z0)); float atan1 = RAD_TO_DEG(atan2(x1, z1)); float t_mapped_rad = DEG_TO_RAD(t * (atan1 - atan0) / 180.0f + atan0); x = R * sin(t_mapped_rad); // Circle Y y = t * (y1 - y0) / 180.0f + y0; z = R * cos(t_mapped_rad); // Circle X 

   
   هذا هو المكان الذي تبدأ فيه المتعة. تحدد الإحداثيات اتجاه الأشعة للحد من القوس وقد لا تتزامن مع الإحداثيات الفعلية. تقع الأشعة في نفس المستوى ، في حين أن نصف قطر الدائرة يساوي طول المتجه إلى نقطة البداية.
   
   
   التصور المسار

   
   
   
   
   
   
3. XZ_ARC_Y_SINUS - يسمح لك هذا المسار أيضًا بتنفيذ الحركة على طول قوس ، ولكنه أكثر تعقيدًا من YZ_ARC_Y_LINEAR. يتم استخدام المسار عند الدوران ، عندما تحتاج إلى تحريك أحد الأطراف عبر الهواء. يتم حساب الإحداثيات على النحو التالي:
   
   

    float R = sqrt(x0 * x0 + z0 * z0); float atan0 = RAD_TO_DEG(atan2(x0, z0)); float atan1 = RAD_TO_DEG(atan2(x1, z1)); float t_mapped_rad = DEG_TO_RAD(t * (atan1 - atan0) / 180.0f + atan0); x = R * sin(t_mapped_rad); // circle Y y = (y1 - y0) * sin(DEG_TO_RAD(t)) + y0; z = R * cos(t_mapped_rad); // circle X 

   
   يستمر المرح. تحدد الإحداثيات أيضًا اتجاه الأشعة للحد من القوس ، لكنها لا تتزامن مع الإحداثيات الفعلية. إحداثي Y النقطة الهدف يعين ارتفاع الجيب.
   
   
   التصور المسار

   
   
   
   
4. XZ_ELLIPTICAL_Y_SINUS - يسمح لك هذا المسار بتنفيذ الحركة على طول القطع الناقص. يتم استخدام المسار عند التحرك للأمام والخلف ، عندما تحتاج إلى تحريك أحد الأطراف عبر الهواء. يعد هذا المسار من مضاعفات XZ_ARC_Y_SINUS وكان مطلوبًا فقط بسبب المشية القبيحة بصريًا عند استخدام XZ_ARC_Y_SINUS (تمدد الأرجل كثيرًا). يتم حساب الإحداثيات على النحو التالي:
   
   

    float a = (z1 - z0) / 2.0f; float b = (x1 - x0); float c = (y1 - y0); x = b * sin(DEG_TO_RAD(180.0f - t)) + x0; // circle Y y = c * sin(DEG_TO_RAD(t)) + y0; z = a * cos(DEG_TO_RAD(180.0f - t)) + z0 + a; 

   
   تحدد الإحداثيات الزوايا المتوازية ولا تتزامن مع الإحداثيات الحقيقية. تحدث الحركة من الزوايا السفلية للمربع ، والتي تقع في نفس الطائرة مثل لمس الجزء العلوي من جانبها. من الأفضل إلقاء نظرة على الصورة في المفسد ، ولا أعرف كيف أصفها باختصار بالكلمات.
   
   
   التصور المسار

   
   

هذا يكمل الرياضيات الأساسية لحركة hexapod. في مشروعي ، هذا هو الحد الأدنى الضروري لتنفيذ أي حركة تقريبًا.

تسلسل

قليلا من الناحية النظرية

التسلسلات هي الإجراءات الأولية التي تشكل المشية. وهي مقسمة إلى دورية وغير دورية.

• يمكن إجراء تسلسل دوري عدة مرات ، وفي نهاية كل دورة يجب إعادة الأطراف إلى وضعها الأصلي (الحركة والدوران) ؛
• يتم تنفيذ تسلسل غير دوري مرة واحدة فقط (الصعود والنسب) ؛

يحتوي كل تسلسل على ثلاث كتل تكرارية: كتلة التحضير ، الكتلة الرئيسية ، كتلة الإنجاز.

• كتلة التدريب - تحتوي على تكرارات لتحريك الأطراف إلى موضع البداية للتسلسل. في حالتي ، يتطلب التحرك إلى الأمام وضع ساقيك في وضع معين قبل البدء في التحرك. يتم تنفيذه مرة واحدة عند الانتقال إلى التسلسل ؛
• الكتلة الرئيسية - تحتوي على التكرار الرئيسي للتسلسل. يمكن أن يؤديها بشكل دوري.
• إكمال الكتلة - يحتوي على تكرارات لنقل الأطراف إلى الموضع الأساسي (الموضع الذي يتم فيه تعيين الأطراف بعد الرفع) ؛

يوضح الشكل أدناه تسلسل التحرك للأمام.

• تشير النقاط الحمراء إلى الوضع الأولي للأطراف قبل البدء في الحركة
• الخطوط الزرقاء تشير إلى مسارات الطرف على الأرض
• الخطوط السوداء تشير إلى مسارات الطرف في الهواء
• الأسهم تشير إلى التسلسل

يتم تحديد إحداثيات النقاط بناء على تكوين السكن. اخترت النقاط على مقربة من الجسم قدر الإمكان لتقليل طول الذراع. في دورة واحدة من التسلسل ، يتحرك hexapod بمقدار 18 سم (في 1 دورة 2 يتم اتخاذ الخطوات - خطوة واحدة في 3 أطراف). إذا قمت بزيادة المسافة ، ستبدأ الأطراف بالتشبث ببعضها البعض. هذه المعلمة محدودة فقط من خلال تكوين الحالة.


يتم تعريف التسلسل بنقطتين (1 ، 2) لكل طرف ويتم استخدام مسارين: XYZ_LINEAR (خطوط زرقاء) و XZ_ELLIPTICAL_Y_SINUS (خطوط سوداء). يتم استخدام النقطة 1 بواسطة المسار XZ_ELLIPTICAL_Y_SINUS لتعيين ارتفاع الجيب ، وبالتالي ، الارتفاع الذي سترتفع عليه الساق. تعتبر النقطتان 2 و 3 من النقاط الحقيقية التي يصل إليها أحد الأطراف عند الحركة.

موقع النقاط يعتمد فقط على خيالك وقدرات hexapod. ربما اتضح أن الأمر معقد بعض الشيء وهناك خيار أكثر بساطة ، لكن يبدو أنني لم أتوصل إليه بعد.

تطبيق

الآن دعونا نلقي نظرة على تنفيذ كل هذه السعادة. الهياكل مع معلمات التسلسل هي كما يلي:

 typedef struct { point_3d_t point_list[SUPPORT_LIMB_COUNT]; path_type_t path_list[SUPPORT_LIMB_COUNT]; uint32_t smooth_point_count; } sequence_iteration_t; typedef struct { bool is_sequence_looped; uint32_t main_sequence_begin; uint32_t finalize_sequence_begin; uint32_t total_iteration_count; sequence_iteration_t iteration_list[15]; sequence_id_t available_sequences[SUPPORT_SEQUENCE_COUNT]; } sequence_info_t; 

sequence_iteration_t - يحتوي على معلومات حول تكرار التسلسل:

• point_list - مجموعة من النقاط لكل طرف بتنسيق XYZ ؛
• path_list - مجموعة من المسارات لكل طرف ؛
• smooth_point_count - يضبط عدد نقاط المسار (خطوة المعلمة t) ؛

sequence_info_t - يحتوي على معلومات حول التسلسل بأكمله:

• is_sequence_looped - يحدد نوع التسلسل: دوري أم لا ؛
• main_sequence_begin - لتعيين فهرس البداية للكتلة الرئيسية في صفيف iteration_list ؛
• finalize_sequence_begin - يحدد فهرس البداية لكتلة الإكمال في صفيف iteration_list ؛
• total_iteration_count - يحدد عدد التكرارات في التسلسل ؛
• iteration_list - مجموعة من التكرارات ؛
• available_sequences - يحدد قائمة التسلسلات المتاحة للانتقال من الحالية (على سبيل المثال ، لا يمكننا البدء في المشي دون الاستيقاظ أولاً من الكلمة) ؛

ملاحظة: لم يتم الإشارة إلى فهرس كتلة الإعداد عن قصد ؛ إنه موجود دائمًا في بداية صفيف التكرار.

لسوء الحظ ، لا يمكنني توفير رمز لتحديد التسلسل هنا ، لأن انها واسعة جدا ويبدو فظيعا بعد النقل. أترك رابطًا لملف التعريف هنا.

مخطط معالجة الحركة

يجدر بنا أن نجعل كل دوائر الجحيم التي يمر بها التسلسل وقت التشغيل. نظام المعالجة على النحو التالي:

1. MOVEMENT Engine - ينظم المعالجة والتبديل بين المتواليات. لا توجد حسابات هناك. إذا تم تبسيطها ، فتنزلق هذه الوحدة النمطية النقطة التالية إلى وحدة LIBMS DRIVER بعد اكتمال المعالجة الحالية.
   وحدة الإدخال: مجموعة من إحداثيات النقاط الهدف.
   إخراج الوحدة النمطية: نقطة الهدف للتكرار الحالي للتسلسل.
2. LIBMS DRIVER هو الأكثر تعقيدًا في جميع الوحدات. تسود جميع رياضيات الحركة هنا: عكس الحركات ، وحسابات المسار وتنعيم الحركات. هذه الوحدة لديها تزامن صارم مع وحدة PWM. يتم إجراء العمليات الحسابية بتردد 150 هرتز ، على التوالي ، كما يتم تزويد نبض التحكم في محركات الأقراص بتردد 150 هرتز.
   مدخلات الوحدة: إحداثيات الهدف.
   وحدة الإخراج: زوايا دوران المؤازرة.
3. سائق سيرفو . لا يوجد شيء مميز فيه ، باستثناء مجموعة من المعلمات لإعداد وضبط محركات الأقراص.
   وحدة الإدخال: زوايا دوران المؤازرة.
   وحدة الإخراج: التحكم في عرض النبض.
4. سائق PWM . برنامج تشغيل PWM للتحكم في القيادة. دبابيس هنا نشل فقط في الوقت المناسب. يتم زيادة متغير التزامن PWM التزامن كل فترة PWM.
   وحدة الإدخال: التحكم في عرض النبض.
   وحدة الإخراج: نبضات على دبابيس التحكم.

حاولت جعل الوحدات مستقلة عن بعضها البعض ونجحت. يسمح لك هذا بإدراج أي وحدة نمطية وسيطة (على سبيل المثال ، وحدة التكيف مع المناظر الطبيعية) ولن ينقطع أي شيء في نفس الوقت ، في حين سيحدث التنفيذ مع الحد الأدنى من التغييرات في الكود.

تم العثور على آخر الأخبار وجراد البحر المشروع