customizing code for Aliexpress 14 bit version with MA702

Dependencies:   mbed FastPWM3

Revision:
38:67e4e1453a4b
Parent:
37:c0f352d6e8e3
Child:
44:8040fa2fcb0d
--- a/FOC/foc.cpp	Fri Apr 13 13:50:54 2018 +0000
+++ b/FOC/foc.cpp	Mon May 14 20:59:02 2018 +0000
@@ -109,13 +109,13 @@
         observer->i_q_est += K_O*observer->e_q + .001f*observer->e_q_int;
         
         
-        //float s_cog = sinf(12.0f*theta);
-        
-        //float cogging_current =-0.33f*s_cog + .25f*s;
+        float scog12 = FastSin(12.0f*theta);
+        float scog1 = s;
+        float cogging_current = 0.25f*scog1 - 0.3f*scog12;
        
        /// PI Controller ///
        float i_d_error = controller->i_d_ref - controller->i_d;
-       float i_q_error = controller->i_q_ref - controller->i_q ;// + cogging_current;
+       float i_q_error = controller->i_q_ref - controller->i_q  + cogging_current;
        
        float v_d_ff = 2.0f*(controller->i_d_ref*R_PHASE - controller->dtheta_elec*L_Q*controller->i_q_ref);   //feed-forward voltages
        float v_q_ff =  2.0f*(controller->i_q_ref*R_PHASE  + controller->dtheta_elec*(L_D*controller->i_d_ref + WB));