Cuboid
Dependencies: mbed
Diff: main.cpp
- Revision:
- 1:2e118d67eeae
- Parent:
- 0:15be70d21d7c
- Child:
- 2:252a61a7e8f9
--- a/main.cpp Wed Jan 10 16:08:07 2018 +0000 +++ b/main.cpp Wed Jan 10 16:35:44 2018 +0000 @@ -103,10 +103,10 @@ float wz = u2gz(3.3f*gz.read()); // cuboid rot-velocity float ang = atan2(-f_ax(u2ax(3.3f*ax.read())),f_ay(u2ay(3.3f*ay.read())))+f_gz(wz)+PI/4.0f; // compl. filter // K matrix: -5.2142 -0.6247 // from Matlab - float desTorque = pi_w2zero(n_soll-vel)-(-5.2142f*ang-0.6247f*wz); // state space controller for balance + float desTorque = pi_w2zero(n_soll-vel)-(-5.2142f*ang-0.6247f*wz); // state space controller for balance, calc desired Torque out.write(u3k3_TO_1V(i2u(desTorque/0.217f))); // convert Nm -> A and write to AOUT //out.write(u3k3_TO_1V(i2u(pi_w(n_soll-vel)))); // test speed controller - if(++k >= 99){ + if(++k >= 199){ k = 0; pc.printf("phi=%3.2f omega=%3.2f \r\n",ang*180.0f/PI,vel); }