Diff: main.cpp

Revision:

8:72f260c467ad

Parent:

7:01a7363583b2

Child:

9:67ee46be0403

--- a/main.cpp	Sun Apr 22 19:54:59 2018 +0000
+++ b/main.cpp	Tue Apr 24 09:13:59 2018 +0000
@@ -53,6 +53,14 @@
 //------------------------------------------
 //LinearCharacteristics i2u(0.8f,-2.0f);
 LinearCharacteristics i2u(-2.0f,2.0f,0.0f,3.2f);
+LinearCharacteristics ax2ms2(0.2972f,0.7028f,-9.81,9.81);
+LinearCharacteristics ay2ms2(0.29866f,0.70134f,-9.81,9.81);
+LinearCharacteristics gz2om(4.6517f,1.4852f);
+//-------------DEFINE FILTERS----------------
+float tau = 1.0f;
+IIR_filter f_ax(tau,Ts,1.0f);        // filter ax signals
+IIR_filter f_ay(tau,Ts,1.0f);        // filter ay signals
+IIR_filter f_gz(tau,Ts,tau);        // filter gz signals
 
 //------------------------------------------
 Ticker  ControllerLoopTimer;            // interrupt for control loop
@@ -96,9 +104,13 @@
     out.write(i2u(torq_des/0.217f));     // the controller! convert torque to Amps km = 0.217 Nm/A
     */
     
+    //float phi1 = atan2(-f_ax(ax2ms2(ax.read())),f_ay(ax2ms2(ay.read()))) + PI/4.0f + f_gz(gz2om(gz.read()*3.3f));
+    float fax = f_ax(ax2ms2(ax.read()));
+    float fay = f_ay(ay2ms2(ay.read()));
     if(++k >= 249){
         k = 0;
-        pc.printf("Some Output  %3.3f\r\n",1.11111);
+        //pc.printf("Phi1: %1.5f \r\n",phi1);
+        pc.printf("f_ax: %1.5f f_ay: %1.5f  \r\n",fax,fay);
     }
 }
 //******************************************************************************