Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 53:b7f824360fe3
- Parent:
- 52:fd3d8df99287
- Child:
- 54:25db122c05f0
--- a/main.cpp Wed Jan 25 02:08:10 2017 +0000 +++ b/main.cpp Wed Jan 25 02:17:28 2017 +0000 @@ -49,6 +49,7 @@ void commutate() { /*safety checks*/ if (!io.throttle_in->get_enabled() || !io.pos->IsValid() || !is_driving()) { + /*do this even in disabled state, to keep integral down*/ go_disabled(); } else if (!control_enabled) { go_enabled(); @@ -128,8 +129,6 @@ } void go_enabled() { - control.d_integral = 0.0f; - control.q_integral = 0.0f; control_enabled = true; io.en->write(1); } @@ -142,7 +141,7 @@ } bool is_driving() { - return io.throttle_in->get_throttle() > 0.01f || read.w > 1.0f; + return io.throttle_in->get_throttle() > 0.01f || fabsf(read.w) > 1.0f; } float update_filter(float old, float x, float str) {