Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 20:91ae97a811e3
- Parent:
- 19:a6cf15f89f3d
- Child:
- 21:b7fb355c8c2d
diff -r a6cf15f89f3d -r 91ae97a811e3 main.cpp --- a/main.cpp Wed Nov 02 12:52:00 2016 +0000 +++ b/main.cpp Sat Nov 05 07:52:36 2016 +0000 @@ -101,9 +101,6 @@ float sin_p = sinf(p); float cos_p = cosf(p); - //float pos_dac = 0.85f * p / (2 * PI) + 0.05f; //uncomment me to write position to the DAC - //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; @@ -123,29 +120,11 @@ 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; + q_integral = constrain(q_integral, -INTEGRAL_MAX, INTEGRAL_MAX); + d_integral = constrain(d_integral, -INTEGRAL_MAX, INTEGRAL_MAX); - if(control_enabled) { - vd = KP * d_err + d_integral; - vq = KP * q_err + q_integral; - } else { - vd = 0; - vq = 0; - } - - 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); //uncomment me to write I_q to the DAC - //DAC->DHR12R2 = (unsigned int) (-vd * 2000 + 2048); - - //vd = 0.0f; //uncomment me for voltage mode testing - //vq = 1.0f; + vd = constrain(vd, -1.0f, 1.0f); + vq = constrain(vq, -1.0f, 1.0f); float valpha = vd * cos_p - vq * sin_p; float vbeta = vd * sin_p + vq * cos_p; @@ -169,8 +148,6 @@ startup_msg(); for (;;) { - //pc.printf("%f\n\r", p); - //wait(0.1); } }