Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 22:72840d3db788
- Parent:
- 21:b7fb355c8c2d
- Child:
- 23:c77d4b42de17
--- a/main.cpp Sat Nov 05 09:12:10 2016 +0000 +++ b/main.cpp Sat Nov 05 10:50:25 2016 +0000 @@ -23,6 +23,7 @@ int adval1, adval2; float ia, ib, ic, alpha, beta, d, q, vd, vq, p; float p_mech, last_p_mech, w; +float d_filtered = 0.0f, q_filtered = 0.0f; float ia_supp_offset = 0.0f, ib_supp_offset = 0.0f; //current sensor offset due to bias resistor inaccuracies, etc (mV) @@ -83,7 +84,7 @@ //fill in d, q ref based on torque cmd and current velocity void get_dq(float torque, float w, float *d, float *q) { *d = 0.0f; - *q = torque / KT < Q_MAX ? torque / KT : Q_MAX; + *q = Q_MAX;//torque / KT < Q_MAX ? torque / KT : Q_MAX; } void commutate() { @@ -114,8 +115,11 @@ d = alpha * cos_p - beta * sin_p; q = -alpha * sin_p - beta * cos_p; - float d_err = d_ref - d; - float q_err = q_ref - q; + d_filtered = DQ_FILTER_STRENGTH * d_filtered + (1.0f - DQ_FILTER_STRENGTH) * d; + q_filtered = DQ_FILTER_STRENGTH * q_filtered + (1.0f - DQ_FILTER_STRENGTH) * q; + + float d_err = d_ref - d_filtered; + float q_err = q_ref - q_filtered; d_integral += d_err * KI; q_integral += q_err * KI; @@ -209,7 +213,7 @@ TIM1->EGR |= TIM_EGR_UG; TIM1->PSC = 0x00; //no prescaler, timer counts up in sync with the peripheral clock - TIM1->ARR = 0x4650; //5 Khz + TIM1->ARR = (int) ((float) 9e7 / F_SW); TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on. TIM1->CR1 |= TIM_CR1_CEN;