177 motion control create new differentialdrive class

lib/utils/include/utils.hpp

@@ -30,3 +30,21 @@ bool areDoublesEqual(double a, double b, double epsilon = 1e-3);
     #define COGIP_DEBUG_COUT(x)
     #define COGIP_DEBUG_CERR(x)
 #endif
+
+/// @brief Saturate a value between min and max
+/// @param x Value to saturate
+/// @param min minimun interval
+/// @param max maxium insterval
+/// @return satured value
+inline double saturate(double x, double min, double max)
+{
+    if (x < min) {
+        return min;
+    }
+    else if (x > max) {
+        return max;
+    }
+    else {
+        return x;
+    }
+}

motion_control/drive_controller/Makefile

@@ -0,0 +1 @@
+include $(RIOTBASE)/Makefile.base

motion_control/drive_controller/Makefile.include

@@ -0,0 +1,2 @@
+USEMODULE_INCLUDES_drive_controller := $(LAST_MAKEFILEDIR)/include
+USEMODULE_INCLUDES += $(USEMODULE_INCLUDES_drive_controller)

motion_control/drive_controller/include/drive_controller/DifferentialDriveController.hpp

@@ -0,0 +1,62 @@
+#pragma once
+
+#include "etl/math_constants.h"
+
+#include "cogip_defs/Polar.hpp"
+
+#include "utils.hpp"
+
+#include "DriveControllerInterface.hpp"
+#include "DifferentialDriveControllerParameters.hpp"
+#include "motor/MotorInterface.hpp"
+
+namespace cogip {
+
+namespace drive_controller {
+
+class DifferentialDriveController: public DriveControllerInterface {
+public:
+    DifferentialDriveController(DifferentialDriveControllerParameters &parameters,
+                                cogip::motor::MotorInterface &left_motor, 
+                                cogip::motor::MotorInterface &right_motor) :
+                                parameters_(parameters), left_motor_(left_motor), right_motor_(right_motor)
+    {
+    }
+
+    /// @brief Set polar velocity command
+    /// @param command polar velocity command reference
+    /// @return  0 on success, negative on error
+    int set_polar_velocity(cogip_defs::Polar &command) override
+    {
+        // Compute wheel speed in mm/period from polar speed
+        const double left_wheel_speed_mm_period = command.distance() - (command.angle() * parameters_.track_width_mm() / 2);
+        const double right_wheel_speed_mm_period = command.distance() + (command.angle() * parameters_.track_width_mm() / 2);
+
+        // Compute wheel speed in mm/s and rad/s from mm/period and rad/period speeds
+        const double left_wheel_speed_mm_s = left_wheel_speed_mm_period * 1000 / parameters_.loop_period_ms();
+        const double right_wheel_speed_mm_s = right_wheel_speed_mm_period * 1000/ parameters_.loop_period_ms();
+
+        // Compute motor speed in percent using the motor constant.
+        // The motor constant allow convert a speed in mm/s into a speed ratio (% of nominal motor voltage).
+        const double left_motor_speed_percent = left_wheel_speed_mm_s / (etl::math::pi * parameters_.left_wheel_diameter_mm()) * parameters_.left_motor_constant();
+        const double right_motor_speed_percent = right_wheel_speed_mm_s / (etl::math::pi * parameters_.right_wheel_diameter_mm()) * parameters_.right_motor_constant();
+
+        // Apply motor speed
+        left_motor_.speed((int)saturate(left_motor_speed_percent, -parameters_.max_speed_percentage(), parameters_.max_speed_percentage()));
+        right_motor_.speed((int)saturate(right_motor_speed_percent, -parameters_.max_speed_percentage(), parameters_.max_speed_percentage()));
+
+        return 0;
+    }
+
+private:
+    DifferentialDriveControllerParameters &parameters_;
+
+    cogip::motor::MotorInterface &left_motor_;
+    cogip::motor::MotorInterface &right_motor_;
+};
+
+} // namespace drive_controller
+
+} // namespace cogip
+
+// @}

motion_control/drive_controller/include/drive_controller/DifferentialDriveControllerParameters.hpp

