Diff: main.cpp

Revision:

1:2e118d67eeae

Parent:

0:15be70d21d7c

Child:

2:38d9d1c3c85f

--- 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);
     }