drive controller: add differential drive controller implementation an…

…d parameters

COGIP robots are differential. So, implement the differential drive
controller in order to convert robot polar speed into speed ratio for
each motors.

The speed ratio is defined using the motor electrical informations found
in the datasheet. This ratio reflects the percentage of nominal motor
voltage to apply in order to reach the velocity wanted.

Signed-off-by: Mathis Lécrivain <[email protected]>

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