Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 44:3fd6a43b91f0
- Parent:
- 42:030e0ec4eac5
- Child:
- 45:cf8ad81fb0f0
--- a/main.cpp Fri Jan 06 09:10:45 2017 +0000 +++ b/main.cpp Sun Jan 08 09:03:15 2017 +0000 @@ -28,18 +28,22 @@ DQMapper *dq; ThrottleMapper *th; +int loop_counter = 0; bool control_enabled = false; void update_velocity() { read.last_p_mech = read.p_mech; read.p_mech = io.pos->GetMechPosition(); + float dp_mech = read.p_mech - read.last_p_mech; if (dp_mech < -PI) dp_mech += 2 * PI; 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 - read.w = update_pid(read.w, w_raw, W_FILTER_STRENGTH); + + read.w = update_filter(read.w, w_raw, W_FILTER_STRENGTH); } void commutate() { @@ -50,8 +54,11 @@ /*update velocity, references*/ update_velocity(); - float torque_percent = th->map(io.throttle_in->get_throttle(), read.w); - dq->map(torque_percent, read.w, &control.d_ref, &control.q_ref); + if (loop_counter % SLOW_LOOP_COUNTER == 0) { + loop_counter = 0; + slow_loop(); + } + loop_counter++; /*update position, sin, cos*/ foc.p = io.pos->GetElecPosition() - POS_OFFSET; @@ -67,8 +74,8 @@ park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q); /*PI controller*/ - control.d_filtered = update_pid(control.d_filtered, foc.d, DQ_FILTER_STRENGTH); - control.q_filtered = update_pid(control.q_filtered, foc.q, DQ_FILTER_STRENGTH); + 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); float d_err = control.d_ref - control.d_filtered; float q_err = control.q_ref - control.q_filtered; @@ -99,7 +106,7 @@ vc = -va - vb; /*SVPWM*/ - voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f; + voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it va = va - voff; vb = vb - voff; vc = vc - voff; @@ -110,6 +117,11 @@ set_dtc(io.c, 0.5f + 0.5f * vc); } +void slow_loop() { + float torque_percent = th->map(io.throttle_in->get_throttle(), read.w); + dq->map(torque_percent, read.w, &control.d_ref, &control.q_ref); +} + void go_enabled() { control.d_integral = 0.0f; control.q_integral = 0.0f; @@ -122,7 +134,7 @@ io.en->write(0); } -float update_pid(float old, float x, float str) { +float update_filter(float old, float x, float str) { return str * old + (1.0f - str) * x; }