Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 153:8a1f9888f003
- Parent:
- 152:6877dceec871
--- a/main.cpp Thu May 04 15:16:56 2017 +0000 +++ b/main.cpp Thu May 04 16:14:29 2017 +0000 @@ -31,6 +31,7 @@ DQMapper *dq; ThrottleMapper *th; MedianFilter *throttle_filter, *velocity_filter; +MovingAverageFilter *d_filter, *q_filter; int loop_counter = 0; bool control_enabled = false; @@ -76,8 +77,8 @@ park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q); /*PI controller*/ - 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); + control.d_filtered = d_filter->update(foc.d); + control.q_filtered = q_filter->update(foc.q); float d_err = control.d_ref - control.d_filtered; float q_err = control.q_ref - control.q_filtered; @@ -160,8 +161,8 @@ } void log() { - //io.pc->printf("%d,%d,%d,%d,%d,%d,%d,%d\n", (int) read.w, (int) control.d_ref, (int) control.d_filtered, (int) control.q_ref, (int) control.q_filtered, (int) (255 * control.torque_percent), - // (int) (255 * foc.vd), (int) (255 * foc.vq)); + io.pc->printf("%d,%d,%d,%d,%d,%d,%d,%d\n", (int) read.w, (int) control.d_ref, (int) control.d_filtered, (int) control.q_ref, (int) control.q_filtered, (int) (255 * control.torque_percent), + (int) (255 * foc.vd), (int) (255 * foc.vq)); //io.pc->printf("%d,%d,%d,%d,%d\n", (int) read.w, (int) control.d_filtered, (int) control.q_filtered, (int) (255 * foc.vd), (int) (255 * foc.vq)); wait(1.0f / LOG_FREQUENCY); } @@ -187,8 +188,11 @@ int main() { dq = new LutMapper(); th = new NullThrottleMapper(); + throttle_filter = new MedianFilter(THROTTLE_FILTER_WINDOW); velocity_filter = new MedianFilter(W_FILTER_WINDOW); + d_filter = new MovingAverageFilter(DQ_FILTER_WINDOW); + q_filter = new MovingAverageFilter(DQ_FILTER_WINDOW); BREMSInit(&io, &read, &foc, &control, false);