Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 21:b7fb355c8c2d
- Parent:
- 20:91ae97a811e3
- Child:
- 22:72840d3db788
--- a/main.cpp Sat Nov 05 07:52:36 2016 +0000 +++ b/main.cpp Sat Nov 05 09:12:10 2016 +0000 @@ -123,6 +123,14 @@ 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; + } + vd = constrain(vd, -1.0f, 1.0f); vq = constrain(vq, -1.0f, 1.0f);