Adding mecanum wheels handling to firmware for Husarion core2 board (#9)

* updated diff_drive_lib and rosserial_lib libraries to the newest versions Signed-off-by: Aleksander Szymański <[email protected]> * adjusted firmware to work with newest versions of the libraries Signed-off-by: Aleksander Szymański <[email protected]> * change diff_drive parameters to the new default values Signed-off-by: Aleksander Szymański <[email protected]> * added mecanum parameters Signed-off-by: Aleksander Szymański <[email protected]> * implemented changes for mecanum wheels handling Signed-off-by: Aleksander Szymański <[email protected]> * format code Signed-off-by: Aleksander Szymański <[email protected]> * format header files Signed-off-by: Aleksander Szymański <[email protected]> * added initial workflow Signed-off-by: Aleksander Szymański <[email protected]> * fix cmake-action version Signed-off-by: Aleksander Szymański <[email protected]> * change workflow name [skip ci] Signed-off-by: Aleksander Szymański <[email protected]> * update hFramework path for cmake-actions Signed-off-by: Aleksander Szymański <[email protected]> * update environment configuration step Signed-off-by: Aleksander Szymański <[email protected]> * pipeline: update git submodules before building the project Signed-off-by: Aleksander Szymański <[email protected]> * formated sensor files Signed-off-by: Aleksander Szymański <[email protected]> * format files with newer version of clang-format Signed-off-by: Aleksander Szymański <[email protected]> * check include orded and strings format to avoid include sorting Signed-off-by: Aleksander Szymański <[email protected]> * use submodules in github action Signed-off-by: Aleksander Szymański <[email protected]> --------- Signed-off-by: Aleksander Szymański <[email protected]>
LeoRover · Sep 5, 2023 · 277e6f1 · 277e6f1
1 parent fc24295
commit 277e6f1
Show file tree

Hide file tree

Showing 11 changed files with 924 additions and 617 deletions.
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
@@ -0,0 +1,45 @@
+name: core2_firmware CI
+
+on:
+  workflow_dispatch:
+  push:
+    branches:
+      - master
+  pull_request:
+    branches:
+      - master
+
+jobs:
+  clang-format-check:
+    name: Clang-format formatting check
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/checkout@v3
+    - name: Run clang-format style check.
+      uses: jidicula/[email protected]
+      with:
+        clang-format-version: '14'
+        include-regex: ^.*\.(cpp|hpp)$
+
+  build:
+    name: Build the project
+    runs-on: ubuntu-latest
+    steps:
+      - name: Checkout the repository
+        uses: actions/checkout@v3
+        with:
+          submodules: true
+      - name: Prepare environment
+        run: |
+          sudo apt install gcc-arm-none-eabi
+          wget http://files.fictionlab.pl/husarion/Husarion_SDK-stable.zip
+          unzip Husarion_SDK-stable.zip -d hFramework
+          mkdir build
+      - name: Configure and build the project
+        uses: threeal/[email protected]
+        with:
+          run-build: true
+          options: HFRAMEWORK_PATH=hFramework
+
+
+
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -17,8 +17,8 @@ endif()
 
 include_directories(
     include/
-    lib/diff_drive_lib/
-    lib/rosserial_lib/
+    lib/diff_drive_lib/include
+    lib/rosserial_lib
     lib/sensors/
 )
 
@@ -27,9 +27,11 @@ add_hexecutable(core2_firmware
     src/parameters.cpp
     src/motor_controller.cpp
     src/imu_receiver.cpp
-    lib/diff_drive_lib/diff_drive_controller.cpp
-    lib/diff_drive_lib/pid_regulator.cpp
-    lib/diff_drive_lib/wheel_controller.cpp
+    lib/diff_drive_lib/src/diff_drive_controller.cpp
+    lib/diff_drive_lib/src/mecanum_controller.cpp
+    lib/diff_drive_lib/src/pid_regulator.cpp
+    lib/diff_drive_lib/src/robot_controller.cpp
+    lib/diff_drive_lib/src/wheel_controller.cpp
     lib/rosserial_lib/time.cpp
     lib/rosserial_lib/duration.cpp
     lib/sensors/MPU9250/MPU9250.cpp)
diff --git a/include/firmware/configuration.hpp b/include/firmware/configuration.hpp
@@ -2,8 +2,8 @@
 
 #include <peripherals.h>
 
-#include "motor_controller.hpp"
-#include "diff_drive_controller.hpp"
+#include "diff_drive_lib/robot_controller.hpp"
+#include "firmware/motor_controller.hpp"
 
 const char *const FIRMWARE_VERSION = "2.0.0";
 
@@ -32,25 +32,29 @@ extern MotorController MotB;
 extern MotorController MotC;
 extern MotorController MotD;
 
-const DiffDriveConfiguration DD_CONFIG = {
+const diff_drive_lib::RobotConfiguration ROBOT_CONFIG = {
     .wheel_FL_conf =
         {
             .motor = MotC,
+            .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY,
             .velocity_rolling_window_size = 10,
         },
     .wheel_RL_conf =
         {
             .motor = MotD,
+            .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY,
             .velocity_rolling_window_size = 10,
         },
     .wheel_FR_conf =
         {
             .motor = MotA,
+            .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY,
             .velocity_rolling_window_size = 10,
         },
     .wheel_RR_conf =
         {
             .motor = MotB,
+            .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY,
             .velocity_rolling_window_size = 10,
         },
 };
diff --git a/include/firmware/motor_controller.hpp b/include/firmware/motor_controller.hpp
@@ -2,9 +2,9 @@
 
 #include <hMotor.h>
 
-#include "motor_controller_interface.hpp"
+#include "diff_drive_lib/motor_controller_interface.hpp"
 
-class MotorController : public MotorControllerInterface {
+class MotorController : public diff_drive_lib::MotorControllerInterface {
  public:
   explicit MotorController(hFramework::hMotor& motor,
                            const bool reverse_polarity)

diff --git a/include/firmware/parameters.hpp b/include/firmware/parameters.hpp
@@ -2,9 +2,9 @@
 
 #include <ros.h>
 
-#include "diff_drive_controller.hpp"
+#include "diff_drive_lib/robot_controller.hpp"
 
-struct Parameters : DiffDriveParams {
+struct Parameters : diff_drive_lib::RobotParams {
   // Override inherited parameters
   Parameters() {
     // Wheel
@@ -17,10 +17,11 @@ struct Parameters : DiffDriveParams {
     wheel_encoder_jump_threshold = 20000.0F;
 
     // Differential drive
-    dd_wheel_radius = 0.0625F;
-    dd_wheel_separation = 0.33F;
-    dd_angular_velocity_multiplier = 1.91F;
-    dd_input_timeout = 500;
+    robot_wheel_radius = 0.0625F;
+    robot_wheel_separation = 0.358F;
+    robot_wheel_base = 0.3052F;
+    robot_angular_velocity_multiplier = 1.76F;
+    robot_input_timeout = 500;
   }
 
   // Servo
@@ -33,5 +34,7 @@ struct Parameters : DiffDriveParams {
 
   float battery_min_voltage = 10.0;
 
+  bool mecanum_wheels = false;
+
   void load(ros::NodeHandle &nh);
 };
diff --git a/lib/diff_drive_lib b/lib/diff_drive_lib
diff --git a/lib/rosserial_lib b/lib/rosserial_lib
+21 −0		LICENSE
+0 −135		diff_drive_controller.cpp
+8 −1		include/diff_drive_lib/circular_buffer.hpp
+18 −0		include/diff_drive_lib/diff_drive_controller.hpp
+18 −0		include/diff_drive_lib/mecanum_controller.hpp
+4 −0		include/diff_drive_lib/motor_controller_interface.hpp
+4 −0		include/diff_drive_lib/pid_regulator.hpp
+40 −25		include/diff_drive_lib/robot_controller.hpp
+36 −6		include/diff_drive_lib/wheel_controller.hpp
+15 −0		library.json
+74 −0		src/diff_drive_controller.cpp
+92 −0		src/mecanum_controller.cpp
+5 −1		src/pid_regulator.cpp
+84 −0		src/robot_controller.cpp
+42 −12		src/wheel_controller.cpp
+184 −0		leo_msgs/SetImuCalibration.h
+206 −0		leo_msgs/WheelOdomMecanum.h