N K
/
foc-ed_in_the_bot_compact
last working
Fork of foc-ed_in_the_bot_compact by
Diff: main.cpp
- Revision:
- 2:eabe8feaaabb
- Parent:
- 1:7b61790f6be9
- Child:
- 3:9b20da3f0055
--- a/main.cpp Wed Mar 09 17:21:01 2016 +0000 +++ b/main.cpp Fri Mar 18 10:07:35 2016 +0000 @@ -11,18 +11,23 @@ DigitalOut en(EN); DigitalOut toggle(PC_10); -AnalogIn Ia(IA); -AnalogIn Ib(IB); - PositionSensorEncoder pos(CPR, 0); Serial pc(USBTX, USBRX); int state = 0; int adval1, adval2; -float ia, ib; +float ia, ib, ic, alpha, beta, d, q, vd, vq, p; + float ia_supp_offset = 0.0f, ib_supp_offset = 0.0f; //current sensor offset due to bias resistor inaccuracies, etc (mV) +float d_integral = 0.0f, q_integral = 0.0f; +float last_d = 0.0f, last_q = 0.0f; +float d_ref = 0.0f, q_ref = -50.0f; + +float d_filtered = 0.0f; +float q_filtered = 0.0f; + extern "C" void TIM1_UP_TIM10_IRQHandler(void) { if (TIM1->SR & TIM_SR_UIF ) { adval1 = ADC1->DR; @@ -30,8 +35,6 @@ ADC1->CR2 |= 0x40000000; } TIM1->SR = 0x00; - toggle = state; - state = !state; } void zero_current(){ @@ -132,18 +135,74 @@ } void main_loop() { - float p = pos.GetElecPosition() - POS_OFFSET + PI / 2; + //float p = pos.GetElecPosition() - POS_OFFSET + PI / 2; + p = pos.GetElecPosition() - POS_OFFSET; if (p < 0) p += 2 * PI; - float pos_dac = 0.85f * p / (2 * PI) + 0.05f; - DAC->DHR12R2 = (unsigned int) (pos_dac * 4096); + float sin_p = sinf(p); + float cos_p = cosf(p); + + //float pos_dac = 0.85f * p / (2 * PI) + 0.05f; + //DAC->DHR12R2 = (unsigned int) (pos_dac * 4096); ia = ((float) adval1 / 4096.0f * AVDD - I_OFFSET - ia_supp_offset) / I_SCALE; ib = ((float) adval2 / 4096.0f * AVDD - I_OFFSET - ib_supp_offset) / I_SCALE; + ic = -ia - ib; - set_dtc(a, 0.5f + 0.5f * cosf(p)); - set_dtc(b, 0.5f + 0.5f * cosf(p + 2 * PI / 3)); - set_dtc(c, 0.5f + 0.5f * cosf(p - 2 * PI / 3)); + float u = ib;//ic; + float v = ic;//ia; + + alpha = u; + beta = 1 / sqrtf(3.0f) * u + 2 / sqrtf(3.0f) * v; + + d = alpha * cos_p - beta * sin_p; + q = -alpha * sin_p - beta * cos_p; + + d_filtered = 0.9f * d_filtered + 0.1f * d; + q_filtered = 0.9f * q_filtered + 0.1f * 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; + + if (q_integral > INTEGRAL_MAX) q_integral = INTEGRAL_MAX; + if (d_integral > INTEGRAL_MAX) d_integral = INTEGRAL_MAX; + if (q_integral < -INTEGRAL_MAX) q_integral = -INTEGRAL_MAX; + if (d_integral < -INTEGRAL_MAX) d_integral = -INTEGRAL_MAX; + + vd = KP * d_err + d_integral; + vq = KP * q_err + q_integral; + + if (vd < -1.0f) vd = -1.0f; + if (vd > 1.0f) vd = 1.0f; + if (vq < -1.0f) vq = -1.0f; + if (vq > 1.0f) vq = 1.0f; + + DAC->DHR12R2 = (unsigned int) (-q * 20 + 2048); + //DAC->DHR12R2 = (unsigned int) (-vd * 2000 + 2048); + + //vd = 0.0f; + //vq = -1.0f; + + //float phase = asinf(vd / sqrtf(vq * vq + vd * vd)); + //DAC->DHR12R2 = (unsigned int) (phase / (PI / 2.0f) * 2000 + 2048); + + float valpha = vd * cos_p - vq * sin_p; + float vbeta = vd * sin_p + vq * cos_p; + + float va = valpha; + float vb = -0.5f * valpha - sqrtf(3) / 2.0f * vbeta; + float vc = -0.5f * valpha + sqrtf(3) / 2.0f * vbeta; + + set_dtc(a, 0.5f + 0.5f * va); + set_dtc(b, 0.5f + 0.5f * vb); + set_dtc(c, 0.5f + 0.5f * vc); + + //set_dtc(a, 0.5f + 0.5f * cosf(p)); + //set_dtc(b, 0.5f + 0.5f * cosf(p + 2 * PI / 3)); + //set_dtc(c, 0.5f + 0.5f * cosf(p - 2 * PI / 3)); } int main() { @@ -153,6 +212,33 @@ Ticker loop; loop.attach_us(main_loop, 200); + //vd = 0.0f; + //vq = 1.0f; + for (;;) { + + printf("%f\n\r", vd); + wait_ms(100); + + /* + q_ref = 0.0f; + wait(3); + toggle = state; + state = !state; + q_ref = -50.0f; + wait(3); + */ + + /* + vq = 0.0f; + wait(3); + toggle = state; + state = !state; + vq = -1.0f; + wait(3); + */ + + //toggle = state; + //state = !state; } }