Added bemf observer test and fixed libfoc error

CMakeLists.txt

@@ -12,14 +12,15 @@ enable_testing()
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin)
 
 # Declare a C++ library
-add_library(bemf_observer
+add_library(${PROJECT_NAME}
     src/controllers/pid_controller.cpp
     src/controllers/pi_controller.cpp
-    src/math/foc.cpp
     src/math/functions.cpp
+    src/math/foc.cpp
     src/math/operators.cpp
     src/observers/tracker.cpp
     src/observers/dq_update.cpp
+    src/observers/bemf_observer.cpp
 )
 
 ## Add C++ tests here
@@ -31,6 +32,10 @@ add_executable(pid_test tests/pid_test.cpp)
 target_link_libraries(pid_test ${PROJECT_NAME})
 add_test(NAME pid_test COMMAND "${CMAKE_SOURCE_DIR}/bin/pid_test")
 
+add_executable(bemf_observer_test tests/bemf_observer_test.cpp)
+target_link_libraries(bemf_observer_test ${PROJECT_NAME})
+add_test(NAME bemf_observer_test COMMAND "${CMAKE_SOURCE_DIR}/bin/bemf_observer_test")
+
 # Include Header files
 include_directories(
     include

build.sh

@@ -35,12 +35,16 @@ fi
 cd ${BUILD_DIR}
 
 # Configure and build the project.
-cmake ${PROJECT_DIR}
+cmake ..
 make -j4
-ctest -C Release
 
-# Executable directory.
-EXEC_DIR=${PROJECT_DIR}/bin
+# If make completes successfully, run ctest
+if [ $? -eq 0 ]; then
+  cd ${BUILD_DIR}
+  ctest -C Release
+else
+  echo -e "\nMake failed, so ctest will not be run."
+fi
 
 echo -e "\nExecutable files generated:"
-cd "$EXEC_DIR" && find . -type f -executable
+cd "$PROJECT_DIR" && find ./bin -type f -executable

include/math/foc.hpp

@@ -30,6 +30,11 @@ struct frame_dq
 };
 
 frame_alpha_beta clarke_transform(frame_abc X);
+
 frame_dq park_transform(frame_alpha_beta X, float angle);
 
+frame_abc inverse_clarke_transform(frame_alpha_beta X);
+
+frame_alpha_beta inverse_park_transform(frame_dq X, float angle);
+
 }  // namespace math

include/observers/bemf_observer.hpp

@@ -0,0 +1,18 @@
+# pragma once
+
+#include <observers/dq_update.hpp>
+#include <observers/tracker.hpp>
+#include <math/foc.hpp>
+
+namespace observers
+{
+class bemf_observer
+{
+    bemf_solver dq_update;
+    position_tracker tracker;
+    float speed, angle, Ts;
+  public:
+    bemf_observer(const bemf_gains& gains, const controllers::pi_config& update_config, const controllers::pi_config& tracker_config, float Ts);
+    float loop(math::frame_abc line_currents, math::frame_abc line_voltages);
+};
+} // namespace observers

include/observers/dq_update.hpp

@@ -8,6 +8,21 @@ namespace observers
 struct bemf_gains
 {
   float VOLTAGE_GAIN, CURRENT_GAIN, EMF_GAIN, SPEED_CURRENT_GAIN;
+  bemf_gains(float Ld, float Lq, float Rs, float Ts, int axis)
+  {
+    if(axis == 0) {
+      VOLTAGE_GAIN = Ts/(2*Ld + Rs*Ts);
+      CURRENT_GAIN = Lq*Ts/(2*Ld + Rs*Ts);
+      EMF_GAIN = Ts/(2*Ld + Rs*Ts);
+      SPEED_CURRENT_GAIN = (2*Ld - Rs*Ts)/(2*Ld + Rs*Ts);
+    }
+    if(axis == 1) {
+      VOLTAGE_GAIN = Ts/(2*Lq + Rs*Ts);
+      CURRENT_GAIN = -Ld*Ts/(2*Lq + Rs*Ts);
+      EMF_GAIN = Ts/(2*Lq + Rs*Ts);
+      SPEED_CURRENT_GAIN = (2*Lq - Rs*Ts)/(2*Lq + Rs*Ts);
+    }
+  }
 };
 
 class bemf_solver

