Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 121:de10418bf2c2
- Parent:
- 120:57b6f3b1356b
- Child:
- 122:53be0630f79d
diff -r 57b6f3b1356b -r de10418bf2c2 main.cpp --- a/main.cpp Tue Apr 25 07:03:08 2017 +0000 +++ b/main.cpp Tue Apr 25 07:21:42 2017 +0000 @@ -43,27 +43,12 @@ 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; //with this limiting scheme noise < 0 - if (w_raw < -W_CRAZY) w_raw = read.w; //so we need to throw out the large deltas first + if (w_raw > W_CRAZY) w_raw = read.w; //throw out inplausible results + if (w_raw < -W_CRAZY) w_raw = read.w; read.w = update_filter(read.w, w_raw, W_FILTER_STRENGTH); } -void update_velocity2() { - read.last_p_mech2 = read.p_mech2; - read.p_mech2 = io.pos->GetUnlimitedMechPosition(); - - float dp_mech2 = read.p_mech2 - read.last_p_mech2; - if (dp_mech2 < -PI) dp_mech2 += 2 * PI; - if (dp_mech2 > PI) dp_mech2 -= 2 * PI; - - float w_raw2 = dp_mech2 * F_SW; //rad/s - if (w_raw2 > W_CRAZY) w_raw2 = read.w2; //with this limiting scheme noise < 0 - if (w_raw2 < -W_CRAZY) w_raw2 = read.w2; //so we need to throw out the large deltas first - - read.w2 = update_filter(read.w2, w_raw2, W_FILTER_STRENGTH); -} - void commutate() { /*safety checks, do we do anything this cycle?*/ if (!control_enabled && io.throttle_in->get_enabled() && io.pos->IsValid() && is_driving()) { @@ -72,7 +57,6 @@ /*update velocity, references*/ update_velocity(); - update_velocity2(); if (loop_counter % SLOW_LOOP_COUNTER == 0) { loop_counter = 0; slow_loop(); @@ -168,10 +152,9 @@ } 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\n", io.pos->IsValid()); - io.pc->printf("%f,%f\n", read.w, read.w2); wait(1.0f / LOG_FREQUENCY); } @@ -200,13 +183,6 @@ th = new NullThrottleMapper(); BREMSInit(&io, &read, &foc, &control, false); - /* - for (;;) { - float x = io.throttle_in->get_throttle(); - if (x > 0.5f) calibrate_position(&io); - } - */ - for (;;) { if (ENABLE_LOGGING) { log();