.. _program_listing_file__tmp_ws_src_raspimouse_ros2_examples_include_raspimouse_ros2_examples_direction_controller_component.hpp:

Program Listing for File direction_controller_component.hpp
===========================================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_raspimouse_ros2_examples_include_raspimouse_ros2_examples_direction_controller_component.hpp>` (``/tmp/ws/src/raspimouse_ros2_examples/include/raspimouse_ros2_examples/direction_controller_component.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   // Copyright 2020 RT Corporation
   //
   // Licensed under the Apache License, Version 2.0 (the "License");
   // you may not use this file except in compliance with the License.
   // You may obtain a copy of the License at
   //
   //     http://www.apache.org/licenses/LICENSE-2.0
   //
   // Unless required by applicable law or agreed to in writing, software
   // distributed under the License is distributed on an "AS IS" BASIS,
   // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   // See the License for the specific language governing permissions and
   // limitations under the License.
   
   #ifndef RASPIMOUSE_ROS2_EXAMPLES__DIRECTION_CONTROLLER_COMPONENT_HPP_
   #define RASPIMOUSE_ROS2_EXAMPLES__DIRECTION_CONTROLLER_COMPONENT_HPP_
   
   #include <memory>
   #include <string>
   #include <vector>
   
   #include "raspimouse_ros2_examples/visibility_control.h"
   #include "raspimouse_msgs/msg/switches.hpp"
   #include "rclcpp/rclcpp.hpp"
   #include "sensor_msgs/msg/imu.hpp"
   #include "std_msgs/msg/int16.hpp"
   #include "std_msgs/msg/float64.hpp"
   #include "std_msgs/msg/string.hpp"
   #include "std_srvs/srv/set_bool.hpp"
   #include "geometry_msgs/msg/twist.hpp"
   #include "geometry_msgs/msg/vector3.hpp"
   
   namespace direction_controller
   {
   
   class PIDController
   {
   public:
     PIDController()
     : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0),
       error1_(0.0), error2_(0.0), output_(0.0)
     {
     }
   
     double update(const double current, const double target)
     {
       double error = target - current;
   
       double delta_output = p_gain_ * (error - error1_);
       delta_output += i_gain_ * (error);
       delta_output += d_gain_ * (error - 2.0 * error1_ + error2_);
   
       output_ += delta_output;
   
       error2_ = error1_;
       error1_ = error;
   
       return output_;
     }
   
     void set_gain(const double p_gain, const double i_gain, const double d_gain)
     {
       p_gain_ = p_gain;
       i_gain_ = i_gain;
       d_gain_ = d_gain;
     }
   
     void reset_output_and_errors()
     {
       error1_ = 0.0;
       error2_ = 0.0;
       output_ = 0.0;
     }
   
   private:
     double p_gain_;
     double i_gain_;
     double d_gain_;
     double error1_;
     double error2_;
     double output_;
   };
   
   class Controller : public rclcpp::Node
   {
   public:
     RASPIMOUSE_ROS2_EXAMPLES_PUBLIC
     explicit Controller(const rclcpp::NodeOptions & options);
   
   protected:
     void on_cmd_vel_timer();
   
   private:
     raspimouse_msgs::msg::Switches switches_;
     sensor_msgs::msg::Imu imu_data_raw_;
   
     rclcpp::Publisher<std_msgs::msg::Int16>::SharedPtr buzzer_pub_;
     rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
     rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr heading_angle_pub_;
     rclcpp::Subscription<raspimouse_msgs::msg::Switches>::SharedPtr switches_sub_;
     rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_data_raw_sub_;
     rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr motor_power_client_;
   
     rclcpp::TimerBase::SharedPtr cmd_vel_timer_;
   
     int pressed_switch_number_;
     int control_mode_;
     PIDController omega_pid_controller_;
     std::vector<double> omega_samples_;
     double omega_bias_;
     double heading_angle_;
     double prev_heading_calculation_time_;
     double target_angle_;
     bool increase_target_angle_;
     geometry_msgs::msg::Vector3 filtered_acc_;
     geometry_msgs::msg::Vector3 prev_acc_;
   
     void callback_switches(const raspimouse_msgs::msg::Switches::SharedPtr msg);
     void callback_imu_data_raw(const sensor_msgs::msg::Imu::SharedPtr msg);
   
     bool set_motor_power(const bool motor_on);
     void filter_acceleration(const geometry_msgs::msg::Vector3 acc);
     bool omega_calibration(const double omega);
     void calculate_heading_angle(const double omega, const double current_time);
     void angle_control(const double target_angle);
     void rotation(void);
     void beep_buzzer(const int freq, const std::chrono::nanoseconds & beep_time);
     void beep_start(void);
     void beep_success(void);
     void beep_failure(void);
   };
   
   }  // namespace direction_controller
   
   #endif  // RASPIMOUSE_ROS2_EXAMPLES__DIRECTION_CONTROLLER_COMPONENT_HPP_