src/math/foc.cpp

@@ -1,19 +1,37 @@
 #include <math/foc.hpp>
 #include <math/functions.hpp>
 
-math::frame_alpha_beta clarke_transform(math::frame_abc X)
+math::frame_alpha_beta math::clarke_transform(math::frame_abc X)
 {
   math::frame_alpha_beta Y;
   Y.alpha = X.a;
   Y.beta = (X.b - X.c) * 0.5773;
   return Y;
 }
 
-math::frame_dq park_transform(math::frame_alpha_beta X, float angle)
+math::frame_dq math::park_transform(math::frame_alpha_beta X, float angle)
 {
   math::frame_dq Y;
   float cosine = math::cos(angle), sine = math::sin(angle);
   Y.d = X.alpha * cosine + X.beta * sine;
   Y.q = -X.alpha * sine + X.beta * cosine;
   return Y;
 }
+
+math::frame_abc math::inverse_clarke_transform(math::frame_alpha_beta X)
+{
+  math::frame_abc Y;
+  Y.a = X.alpha;
+  Y.b = -0.5 * X.alpha + 0.866 * X.beta;
+  Y.c = -0.5 * X.alpha - 0.866 * X.beta;
+  return Y;
+}
+
+math::frame_alpha_beta math::inverse_park_transform(math::frame_dq X, float angle)
+{
+  math::frame_alpha_beta Y;
+  float cosine = math::cos(angle), sine = math::sin(angle);
+  Y.alpha = X.d * cosine - X.q * sine;
+  Y.beta = X.d * sine + X.q * cosine;
+  return Y;
+}

src/observers/bemf_observer.cpp

@@ -0,0 +1,15 @@
+#include <observers/bemf_observer.hpp>
+
+observers::bemf_observer::bemf_observer(const bemf_gains& gains, const controllers::pi_config& update_config, const controllers::pi_config& tracker_config, float Ts)
+  : dq_update(update_config, gains, Ts), tracker(tracker_config, Ts), Ts(Ts)
+{
+}
+
+float observers::bemf_observer::loop(math::frame_abc line_currents, math::frame_abc line_voltages)
+{
+  float phase_error = dq_update.loop(line_currents, line_voltages, speed, angle);
+  float angle_est = tracker.loop(phase_error);
+  speed = tracker.speed_tracker(angle_est);
+  angle = angle_est;
+  return angle;
+}

tests/bemf_observer_test.cpp

@@ -0,0 +1,26 @@
+#include <iostream>
+#include <math/foc.hpp>
+#include <observers/bemf_observer.hpp>
+#include <observers/dq_update.hpp>
+#include <observers/tracker.hpp>
+#include <controllers/pi_controller.hpp>
+#define Ts 1e-4
+
+int main() 
+{
+  observers::bemf_gains gains(1, 1, 1, Ts, 0);
+  controllers::pi_config update_config = {1, 1, Ts, 180, -180};
+  controllers::pi_config tracker_config  = {1, 1, Ts, 180,-180};
+  observers::bemf_observer bemf_observer(gains, update_config, tracker_config, Ts);
+
+  math::frame_abc line_currents;
+  math::frame_abc line_voltages;
+  math::frame_dq U;
+
+  for(float t = 0; t < 10; t += Ts)
+  {
+    float angle = bemf_observer.loop(line_currents, line_voltages);
+    math::frame_abc duties = math::inverse_clarke_transform(math::inverse_park_transform(U, angle));
+    std::cout << "Duties: " << duties.a << " " << duties.b << " " << duties.c << std::endl;
+  }
+}