Austin Brown
/
ESCmk2_Inductance
Inductance Testing Code
Fork of CurrentModeSine by
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?
User | Revision | Line number | New 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 |