Triangle wave implementation is changed.

boyali · Jun 1, 2022 · 031da29 · 031da29
1 parent 410de74
commit 031da29
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 9 deletions.
diff --git a/control/autoware_control_toolbox/CMakeLists.txt b/control/autoware_control_toolbox/CMakeLists.txt
@@ -177,6 +177,7 @@ ament_auto_add_executable(main_pade usage_examples/main_pade.cpp)
 ament_auto_add_executable(main_tf2ss usage_examples/main_tf2ss.cpp)
 ament_auto_add_executable(main_tf_algebra usage_examples/main_tf_algebra.cpp)
 ament_auto_add_executable(main_balance usage_examples/main_balance.cpp)
+ament_auto_add_executable(main_signal_generator usage_examples/main_signal_generator.cpp)
 #target_link_libraries(main_tf fmt::fmt stdc++fs ${AUTOWARE_CONTROL_TOOLBOX_LIB})
 #target_link_directories(main_tf PUBLIC ${AUTOWARE_CONTROL_LIB_INCLUDE_DIR})
 #target_include_directories(main_tf PRIVATE include include/utils)
diff --git a/control/autoware_control_toolbox/src/utils_act/act_signal_builder.cpp b/control/autoware_control_toolbox/src/utils_act/act_signal_builder.cpp
@@ -45,24 +45,24 @@ Eigen::VectorXd ns_control_toolbox::make_square_signal(const Eigen::VectorXd& ti
 Eigen::VectorXd ns_control_toolbox::make_triangle_signal(const Eigen::VectorXd& time_vec, const double& frequency_hz)
 {
 	double const w_ = 2 * M_PI * frequency_hz; // [rad/sec]
-	double half_period{ 1. };
+	double period{ 1. };
 
 	if (frequency_hz > 0)
 	{
-		half_period = 1. / frequency_hz / 2.;
+		period = 1. / frequency_hz;
 	}
 
+	auto tmax = time_vec.maxCoeff();
+
 	// define a starting point.
-	double y0{};
+
 	auto trg_vec = Eigen::VectorXd(time_vec.unaryExpr([&](auto const& t)
 	                                                  {
-		                                                  auto slope =
-				                                                  sin(w_ * t) < 0 ? -1. : sin(w_ * t) == 0 ? 0. : 1.;
+		                                                  auto tn = t / tmax;
+		                                                  auto y = 2 * std::fabs(
+				                                                  tn / period - std::floor(tn / period + 1. / 2));
 
-		                                                  // use normalized time intervals.
-		                                                  auto&& dt = std::fmod(t, half_period) / half_period;
-		                                                  y0 += slope * dt;
-		                                                  return y0;
+		                                                  return y;
 	                                                  }));
 	return trg_vec;
 }
diff --git a/control/autoware_control_toolbox/usage_examples/main_signal_generator.cpp b/control/autoware_control_toolbox/usage_examples/main_signal_generator.cpp
@@ -0,0 +1,17 @@
+//
+// Created by ali on 1/06/22.
+//
+
+#include "autoware_control_toolbox.hpp"
+
+int main()
+{
+	auto time_vec = ns_control_toolbox::make_time_signal(0.1, 10);
+	ns_eigen_utils::printEigenMat(time_vec);
+
+	double frequency_hz = 2;
+	// test triangle wave
+	auto triangle_wave = ns_control_toolbox::make_triangle_signal(time_vec, frequency_hz);
+	ns_eigen_utils::printEigenMat(triangle_wave);
+	return 0;
+}