@@ -0,0 +1,120 @@
+// Copyright (C) 2024 COGIP Robotics association <[email protected]>
+// This file is subject to the terms and conditions of the GNU Lesser
+// General Public License v2.1. See the file LICENSE in the top level
+// directory for more details.
+
+/// @brief       Differential drive controller
+/// @author      Mathis Lécrivain <[email protected]>
+
+#pragma once
+
+namespace cogip {
+
+namespace drive_controller {
+
+class DifferentialDriveControllerParameters {
+public:
+    /// @brief Construct a new DifferentialDriveControllerParameters object
+    /// @param left_wheel_diameter left wheel diameter value (mm)
+    /// @param right_wheel_diameter right wheel diameter value (mm)
+    /// @param track_width Track width value (mm)
+    /// @param left_motor_constant left motor constant (no unit)
+    /// @param right_motor_constant right motor constant (no unit)
+    /// @param max_speed_percentage max speed ration to send to motors (%) in range [0;100]
+    /// @param loop_period regulation loop period (ms)
+    DifferentialDriveControllerParameters(double left_wheel_diameter,
+                                          double right_wheel_diameter,
+                                          double track_width,
+                                          double left_motor_constant,
+                                          double right_motor_constant,
+                                          double max_speed_percentage,
+                                          double loop_period)
+                                          : left_wheel_diameter_(left_wheel_diameter),
+                                            right_wheel_diameter_(right_wheel_diameter),
+                                            track_width_(track_width),
+                                            left_motor_constant_(left_motor_constant),
+                                            right_motor_constant_(right_motor_constant),
+                                            max_speed_percentage_(max_speed_percentage),
+                                            loop_period_(loop_period) {};
+
+    /// @brief Set the left motor wheel diameter dimension
+    /// @param wheel_diameter Wheel diameter value (mm)
+    void set_left_wheel_diameter(double wheel_diameter) { left_wheel_diameter_ = wheel_diameter; }
+
+    /// @brief Return left motor wheel diameter value in mm.
+    /// @return double Wheel diameter value (mm)
+    double left_wheel_diameter_mm() const { return left_wheel_diameter_; }
+
+    /// @brief Set the right wheel motor diameter dimension
+    /// @param wheel_diameter Wheel diameter value (mm)
+    void set_right_wheel_diameter(double wheel_diameter) { right_wheel_diameter_ = wheel_diameter; }
+
+    /// @brief Return right motor wheel diameter value in mm.
+    /// @return double Wheel diameter value (mm)
+    double right_wheel_diameter_mm() const { return right_wheel_diameter_; }
+
+    /// @brief Set the left motor constant value.
+    ///        This value needs to be defined by using the motors characteristics from the datasheet.
+    ///        This value is useful to convert linear velocity in mm/s to a speed ration between 0 to 100%.
+    ///        This speed ratio need to by applied to the max motor voltage in order to get the voltage needed
+    ///        to theoretically reach the desired speed.
+    ///
+    /// @note motor_constant = ((60 * Reduction Ratio / velocity Constant (RPM/V)) / Motor Voltage (V)) * 100
+    /// @param motor_constant motor constant (no unit)
+    void set_left_motor_constant(double motor_constant) { left_motor_constant_ = motor_constant; }
+
+    /// @brief Return left motor constant value
+    /// @return double motor constant (no unit)
+    double left_motor_constant() const { return left_motor_constant_; }
+
+    /// @brief Set the right wheel motor constant value.
+    ///        This value needs to be defined by using the motors characteristics from the datasheet.
+    ///        This value is useful to convert linear velocity in mm/s to a speed ration between 0 to 100%.
+    ///        This speed ratio need to by applied to the max motor voltage in order to get the voltage needed
+    ///        to theoretically reach the desired speed.
+    ///
+    /// @note motor_constant = ((60 * Reduction Ratio / velocity Constant (RPM/V)) / Motor Voltage (V)) * 100
+    /// @param motor_constant motor constant (no unit)
+    void set_right_motor_constant(double motor_constant) { right_motor_constant_ = motor_constant; }
+
+    /// @brief Return right motor constant value
+    /// @return double Wheel motor constant (no unit)
+    double right_motor_constant() const { return right_motor_constant_; }
+
+    /// @brief Set the axle track dimension
+    /// @param track_width Track width value (mm)
+    void set_track_width(double track_width) { track_width_ = track_width; }
+
+    /// @brief Get the axle track value
+    /// @return double Track width value (mm)
+    double track_width_mm() const { return track_width_; }
+
+    /// @brief Set the max speed ratio in percent
+    /// @param max speed ratio (%)
+    void set_max_speed_percentage(double max) { max_speed_percentage_ = max; }
+
+    /// @brief get the max speed ratio in percent
+    /// @return double speed ratio (%)
+    double max_speed_percentage() const { return max_speed_percentage_; }
+
+    /// @brief Set the loop period in milliseconds
+    /// @param period loop period (ms)
+    void set_loop_period(double period) { loop_period_ = period; }
+
+    /// @brief Get the axle track value
+    /// @return double Track width value (mm)
+    double loop_period_ms() const { return loop_period_; }
+
+private:
+    double left_wheel_diameter_;
+    double right_wheel_diameter_;
+    double track_width_;
+    double left_motor_constant_;
+    double right_motor_constant_;
+    double max_speed_percentage_;
+    double loop_period_;
+};
+
+} // namespace odometer
+
+} // namespace cogip

motion_control/drive_controller/include/drive_controller/DriveControllerInterface.hpp

@@ -0,0 +1,34 @@
+// Copyright (C) 2024 COGIP Robotics association <[email protected]>
+// This file is subject to the terms and conditions of the GNU Lesser
+// General Public License v2.1. See the file LICENSE in the top level
+// directory for more details.
+
+/// @brief       Drive controller interface
+/// @author      Mathis Lécrivain <[email protected]>
+
+#pragma once
+
+#include "cogip_defs/Polar.hpp"
+
+namespace cogip {
+
+namespace drive_controller {
+
+class DriveControllerInterface {
+public:
+    /// @brief Virtual destructor
+    ~DriveControllerInterface()
+    {
+    }
+
+    /// @brief Set polar velocity command
+    /// @param command polar velocity command reference
+    /// @return  0 on success, negative on error
+    virtual int set_polar_velocity(cogip_defs::Polar &command) = 0;
+};
+
+} // namespace drive_controller
+
+} // namespace cogip
+
+// @}