foc.cpp

00001 
00002 #include "foc.h"
00003 using namespace FastMath;
00004 
00005 
00006 void abc( float theta, float d, float q, float *a, float *b, float *c){
00007     /// Inverse DQ0 Transform ///
00008     ///Phase current amplitude = lengh of dq vector///
00009     ///i.e. iq = 1, id = 0, peak phase current of 1///
00010     float cf = FastCos(theta);
00011     float sf = FastSin(theta);
00012     
00013     *a = cf*d - sf*q;                // Faster Inverse DQ0 transform
00014     *b = (0.86602540378f*sf-.5f*cf)*d - (-0.86602540378f*cf-.5f*sf)*q;
00015     *c = (-0.86602540378f*sf-.5f*cf)*d - (0.86602540378f*cf-.5f*sf)*q;
00016     }
00017     
00018     
00019 void dq0(float theta, float a, float b, float c, float *d, float *q){
00020     /// DQ0 Transform ///
00021     ///Phase current amplitude = lengh of dq vector///
00022     ///i.e. iq = 1, id = 0, peak phase current of 1///
00023     
00024     float cf = FastCos(theta);
00025     float sf = FastSin(theta);
00026     
00027     *d = 0.6666667f*(cf*a + (0.86602540378f*sf-.5f*cf)*b + (-0.86602540378f*sf-.5f*cf)*c);   ///Faster DQ0 Transform
00028     *q = 0.6666667f*(-sf*a - (-0.86602540378f*cf-.5f*sf)*b - (0.86602540378f*cf-.5f*sf)*c);
00029        
00030     }
00031     
00032 void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w){
00033     /// Space Vector Modulation ///
00034     /// u,v,w amplitude = v_bus for full modulation depth ///
00035     
00036     float v_offset = (fminf3(u, v, w) + fmaxf3(u, v, w))/2.0f;
00037     *dtc_u = fminf(fmaxf(((u -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX);
00038     *dtc_v = fminf(fmaxf(((v -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX);
00039     *dtc_w = fminf(fmaxf(((w -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX);  
00040     
00041     }
00042 
00043 void zero_current(int *offset_1, int *offset_2){                                // Measure zero-offset of the current sensors
00044     int adc1_offset = 0;
00045     int adc2_offset = 0;
00046     int n = 1024;
00047     for (int i = 0; i<n; i++){                                                  // Average n samples of the ADC
00048         TIM1->CCR3 = (PWM_ARR>>1)*(1.0f);                                               // Write duty cycles
00049         TIM1->CCR2 = (PWM_ARR>>1)*(1.0f);
00050         TIM1->CCR1 = (PWM_ARR>>1)*(1.0f);
00051         ADC1->CR2  |= 0x40000000;                                               // Begin sample and conversion
00052         wait(.001);
00053         adc2_offset += ADC2->DR;
00054         adc1_offset += ADC1->DR;
00055         }
00056     *offset_1 = adc1_offset/n;
00057     *offset_2 = adc2_offset/n;
00058     }
00059 
00060 void reset_foc(ControllerStruct *controller){
00061     TIM1->CCR3 = (PWM_ARR>>1)*(0.5f);
00062     TIM1->CCR1 = (PWM_ARR>>1)*(0.5f);
00063     TIM1->CCR2 = (PWM_ARR>>1)*(0.5f);
00064     controller->i_d_ref = 0;
00065     controller->i_q_ref = 0;
00066     controller->i_d = 0;
00067     controller->i_q = 0;
00068     controller->i_q_filt = 0;
00069     controller->q_int = 0;
00070     controller->d_int = 0;
00071     controller->v_q = 0;
00072     controller->v_d = 0;
00073     }
00074 
00075 
00076 void commutate(ControllerStruct *controller, ObserverStruct *observer, GPIOStruct *gpio, float theta){
00077        /// Observer Prediction ///
00078        observer->i_d_est += DT*(observer->i_d_dot);
00079        observer->i_q_est += DT*(observer->i_q_dot);
00080        
00081        /// Commutation Loop ///
00082        controller->loop_count ++;   
00083        if(PHASE_ORDER){                                                                          // Check current sensor ordering
00084            controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);    // Calculate phase currents from ADC readings
00085            controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
00086            }
00087         else{
00088             controller->i_b = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);    
00089            controller->i_c = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
00090            }
00091        controller->i_a = -controller->i_b - controller->i_c;       
00092        
00093        float s = FastSin(theta); 
00094        float c = FastCos(theta);                            
00095        dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q);    //dq0 transform on currents
00096        //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
00097        //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);
00098         
00099         controller->i_q_filt = 0.95f*controller->i_q_filt + 0.05f*controller->i_q;
00100         observer->i_d_m = controller->i_d;
00101         observer->i_q_m = controller->i_q;
00102         
00103         observer->e_d = observer->i_d_m - observer->i_d_est;
00104         observer->e_q = observer->i_q_m - observer->i_q_est;
00105         observer->e_d_int += observer->e_d;
00106         observer->e_q_int += observer->e_q;
00107         
00108         observer->i_d_est +=  K_O*observer->e_d + .001f*observer->e_d_int;
00109         observer->i_q_est += K_O*observer->e_q + .001f*observer->e_q_int;
00110         
00111         
00112         float scog12 = FastSin(12.0f*theta);
00113         float scog1 = s;
00114         float cogging_current = 0.25f*scog1 - 0.3f*scog12;
00115        
00116        /// PI Controller ///
00117        float i_d_error = controller->i_d_ref - controller->i_d;
00118        float i_q_error = controller->i_q_ref - controller->i_q  + cogging_current;
00119        
00120        float v_d_ff = 2.0f*(controller->i_d_ref*R_PHASE - controller->dtheta_elec*L_Q*controller->i_q_ref);   //feed-forward voltages
00121        float v_q_ff =  2.0f*(controller->i_q_ref*R_PHASE  + controller->dtheta_elec*(L_D*controller->i_d_ref + WB));
00122        
00123        controller->d_int += i_d_error;   
00124        controller->q_int += i_q_error;
00125        
00126        //v_d_ff = 0;
00127        //v_q_ff = 0;
00128        
00129        limit_norm(&controller->d_int, &controller->q_int, V_BUS/(K_SCALE*I_BW*KI_Q));        // Limit integrators to prevent windup
00130        controller->v_d = K_SCALE*I_BW*i_d_error + K_SCALE*I_BW*KI_D*controller->d_int;// + v_d_ff;  
00131        controller->v_q = K_SCALE*I_BW*i_q_error + K_SCALE*I_BW*KI_Q*controller->q_int;// + v_q_ff; 
00132        
00133        //controller->v_q = 4.0f;
00134        //controller->v_d = 0.0f;
00135        
00136        //controller->v_d = v_d_ff;
00137        //controller->v_q = v_q_ff; 
00138        
00139        limit_norm(&controller->v_d, &controller->v_q, OVERMODULATION*controller->v_bus);       // Normalize voltage vector to lie within curcle of radius v_bus
00140        abc(controller->theta_elec, controller->v_d, controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //inverse dq0 transform on voltages
00141     
00142         //controller->v_u = c*controller->v_d - s*controller->v_q;                // Faster Inverse DQ0 transform
00143         //controller->v_v = (0.86602540378f*s-.5f*c)*controller->v_d - (-0.86602540378f*c-.5f*s)*controller->v_q;
00144         //controller->v_w = (-0.86602540378f*s-.5f*c)*controller->v_d - (0.86602540378f*c-.5f*s)*controller->v_q;
00145        svm(controller->v_bus, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation
00146 
00147        observer->i_d_dot = 0.5f*(controller->v_d - 2.0f*(observer->i_d_est*R_PHASE - controller->dtheta_elec*L_Q*observer->i_q_est))/L_D;   //feed-forward voltage
00148        observer->i_q_dot =  0.5f*(controller->v_q - 2.0f*(observer->i_q_est*R_PHASE  + controller->dtheta_elec*(L_D*observer->i_d_est + WB)))/L_Q;
00149        
00150        if(PHASE_ORDER){                                                         // Check which phase order to use, 
00151             TIM1->CCR3 = (PWM_ARR)*(1.0f-controller->dtc_u);                        // Write duty cycles
00152             TIM1->CCR2 = (PWM_ARR)*(1.0f-controller->dtc_v);
00153             TIM1->CCR1 = (PWM_ARR)*(1.0f-controller->dtc_w);
00154         }
00155         else{
00156             TIM1->CCR3 = (PWM_ARR)*(1.0f-controller->dtc_u);
00157             TIM1->CCR1 = (PWM_ARR)*(1.0f-controller->dtc_v);
00158             TIM1->CCR2 =  (PWM_ARR)*(1.0f-controller->dtc_w);
00159         }
00160 
00161        controller->theta_elec = theta;                                          //For some reason putting this at the front breaks thins
00162        
00163 
00164        if(controller->loop_count >400){
00165            //controller->i_q_ref = -controller->i_q_ref;
00166           controller->loop_count  = 0;
00167            
00168            //printf("%.2f  %.2f  %.2f\n\r", controller->i_a, controller->i_b, controller->i_c);
00169            //printf("%f\n\r", controller->dtheta_mech*GR);
00170            //pc.printf("%f    %f    %f\n\r", controller->i_a, controller->i_b, controller->i_c);
00171            //pc.printf("%f    %f\n\r", controller->i_d, controller->i_q);
00172            //pc.printf("%d    %d\n\r", controller->adc1_raw, controller->adc2_raw);
00173             }
00174     }
00175     
00176     
00177 void torque_control(ControllerStruct *controller){
00178     float torque_ref = controller->kp*(controller->p_des - controller->theta_mech) + controller->t_ff + controller->kd*(controller->v_des - controller->dtheta_mech);
00179     //float torque_ref = -.1*(controller->p_des - controller->theta_mech);
00180     controller->i_q_ref = torque_ref/KT_OUT;    
00181     controller->i_d_ref = 0.0f;
00182     }
00183 
00184 
00185 /*    
00186 void zero_encoder(ControllerStruct *controller, GPIOStruct *gpio, ){
00187     
00188     }
00189 */