Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 24:5e18a87a0e95
- Parent:
- 23:c77d4b42de17
- Child:
- 25:3f2b585ae72d
--- a/main.cpp Sat Nov 05 11:10:37 2016 +0000 +++ b/main.cpp Sat Nov 05 14:02:22 2016 +0000 @@ -31,7 +31,7 @@ float last_d = 0.0f, last_q = 0.0f; float d_ref = 0.0f, q_ref = 0.0f; -bool control_enabled = false; +bool control_enabled = true; void commutate(); void zero_current(); @@ -71,9 +71,10 @@ p_mech = pos.GetMechPosition(); float dp_mech = p_mech - last_p_mech; if (dp_mech < -PI) dp_mech += 2 * PI; - if (dp_mech > PI) dp_mech -= 2 * PI; - float loop_period = (float) (TIM1->ARR) / 90.0f; - float w_raw = dp_mech * (float) 1e6 / loop_period; //rad/s + if (dp_mech > PI) dp_mech -= 2 * PI; + float w_raw = dp_mech * F_SW; //rad/s + if (w_raw > W_MAX) w_raw = w; //with this limiting scheme noise < 0 + if (w_raw < -W_MAX) w_raw = w; //so we need to throw out the large deltas first w = W_FILTER_STRENGTH * w + (1.0f - W_FILTER_STRENGTH) * w_raw; } @@ -88,13 +89,12 @@ } void commutate() { - if(control_enabled && !throttle_in.get_enabled()) go_disabled(); - if(!control_enabled && throttle_in.get_enabled()) go_enabled(); + //if(control_enabled && !throttle_in.get_enabled()) go_disabled(); + //if(!control_enabled && throttle_in.get_enabled()) go_enabled(); update_velocity(); p = pos.GetElecPosition() - POS_OFFSET; - if (p < 0) p += 2 * PI; float torque = get_torque_cmd(throttle_in.get_throttle(), w); get_dq(torque, w, &d_ref, &q_ref); @@ -112,8 +112,8 @@ alpha = u; beta = 1 / sqrtf(3.0f) * u + 2 / sqrtf(3.0f) * v; - d = alpha * cos_p - beta * sin_p; - q = -alpha * sin_p - beta * cos_p; + d = alpha * cos_p + beta * sin_p; + q = -alpha * sin_p + beta * cos_p; d_filtered = DQ_FILTER_STRENGTH * d_filtered + (1.0f - DQ_FILTER_STRENGTH) * d; q_filtered = DQ_FILTER_STRENGTH * q_filtered + (1.0f - DQ_FILTER_STRENGTH) * q; @@ -142,8 +142,8 @@ float vbeta = vd * sin_p + vq * cos_p; float va = valpha; - float vb = -0.5f * valpha - sqrtf(3) / 2.0f * vbeta; - float vc = -0.5f * valpha + sqrtf(3) / 2.0f * vbeta; + float vb = -0.5f * valpha + sqrtf(3) / 2.0f * vbeta; + float vc = -0.5f * valpha - sqrtf(3) / 2.0f * vbeta; float voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f; va = va - voff;