From 4de8e046792bc83752d960b434fa3fc081bf886c Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 15 Jul 2024 21:07:35 +0100 Subject: [PATCH] AR_Motors: add frame type Omni3Mecanum Signed-off-by: Rhys Mainwaring --- libraries/AR_Motors/AP_MotorsUGV.cpp | 7 +++++++ libraries/AR_Motors/AP_MotorsUGV.h | 1 + 2 files changed, 8 insertions(+) diff --git a/libraries/AR_Motors/AP_MotorsUGV.cpp b/libraries/AR_Motors/AP_MotorsUGV.cpp index b211384e1c..9f0255059a 100644 --- a/libraries/AR_Motors/AP_MotorsUGV.cpp +++ b/libraries/AR_Motors/AP_MotorsUGV.cpp @@ -647,6 +647,13 @@ void AP_MotorsUGV::setup_omni() add_omni_motor(2, 0.0f, -1.0f, 1.0f); add_omni_motor(3, 1.0f, 0.0f, 0.0f); break; + + case FRAME_TYPE_OMNI3MECANUM: + _motors_num = 3; + add_omni_motor(0, -1.0f, 1.0f, -0.26795f); + add_omni_motor(1, 0.73205f, 1.0f, -0.73205f); + add_omni_motor(2, 0.26795f, 1.0f, 1.0f); + break; } } diff --git a/libraries/AR_Motors/AP_MotorsUGV.h b/libraries/AR_Motors/AP_MotorsUGV.h index 42c2df5377..4df000f74e 100644 --- a/libraries/AR_Motors/AP_MotorsUGV.h +++ b/libraries/AR_Motors/AP_MotorsUGV.h @@ -27,6 +27,7 @@ public: FRAME_TYPE_OMNI3 = 1, FRAME_TYPE_OMNIX = 2, FRAME_TYPE_OMNIPLUS = 3, + FRAME_TYPE_OMNI3MECANUM = 4, }; // initialise motors