diff --git a/include/math/foc.hpp b/include/math/foc.hpp index b750793..c97611c 100644 --- a/include/math/foc.hpp +++ b/include/math/foc.hpp @@ -37,4 +37,6 @@ FrameABC inverse_clarke_transform(FrameAlphaBeta X); FrameAlphaBeta inverse_park_transform(FrameDQ X, float angle); +FrameABC space_vector_modulation(FrameAlphaBeta U); + } // namespace math diff --git a/src/math/foc.cpp b/src/math/foc.cpp index efed937..9287ce5 100644 --- a/src/math/foc.cpp +++ b/src/math/foc.cpp @@ -35,3 +35,105 @@ math::FrameAlphaBeta math::inverse_park_transform(math::FrameDQ X, float angle) Y.beta = X.d * sine + X.q * cosine; return Y; } + +math::FrameABC math::space_vector_modulation(math::FrameAlphaBeta U) +{ + math::FrameABC duty; + + float Z = U.beta * 0.5F - U.alpha * 0.8660254037844386F; + float Y = U.alpha * 0.8660254037844386F + U.beta * 0.5F; + float X = U.beta; + + // Sector 1 + if ((-Y <= 0.F) && (-Z > 0.F) && (X > 0.F)) { + float T1 = (Z - X + 1.F) / 2; + float T2 = T1 + X; + float T3 = T2 - Z; + + duty.a = T3; + duty.b = T2; + duty.c = T1; + } + // Sector 2 + else if ((-Y <= 0.F) && (-Z <= 0.F)) { + float T1 = (1.F - Y - Z) / 2; + float T2 = T1 + Y; + float T3 = T2 + Z; + + duty.a = T2; + duty.b = T3; + duty.c = T1; + } + // Sector 3 + else if ((-Y > 0.F) && (-Z <= 0.F) && (X > 0.F)) { + float T1 = (Y - X + 1.F) / 2; + float T2 = T1 - Y; + float T3 = T2 + X; + + duty.a = T1; + duty.b = T3; + duty.c = T2; + } + // Sector 4 + else if ((-Y > 0.F) && (-Z <= 0.F) && (X <= 0.F)) { + float T1 = (X - Z + 1.F) / 2; + float T2 = T1 + Z; + float T3 = T2 - X; + + duty.a = T1; + duty.b = T2; + duty.c = T3; + } + // Sector 5 + else if ((-Y > 0.F) && (-Z > 0.F)) { + float T1 = (Z + Y + 1.F) / 2; + float T2 = T1 - Z; + float T3 = T2 - Y; + + duty.a = T2; + duty.b = T1; + duty.c = T3; + } + // Sector 6 + else if ((-Y <= 0.F) && (-Z > 0.F) && (X <= 0.F)) { + float T1 = (X - Y + 1.F) / 2; + float T2 = T1 - X; + float T3 = T2 + Y; + + duty.a = T3; + duty.b = T1; + duty.c = T2; + } + // Default + else { + duty.a = 0.F; + duty.b = 0.F; + duty.c = 0.F; + } + + // Saturation of Duty A + if (duty.a > 1.F) + duty.a = 1.F; + else if (duty.a < 0.F) + duty.a = 0.F; + else + duty.a = duty.a; + + // Saturation of Duty B + if (duty.b > 1.F) + duty.b = 1.F; + else if (duty.b < 0.F) + duty.b = 0.F; + else + duty.b = duty.b; + + // Saturation of Duty C + if (duty.c > 1.F) + duty.c = 1.F; + else if (duty.c < 0.F) + duty.c = 0.F; + else + duty.c = duty.c; + + return duty; +} \ No newline at end of file