Diff: main.cpp

Revision:

2:b3aed371adb5

Parent:

1:f39d75a48955

Child:

4:3d8dd3d17564

--- a/main.cpp	Thu Mar 07 07:04:11 2019 +0000
+++ b/main.cpp	Thu Mar 07 09:16:11 2019 +0000
@@ -99,7 +99,7 @@
 //******************************************************************************
 int main()
 {
-    pc.baud(2000000);   // for serial comm.
+    pc.baud(115200);   // for serial comm.
     counter1.reset();   // encoder reset
     diff.reset(0.0f,0);  
     ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...;
@@ -159,7 +159,12 @@
     default: break;
     }
     out.write( i2u(torq/0.217f)); // motor const. is 0.217, 
-                                        // transform M -> i -> u -> normalized output
+    if(++k >= 249){
+        k = 0;
+        //pc.printf("phi: %1.5f, Torq: %1.2f  \r\n",phi1,torq);
+        pc.printf("ax %1.5f, ay: %1.5f, gyro: %1.5f, ph1: %1.5f\r\n",u2ax(ax.read()),u2ay(ay.read()),u2gz(gz.read()),phi1);
+        }
+
 } // END OF updateLoop(void)
  
 //******************************************************************************
@@ -187,10 +192,6 @@
     float M_des = om2zero(0.0f-vel);    // outer controller to bring motor to zero
     float torq = M_des    +  (-7.1*phi1_des) -(-9.6910f * phi1 -0.7119f * gyro);  // calculationof gains are based on the MATLAB script at the end of this file!!!
     //          output PI       V               K(1)              K(2)
-        if(++k >= 249){
-            k = 0;
-            pc.printf("phi: %1.5f, Torq: %1.2f, M_soll %1.2f  \r\n",phi1,torq,M_des);
-            }
     return torq;
 }
 // ************ calculate angle *****************