Austin Brown / Mbed 2 deprecated ESCmk2_Inductance

Inductance Testing Code

Dependencies:   mbed

Fork of CurrentModeSine by Austin Brown

FOC/foc.cpp@0:9edd6ec0f56a, 2017-05-20 (annotated)

Committer:
austinbrown124
Date:
Sat May 20 21:42:20 2017 +0000
Revision:
0:9edd6ec0f56a
Child:
1:64b881306f6f
First Commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
austinbrown124 0:9edd6ec0f56a 1
austinbrown124 0:9edd6ec0f56a 2 #include "foc.h"
austinbrown124 0:9edd6ec0f56a 3
austinbrown124 0:9edd6ec0f56a 4 //#include "FastMath.h"
austinbrown124 0:9edd6ec0f56a 5 //using namespace FastMath;
austinbrown124 0:9edd6ec0f56a 6
austinbrown124 0:9edd6ec0f56a 7
austinbrown124 0:9edd6ec0f56a 8 void abc( float theta, float d, float q, float *a, float *b, float *c){
austinbrown124 0:9edd6ec0f56a 9 ///Phase current amplitude = lengh of dq vector///
austinbrown124 0:9edd6ec0f56a 10 ///i.e. iq = 1, id = 0, peak phase current of 1///
austinbrown124 0:9edd6ec0f56a 11
austinbrown124 0:9edd6ec0f56a 12 *a = d*cosf(theta) + q*sinf(theta);
austinbrown124 0:9edd6ec0f56a 13 *b = d*cosf((2.0f*PI/3.0f)+theta) + q*sinf((2.0f*PI/3.0f)+theta);
austinbrown124 0:9edd6ec0f56a 14 *c = d*cosf((-2.0f*PI/3.0f)+theta) + q*sinf((-2.0f*PI/3.0f)+theta);
austinbrown124 0:9edd6ec0f56a 15 }
austinbrown124 0:9edd6ec0f56a 16
austinbrown124 0:9edd6ec0f56a 17 void dq0(float theta, float a, float b, float c, float *d, float *q){
austinbrown124 0:9edd6ec0f56a 18 ///Phase current amplitude = lengh of dq vector///
austinbrown124 0:9edd6ec0f56a 19 ///i.e. iq = 1, id = 0, peak phase current of 1///
austinbrown124 0:9edd6ec0f56a 20
austinbrown124 0:9edd6ec0f56a 21 *d = (2.0f/3.0f)*(a*cosf(theta) + b*cosf((2.0f*PI/3.0f)+theta) + c*cosf((-2.0f*PI/3.0f)+theta));
austinbrown124 0:9edd6ec0f56a 22 *q = (2.0f/3.0f)*(a*sinf(theta) + b*sinf((2.0f*PI/3.0f)+theta) + c*sinf((-2.0f*PI/3.0f)+theta));
austinbrown124 0:9edd6ec0f56a 23 }
austinbrown124 0:9edd6ec0f56a 24
austinbrown124 0:9edd6ec0f56a 25 void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w){
austinbrown124 0:9edd6ec0f56a 26 ///u,v,w amplitude = v_bus for full modulation depth///
austinbrown124 0:9edd6ec0f56a 27
austinbrown124 0:9edd6ec0f56a 28 float v_offset = (fminf3(u, v, w) + fmaxf3(u, v, w))/2.0f;
austinbrown124 0:9edd6ec0f56a 29 *dtc_u = fminf(fmaxf(((u - v_offset)*0.5f/v_bus + ((DTC_MAX-DTC_MIN)/2)), DTC_MIN), DTC_MAX);
austinbrown124 0:9edd6ec0f56a 30 *dtc_v = fminf(fmaxf(((v - v_offset)*0.5f/v_bus + ((DTC_MAX-DTC_MIN)/2)), DTC_MIN), DTC_MAX);
austinbrown124 0:9edd6ec0f56a 31 *dtc_w = fminf(fmaxf(((w - v_offset)*0.5f/v_bus + ((DTC_MAX-DTC_MIN)/2)), DTC_MIN), DTC_MAX);
austinbrown124 0:9edd6ec0f56a 32
austinbrown124 0:9edd6ec0f56a 33 }
austinbrown124 0:9edd6ec0f56a 34
austinbrown124 0:9edd6ec0f56a 35 void zero_current(int *offset_1, int *offset_2){
austinbrown124 0:9edd6ec0f56a 36 int adc1_offset = 0;
austinbrown124 0:9edd6ec0f56a 37 int adc2_offset = 0;
austinbrown124 0:9edd6ec0f56a 38 int n = 1024;
austinbrown124 0:9edd6ec0f56a 39 for (int i = 0; i<n; i++){
austinbrown124 0:9edd6ec0f56a 40 ADC1->CR2 |= 0x40000000;
austinbrown124 0:9edd6ec0f56a 41 wait(.001);
austinbrown124 0:9edd6ec0f56a 42 adc2_offset += ADC2->DR;
austinbrown124 0:9edd6ec0f56a 43 adc1_offset += ADC1->DR;
austinbrown124 0:9edd6ec0f56a 44 }
austinbrown124 0:9edd6ec0f56a 45 *offset_1 = adc1_offset/n;
austinbrown124 0:9edd6ec0f56a 46 *offset_2 = adc2_offset/n;
austinbrown124 0:9edd6ec0f56a 47 }
austinbrown124 0:9edd6ec0f56a 48
austinbrown124 0:9edd6ec0f56a 49 void reset_foc(ControllerStruct *controller){
austinbrown124 0:9edd6ec0f56a 50 controller->q_int = 0;
austinbrown124 0:9edd6ec0f56a 51 controller->d_int = 0;
austinbrown124 0:9edd6ec0f56a 52 }
austinbrown124 0:9edd6ec0f56a 53
austinbrown124 0:9edd6ec0f56a 54
austinbrown124 0:9edd6ec0f56a 55 void commutate(ControllerStruct *controller, GPIOStruct *gpio, float theta){
austinbrown124 0:9edd6ec0f56a 56
austinbrown124 0:9edd6ec0f56a 57 controller->loop_count ++;
austinbrown124 0:9edd6ec0f56a 58 if(gpio->phasing){
austinbrown124 0:9edd6ec0f56a 59 controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings
austinbrown124 0:9edd6ec0f56a 60 controller->i_a = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
austinbrown124 0:9edd6ec0f56a 61 }
austinbrown124 0:9edd6ec0f56a 62 else{
austinbrown124 0:9edd6ec0f56a 63 controller->i_b = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset); //Calculate phase currents from ADC readings
austinbrown124 0:9edd6ec0f56a 64 controller->i_a = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
austinbrown124 0:9edd6ec0f56a 65 }
austinbrown124 0:9edd6ec0f56a 66 controller->i_c = -controller->i_b - controller->i_a;
austinbrown124 0:9edd6ec0f56a 67
austinbrown124 0:9edd6ec0f56a 68
austinbrown124 0:9edd6ec0f56a 69 dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents
austinbrown124 0:9edd6ec0f56a 70
austinbrown124 0:9edd6ec0f56a 71 ///Cogging Compensation Lookup///
austinbrown124 0:9edd6ec0f56a 72 //int ind = theta * (128.0f/(2.0f*PI));
austinbrown124 0:9edd6ec0f56a 73 //float cogging_current = controller->cogging[ind];
austinbrown124 0:9edd6ec0f56a 74 //float cogging_current = 1.0f*cos(6*theta);
austinbrown124 0:9edd6ec0f56a 75 ///Controller///
austinbrown124 0:9edd6ec0f56a 76 float i_d_error = controller->i_d_ref - controller->i_d;
austinbrown124 0:9edd6ec0f56a 77 float i_q_error = controller->i_q_ref - controller->i_q;// + cogging_current;
austinbrown124 0:9edd6ec0f56a 78 //float v_d_ff = 2.0f*(2*controller->i_d_ref*R_PHASE); //feed-forward voltage
austinbrown124 0:9edd6ec0f56a 79 //float v_q_ff = 2.0f*(2*controller->i_q_ref*R_PHASE + controller->dtheta_elec*WB*0.8165f);
austinbrown124 0:9edd6ec0f56a 80 controller->d_int += i_d_error;
austinbrown124 0:9edd6ec0f56a 81 controller->q_int += i_q_error;
austinbrown124 0:9edd6ec0f56a 82
austinbrown124 0:9edd6ec0f56a 83 //v_d_ff = 0;
austinbrown124 0:9edd6ec0f56a 84 //v_q_ff = 0;
austinbrown124 0:9edd6ec0f56a 85
austinbrown124 0:9edd6ec0f56a 86 limit_norm(&controller->d_int, &controller->q_int, V_BUS/(K_Q*KI_Q));
austinbrown124 0:9edd6ec0f56a 87 //controller->d_int = fminf(fmaxf(controller->d_int, -D_INT_LIM), D_INT_LIM);
austinbrown124 0:9edd6ec0f56a 88 //controller->q_int = fminf(fmaxf(controller->q_int, -Q_INT_LIM), Q_INT_LIM);
austinbrown124 0:9edd6ec0f56a 89
austinbrown124 0:9edd6ec0f56a 90
austinbrown124 0:9edd6ec0f56a 91 controller->v_d = K_D*i_d_error + K_D*KI_D*controller->d_int;// + v_d_ff;
austinbrown124 0:9edd6ec0f56a 92 controller->v_q = K_Q*i_q_error + K_Q*KI_Q*controller->q_int;// + v_q_ff;
austinbrown124 0:9edd6ec0f56a 93
austinbrown124 0:9edd6ec0f56a 94 //controller->v_d = v_d_ff;
austinbrown124 0:9edd6ec0f56a 95 //controller->v_q = v_q_ff;
austinbrown124 0:9edd6ec0f56a 96
austinbrown124 0:9edd6ec0f56a 97 limit_norm(&controller->v_d, &controller->v_q, controller->v_bus);
austinbrown124 0:9edd6ec0f56a 98
austinbrown124 0:9edd6ec0f56a 99 abc(controller->theta_elec, controller->v_d, controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //inverse dq0 transform on voltages
austinbrown124 0:9edd6ec0f56a 100 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
austinbrown124 0:9edd6ec0f56a 101
austinbrown124 0:9edd6ec0f56a 102 //gpio->pwm_u->write(1.0f-controller->dtc_u); //write duty cycles
austinbrown124 0:9edd6ec0f56a 103 //gpio->pwm_v->write(1.0f-controller->dtc_v);
austinbrown124 0:9edd6ec0f56a 104 //gpio->pwm_w->write(1.0f-controller->dtc_w);
austinbrown124 0:9edd6ec0f56a 105
austinbrown124 0:9edd6ec0f56a 106 //bing1 = (controller->dtc_u);
austinbrown124 0:9edd6ec0f56a 107 //bing2 = (controller->dtc_v);
austinbrown124 0:9edd6ec0f56a 108 //bing3 = (controller->dtc_w);
austinbrown124 0:9edd6ec0f56a 109 /*
austinbrown124 0:9edd6ec0f56a 110 //if(gpio->phasing){
austinbrown124 0:9edd6ec0f56a 111 TIM1->CCR1 = 0x1194*(1.0f-controller->dtc_u);
austinbrown124 0:9edd6ec0f56a 112 TIM1->CCR2 = 0x1194*(1.0f-controller->dtc_v);
austinbrown124 0:9edd6ec0f56a 113 TIM1->CCR3 = 0x1194*(1.0f-controller->dtc_w);
austinbrown124 0:9edd6ec0f56a 114 }
austinbrown124 0:9edd6ec0f56a 115 else{
austinbrown124 0:9edd6ec0f56a 116 TIM1->CCR3 = 0x1194*(1.0f-controller->dtc_u);
austinbrown124 0:9edd6ec0f56a 117 TIM1->CCR1 = 0x1194*(1.0f-controller->dtc_v);
austinbrown124 0:9edd6ec0f56a 118 TIM1->CCR2 = 0x1194*(1.0f-controller->dtc_w);
austinbrown124 0:9edd6ec0f56a 119 }*/
austinbrown124 0:9edd6ec0f56a 120 //gpio->pwm_u->write(1.0f - .05f); //write duty cycles
austinbrown124 0:9edd6ec0f56a 121 //gpio->pwm_v->write(1.0f - .05f);
austinbrown124 0:9edd6ec0f56a 122 //gpio->pwm_w->write(1.0f - .1f);
austinbrown124 0:9edd6ec0f56a 123 //TIM1->CCR1 = 0x708*(1.0f-controller->dtc_u);
austinbrown124 0:9edd6ec0f56a 124 //TIM1->CCR2 = 0x708*(1.0f-controller->dtc_v);
austinbrown124 0:9edd6ec0f56a 125 //TIM1->CCR3 = 0x708*(1.0f-controller->dtc_w);
austinbrown124 0:9edd6ec0f56a 126 controller->theta_elec = theta; //For some reason putting this at the front breaks thins
austinbrown124 0:9edd6ec0f56a 127
austinbrown124 0:9edd6ec0f56a 128
austinbrown124 0:9edd6ec0f56a 129 if(controller->loop_count >400){
austinbrown124 0:9edd6ec0f56a 130 //controller->i_q_ref = -controller->i_q_ref;
austinbrown124 0:9edd6ec0f56a 131 controller->loop_count = 0;
austinbrown124 0:9edd6ec0f56a 132
austinbrown124 0:9edd6ec0f56a 133 //printf("%d %f\n\r", ind, cogging_current);
austinbrown124 0:9edd6ec0f56a 134 //printf("%f\n\r", controller->theta_elec);
austinbrown124 0:9edd6ec0f56a 135 //pc.printf("%f %f %f\n\r", controller->i_a, controller->i_b, controller->i_c);
austinbrown124 0:9edd6ec0f56a 136 //pc.printf("%f %f\n\r", controller->i_d, controller->i_q);
austinbrown124 0:9edd6ec0f56a 137 //pc.printf("%d %d\n\r", controller->adc1_raw, controller->adc2_raw);
austinbrown124 0:9edd6ec0f56a 138 }
austinbrown124 0:9edd6ec0f56a 139 }
austinbrown124 0:9edd6ec0f56a 140
austinbrown124 0:9edd6ec0f56a 141 /*
austinbrown124 0:9edd6ec0f56a 142 void zero_encoder(ControllerStruct *controller, GPIOStruct *gpio, ){
austinbrown124 0:9edd6ec0f56a 143
austinbrown124 0:9edd6ec0f56a 144 }
austinbrown124 0:9edd6ec0f56a 145 */
austinbrown124 0:9edd6ec0f56a 146
austinbrown124 0:9edd6ec0f56a 147 void voltageabc( float theta, float d, float q, float *a, float *b, float *c){
austinbrown124 0:9edd6ec0f56a 148 ///Phase current amplitude = lengh of dq vector///
austinbrown124 0:9edd6ec0f56a 149 ///i.e. iq = 1, id = 0, peak phase current of 1///
austinbrown124 0:9edd6ec0f56a 150
austinbrown124 0:9edd6ec0f56a 151 *a = sinf(theta);
austinbrown124 0:9edd6ec0f56a 152 *b = sinf((2.0f*PI/3.0f)+theta);
austinbrown124 0:9edd6ec0f56a 153 *c = sinf((-2.0f*PI/3.0f)+theta);
austinbrown124 0:9edd6ec0f56a 154 }
austinbrown124 0:9edd6ec0f56a 155
austinbrown124 0:9edd6ec0f56a 156
austinbrown124 0:9edd6ec0f56a 157
austinbrown124 0:9edd6ec0f56a 158
austinbrown124 0:9edd6ec0f56a 159
austinbrown124 0:9edd6ec0f56a 160
austinbrown124 0:9edd6ec0f56a 161