Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 52:fd3d8df99287
- Parent:
- 51:8b817927d6e4
- Child:
- 53:b7f824360fe3
diff -r 8b817927d6e4 -r fd3d8df99287 main.cpp --- a/main.cpp Sun Jan 22 05:23:01 2017 +0000 +++ b/main.cpp Wed Jan 25 02:08:10 2017 +0000 @@ -48,9 +48,11 @@ void commutate() { /*safety checks*/ - if (control_enabled && !io.throttle_in->get_enabled()) go_disabled(); - if (control_enabled && !io.pos->IsValid()) go_disabled(); - if (!control_enabled && io.throttle_in->get_enabled()) go_enabled(); + if (!io.throttle_in->get_enabled() || !io.pos->IsValid() || !is_driving()) { + go_disabled(); + } else if (!control_enabled) { + go_enabled(); + } /*update velocity, references*/ update_velocity(); @@ -74,6 +76,9 @@ park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q); /*PI controller*/ + control.d_integral *= 1.0f - INTEGRAL_DECAY; + control.q_integral *= 1.0f - INTEGRAL_DECAY; + control.d_filtered = update_filter(control.d_filtered, foc.d, DQ_FILTER_STRENGTH); control.q_filtered = update_filter(control.q_filtered, foc.q, DQ_FILTER_STRENGTH); @@ -130,10 +135,16 @@ } void go_disabled() { + control.d_integral = 0.0f; + control.q_integral = 0.0f; control_enabled = false; io.en->write(0); } +bool is_driving() { + return io.throttle_in->get_throttle() > 0.01f || read.w > 1.0f; +} + float update_filter(float old, float x, float str) { return str * old + (1.0f - str) * x; } @@ -156,5 +167,7 @@ BREMSInit(&io, &read, &foc, &control, false); for (;;) { + io.pc->printf("%f %f || %f %f || %f %d\n\r", control.d_integral, control.d_filtered, control.q_integral, control.q_filtered, read.w, is_driving()); + wait(0.2); } } \ No newline at end of file