Austin Brown / Mbed 2 deprecated ESCmk2_Inductance

Inductance Testing Code

Dependencies:   mbed

Fork of CurrentModeSine by Austin Brown

FOC/foc.cpp@1:64b881306f6f, 2018-10-11 (annotated)

Committer:
austinbrown124
Date:
Thu Oct 11 04:13:45 2018 +0000
Revision:
1:64b881306f6f
Parent:
0:9edd6ec0f56a 
DINGBAT

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 //using namespace FastMath;
austinbrown124 0:9edd6ec0f56a 5
austinbrown124 0:9edd6ec0f56a 6 void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w){
austinbrown124 0:9edd6ec0f56a 7 ///u,v,w amplitude = v_bus for full modulation depth///
austinbrown124 0:9edd6ec0f56a 8
austinbrown124 0:9edd6ec0f56a 9 float v_offset = (fminf3(u, v, w) + fmaxf3(u, v, w))/2.0f;
austinbrown124 1:64b881306f6f 10 *dtc_u = fminf(fmaxf(((u - v_offset)*MODULATION_FACTOR/v_bus + ((DTC_MAX+DTC_MIN)/2)), DTC_MIN), DTC_MAX);
austinbrown124 1:64b881306f6f 11 *dtc_v = fminf(fmaxf(((v - v_offset)*MODULATION_FACTOR/v_bus + ((DTC_MAX+DTC_MIN)/2)), DTC_MIN), DTC_MAX);
austinbrown124 1:64b881306f6f 12 *dtc_w = fminf(fmaxf(((w - v_offset)*MODULATION_FACTOR/v_bus + ((DTC_MAX+DTC_MIN)/2)), DTC_MIN), DTC_MAX);
austinbrown124 0:9edd6ec0f56a 13
austinbrown124 0:9edd6ec0f56a 14 }
austinbrown124 0:9edd6ec0f56a 15
austinbrown124 0:9edd6ec0f56a 16
austinbrown124 1:64b881306f6f 17 void reset_foc(FocStruct *controller){
austinbrown124 0:9edd6ec0f56a 18 controller->q_int = 0;
austinbrown124 0:9edd6ec0f56a 19 controller->d_int = 0;
austinbrown124 0:9edd6ec0f56a 20 }
austinbrown124 0:9edd6ec0f56a 21
austinbrown124 0:9edd6ec0f56a 22
austinbrown124 1:64b881306f6f 23 void commutate(FocStruct *controller){
austinbrown124 0:9edd6ec0f56a 24
austinbrown124 1:64b881306f6f 25 controller->loop_count ++;
austinbrown124 1:64b881306f6f 26 // controller->i_a = inverter->i_a;
austinbrown124 1:64b881306f6f 27 // controller->i_b = inverter->i_b;
austinbrown124 1:64b881306f6f 28 // controller->i_c = inverter->i_c;
austinbrown124 1:64b881306f6f 29
austinbrown124 1:64b881306f6f 30
austinbrown124 1:64b881306f6f 31 float s = sinf(controller->theta_elec);
austinbrown124 1:64b881306f6f 32 float c = cosf(controller->theta_elec);
austinbrown124 1:64b881306f6f 33 //float s = FastSin(controller->theta_elec);
austinbrown124 1:64b881306f6f 34 //float c = FastCos(controller->theta_elec);
austinbrown124 1:64b881306f6f 35
austinbrown124 1:64b881306f6f 36 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
austinbrown124 1:64b881306f6f 37 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);
austinbrown124 0:9edd6ec0f56a 38
austinbrown124 1:64b881306f6f 39 float i_d_error = controller->i_d_ref - controller->i_d;
austinbrown124 1:64b881306f6f 40 float i_q_error = controller->i_q_ref - controller->i_q;
austinbrown124 1:64b881306f6f 41
austinbrown124 1:64b881306f6f 42 controller->d_int += i_d_error;
austinbrown124 1:64b881306f6f 43 controller->q_int += i_q_error;
austinbrown124 1:64b881306f6f 44 limit_norm(&controller->d_int, &controller->q_int, V_CLIP/(K_Q*KI_Q));
austinbrown124 1:64b881306f6f 45
austinbrown124 1:64b881306f6f 46 controller->v_d = K_D*i_d_error + K_D*KI_D*controller->d_int;// + v_d_ff;
austinbrown124 1:64b881306f6f 47 controller->v_q = K_Q*i_q_error + K_Q*KI_Q*controller->q_int;// + v_q_ff;
austinbrown124 1:64b881306f6f 48
austinbrown124 1:64b881306f6f 49 limit_norm(&controller->v_d, &controller->v_q, V_CLIP);
austinbrown124 1:64b881306f6f 50
austinbrown124 1:64b881306f6f 51 if (USE_THETA_ADV) {
austinbrown124 1:64b881306f6f 52 s = sinf(controller->theta_elec_adv);
austinbrown124 1:64b881306f6f 53 c = cosf(controller->theta_elec_adv);
austinbrown124 1:64b881306f6f 54 }
austinbrown124 1:64b881306f6f 55
austinbrown124 1:64b881306f6f 56 //s = FastSin(controller->theta_elec_adv);
austinbrown124 1:64b881306f6f 57 //c = FastCos(controller->theta_elec_adv);
austinbrown124 1:64b881306f6f 58
austinbrown124 1:64b881306f6f 59 controller->v_u = c*controller->v_d - s*controller->v_q; // Faster Inverse DQ0 transform
austinbrown124 1:64b881306f6f 60 controller->v_v = ( 0.86602540378f*s-.5f*c)*controller->v_d - (-0.86602540378f*c-.5f*s)*controller->v_q;
austinbrown124 1:64b881306f6f 61 controller->v_w = (-0.86602540378f*s-.5f*c)*controller->v_d - ( 0.86602540378f*c-.5f*s)*controller->v_q;
austinbrown124 1:64b881306f6f 62
austinbrown124 1:64b881306f6f 63 svm(V_BUS, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation
austinbrown124 1:64b881306f6f 64
austinbrown124 1:64b881306f6f 65 //controller->dtc_u = 0.2;
austinbrown124 1:64b881306f6f 66 //controller->dtc_v = 0.2;
austinbrown124 1:64b881306f6f 67 //controller->dtc_w = 0.8;
austinbrown124 1:64b881306f6f 68
austinbrown124 1:64b881306f6f 69 // focc->dtc_u = controller->dtc_u;
austinbrown124 1:64b881306f6f 70 // inverter->dtc_v = controller->dtc_v;
austinbrown124 1:64b881306f6f 71 // inverter->dtc_w = controller->dtc_w;
austinbrown124 1:64b881306f6f 72
austinbrown124 1:64b881306f6f 73
austinbrown124 1:64b881306f6f 74 if(controller->loop_count >10000){
austinbrown124 1:64b881306f6f 75 controller->loop_count = 0;
austinbrown124 1:64b881306f6f 76 }
austinbrown124 1:64b881306f6f 77 }
austinbrown124 1:64b881306f6f 78
austinbrown124 1:64b881306f6f 79
austinbrown124 1:64b881306f6f 80
austinbrown124 0:9edd6ec0f56a 81
austinbrown124 1:64b881306f6f 82 void voltage_mode_commutate(FocStruct *controller){
austinbrown124 1:64b881306f6f 83
austinbrown124 1:64b881306f6f 84 controller->loop_count ++;
austinbrown124 1:64b881306f6f 85
austinbrown124 1:64b881306f6f 86 float s = sinf(controller->theta_elec);
austinbrown124 1:64b881306f6f 87 float c = cosf(controller->theta_elec);
austinbrown124 1:64b881306f6f 88
austinbrown124 1:64b881306f6f 89 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
austinbrown124 1:64b881306f6f 90 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);
austinbrown124 0:9edd6ec0f56a 91
austinbrown124 1:64b881306f6f 92
austinbrown124 1:64b881306f6f 93 limit_norm(&controller->v_d, &controller->v_q, V_CLIP);
austinbrown124 1:64b881306f6f 94
austinbrown124 1:64b881306f6f 95 if (USE_THETA_ADV) {
austinbrown124 1:64b881306f6f 96 s = sinf(controller->theta_elec_adv);
austinbrown124 1:64b881306f6f 97 c = cosf(controller->theta_elec_adv);
austinbrown124 1:64b881306f6f 98 }
austinbrown124 1:64b881306f6f 99
austinbrown124 1:64b881306f6f 100 controller->v_u = c*controller->v_d - s*controller->v_q; // Faster Inverse DQ0 transform
austinbrown124 1:64b881306f6f 101 controller->v_v = ( 0.86602540378f*s-.5f*c)*controller->v_d - (-0.86602540378f*c-.5f*s)*controller->v_q;
austinbrown124 1:64b881306f6f 102 controller->v_w = (-0.86602540378f*s-.5f*c)*controller->v_d - ( 0.86602540378f*c-.5f*s)*controller->v_q;
austinbrown124 1:64b881306f6f 103
austinbrown124 1:64b881306f6f 104 svm(V_BUS, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation
austinbrown124 1:64b881306f6f 105
austinbrown124 1:64b881306f6f 106
austinbrown124 1:64b881306f6f 107 if(controller->loop_count >10000){
austinbrown124 1:64b881306f6f 108 controller->loop_count = 0;
austinbrown124 1:64b881306f6f 109 }
austinbrown124 1:64b881306f6f 110 }
austinbrown124 0:9edd6ec0f56a 111
austinbrown124 0:9edd6ec0f56a 112
austinbrown124 0:9edd6ec0f56a 113
austinbrown124 0:9edd6ec0f56a 114
austinbrown124 0:9edd6ec0f56a 115
austinbrown124 0:9edd6ec0f56a 116
austinbrown124 0:9edd6ec0f56a 117
austinbrown124 0:9edd6ec0f56a 118
austinbrown124 0:9edd6ec0f56a 119
austinbrown124 0:9edd6ec0f56a 120
austinbrown124 0:9edd6ec0f56a 121