foc.cpp

00001 
00002 #include "foc.h"
00003 
00004 //using namespace FastMath;
00005     
00006 void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w){
00007     ///u,v,w amplitude = v_bus for full modulation depth///
00008     
00009     float v_offset = (fminf3(u, v, w) + fmaxf3(u, v, w))/2.0f;
00010     *dtc_u = fminf(fmaxf(((u - v_offset)*MODULATION_FACTOR/v_bus + ((DTC_MAX+DTC_MIN)/2)), DTC_MIN), DTC_MAX);
00011     *dtc_v = fminf(fmaxf(((v - v_offset)*MODULATION_FACTOR/v_bus + ((DTC_MAX+DTC_MIN)/2)), DTC_MIN), DTC_MAX);
00012     *dtc_w = fminf(fmaxf(((w - v_offset)*MODULATION_FACTOR/v_bus + ((DTC_MAX+DTC_MIN)/2)), DTC_MIN), DTC_MAX);
00013     
00014     }
00015 
00016 
00017 void reset_foc(FocStruct *controller){
00018     controller->q_int = 0;
00019     controller->d_int = 0;
00020     }
00021 
00022 
00023 void commutate(FocStruct *controller){
00024        
00025     controller->loop_count ++;
00026 //    controller->i_a = inverter->i_a; 
00027 //    controller->i_b = inverter->i_b; 
00028 //    controller->i_c = inverter->i_c;
00029     
00030     
00031     float s = sinf(controller->theta_elec); 
00032     float c = cosf(controller->theta_elec);
00033     //float s = FastSin(controller->theta_elec); 
00034     //float c = FastCos(controller->theta_elec);
00035                           
00036     controller->i_d = 0.6666667f*( c*controller->i_a + ( 0.86602540378f*s-.5f*c)*controller->i_b + (-0.86602540378f*s-.5f*c)*controller->i_c);   ///Faster DQ0 Transform
00037     controller->i_q = 0.6666667f*(-s*controller->i_a - (-0.86602540378f*c-.5f*s)*controller->i_b - ( 0.86602540378f*c-.5f*s)*controller->i_c);
00038        
00039     float i_d_error = controller->i_d_ref - controller->i_d;
00040     float i_q_error = controller->i_q_ref - controller->i_q;
00041     
00042     controller->d_int += i_d_error;   
00043     controller->q_int += i_q_error;
00044     limit_norm(&controller->d_int, &controller->q_int, V_CLIP/(K_Q*KI_Q));
00045         
00046     controller->v_d = K_D*i_d_error + K_D*KI_D*controller->d_int;// + v_d_ff;  
00047     controller->v_q = K_Q*i_q_error + K_Q*KI_Q*controller->q_int;// + v_q_ff; 
00048     
00049     limit_norm(&controller->v_d, &controller->v_q, V_CLIP);
00050     
00051     if (USE_THETA_ADV) {
00052         s = sinf(controller->theta_elec_adv); 
00053         c = cosf(controller->theta_elec_adv);
00054     }
00055     
00056     //s = FastSin(controller->theta_elec_adv); 
00057     //c = FastCos(controller->theta_elec_adv);
00058     
00059     controller->v_u = c*controller->v_d - s*controller->v_q;                // Faster Inverse DQ0 transform
00060     controller->v_v = ( 0.86602540378f*s-.5f*c)*controller->v_d - (-0.86602540378f*c-.5f*s)*controller->v_q;
00061     controller->v_w = (-0.86602540378f*s-.5f*c)*controller->v_d - ( 0.86602540378f*c-.5f*s)*controller->v_q;
00062 
00063     svm(V_BUS, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation
00064     
00065     //controller->dtc_u = 0.2;
00066     //controller->dtc_v = 0.2;
00067     //controller->dtc_w = 0.8;
00068     
00069 //    focc->dtc_u = controller->dtc_u; 
00070 //    inverter->dtc_v = controller->dtc_v; 
00071 //    inverter->dtc_w = controller->dtc_w; 
00072     
00073 
00074     if(controller->loop_count >10000){
00075         controller->loop_count  = 0;
00076     }
00077 }
00078     
00079 
00080 
00081        
00082 void voltage_mode_commutate(FocStruct *controller){
00083        
00084     controller->loop_count ++;
00085     
00086     float s = sinf(controller->theta_elec); 
00087     float c = cosf(controller->theta_elec);
00088                            
00089     controller->i_d = 0.6666667f*( c*controller->i_a + ( 0.86602540378f*s-.5f*c)*controller->i_b + (-0.86602540378f*s-.5f*c)*controller->i_c);   ///Faster DQ0 Transform
00090     controller->i_q = 0.6666667f*(-s*controller->i_a - (-0.86602540378f*c-.5f*s)*controller->i_b - ( 0.86602540378f*c-.5f*s)*controller->i_c);
00091        
00092     
00093     limit_norm(&controller->v_d, &controller->v_q, V_CLIP);
00094     
00095     if (USE_THETA_ADV) {
00096         s = sinf(controller->theta_elec_adv); 
00097         c = cosf(controller->theta_elec_adv);
00098     }
00099     
00100     controller->v_u = c*controller->v_d - s*controller->v_q;                // Faster Inverse DQ0 transform
00101     controller->v_v = ( 0.86602540378f*s-.5f*c)*controller->v_d - (-0.86602540378f*c-.5f*s)*controller->v_q;
00102     controller->v_w = (-0.86602540378f*s-.5f*c)*controller->v_d - ( 0.86602540378f*c-.5f*s)*controller->v_q;
00103 
00104     svm(V_BUS, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation
00105             
00106 
00107     if(controller->loop_count >10000){
00108         controller->loop_count  = 0;
00109     }
00110 }   
00111        
00112        
00113        
00114     
00115     
00116        
00117        
00118        
00119        
00120     
00121