Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 152:6877dceec871
- Parent:
- 150:08c13bfc7417
- Child:
- 153:8a1f9888f003
- Child:
- 154:0a22dcf91577
--- a/main.cpp Thu May 04 14:34:34 2017 +0000 +++ b/main.cpp Thu May 04 15:16:56 2017 +0000 @@ -34,7 +34,6 @@ int loop_counter = 0; bool control_enabled = false; -float throttle_median, w_median; void update_velocity() { read.last_p_mech = read.p_mech; @@ -45,11 +44,8 @@ if (dp_mech > PI) dp_mech -= 2 * PI; float w_raw = dp_mech * F_SW; //rad/s - if (w_raw > W_CRAZY) w_raw = read.w; //throw out inplausible results - if (w_raw < -W_CRAZY) w_raw = read.w; - w_median = velocity_filter->update(w_raw); - read.w = update_filter(read.w, w_raw, W_FILTER_STRENGTH); + read.w = velocity_filter->update(w_raw); } void commutate() { @@ -138,9 +134,8 @@ void slow_loop() { float x = io.throttle_in->get_throttle(); - throttle_median = throttle_filter->update(x); - control.torque_percent = update_filter(control.torque_percent, th->map(io.throttle_in->get_throttle(), read.w), THROTTLE_FILTER_STRENGTH); - + x = throttle_filter->update(x); + control.torque_percent = th->map(x, read.w); dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref); } @@ -167,7 +162,6 @@ 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\n", (int)(255 * control.torque_percent), (int)(255 * throttle_median)); //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); } @@ -193,8 +187,8 @@ int main() { dq = new LutMapper(); th = new NullThrottleMapper(); - throttle_filter = new MedianFilter(3); - velocity_filter = new MedianFilter(3); + throttle_filter = new MedianFilter(THROTTLE_FILTER_WINDOW); + velocity_filter = new MedianFilter(W_FILTER_WINDOW); BREMSInit(&io, &read, &foc, &control, false);