Diff: main.cpp

Revision:

10:ddbc7e4c41a0

Parent:

9:dc0eb7dd0d92

Child:

11:a0074351e154

diff -r dc0eb7dd0d92 -r ddbc7e4c41a0 main.cpp
--- a/main.cpp	Tue Apr 17 13:30:52 2018 +0000
+++ b/main.cpp	Tue Apr 17 15:56:11 2018 +0000
@@ -53,6 +53,11 @@
 //------------------------------------------
 //LinearCharacteristics i2u(0.8f,-2.0f);
 LinearCharacteristics i2u(-2.0f,2.0f,0.0f,3.2f);
+LinearCharacteristics u2ax(0.30265f,0.69835f,9.81f,-9.81f);
+LinearCharacteristics u2ay(0.29875f,0.69665f,9.81f,-9.81f);
+LinearCharacteristics u2gyro(-4.65f,1.5f);
+
+
 
 //------------------------------------------
 Ticker  ControllerLoopTimer;            // interrupt for control loop
@@ -62,6 +67,13 @@
 // ... here define instantiate classes
 //------------------------------------------
 PI_Cntrl vel_cntrl(0.5f,.05f,Ts,0.4f);
+
+float tau = 1.0f;
+IIR_filter LPF_gyro(tau,Ts,tau);
+IIR_filter LPF_accX(tau,Ts,1.0f);
+IIR_filter LPF_accY(tau,Ts,1.0f);
+
+
 //GPA gpa1(1.0f, 200.0f,      150,       4,      400, Ts, 10.0f, 0.3f);
 //float excWobble = 0.0f;
 // GPA(t fMin, t fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1)
@@ -94,13 +106,28 @@
      /*       outWobble = omega;
             excWobble = Wobble(excWobble, outWobble); */
     
-    float torq_des = vel_cntrl(w_soll - vel);
-    out.write(i2u(torq_des/0.217f));     // the controller! convert torque to Amps km = 0.217 Nm/A
+    //float torq_des = vel_cntrl(w_soll - vel);
+    //out.write(i2u(torq_des/0.217f));     // the controller! convert torque to Amps km = 0.217 Nm/A
+    
+    float gyro_dir = (u2gyro(gz.read()*3.3f));
+    float gyroV = LPF_gyro.filter(gyro_dir);
+    float accX = LPF_accX.filter(u2ax(ax.read()));
+    float accY = LPF_accY.filter(u2ay(ay.read()));
+    
+    float phi_mes = (PI/4) + atan2(-accX, accY) + gyroV;
+    
+    float torque = -(-3.7f*phi_mes - 0.42f * gyro_dir);
+    out.write(i2u(torque/0.217f)/3.3f);
     
     
-    if(++k >= 249){
+    // OUTPUT
+    if(++k >= 150){
         k = 0;
-        pc.printf("Des. velocity: %3.3f, Velocity: %3.3f\r\n",w_soll,vel);
+        //pc.printf("Des. velocity: %3.3f, Velocity: %3.3f\r\n",w_soll,vel);
+        pc.printf("Uax val %3.5f; ax val %3.5f; Uay val %3.5f; ay val %3.5f; gyro %3.5f\r\n",ax.read(),u2ax(ax.read()),ay.read(),u2ay(ay.read()), gz.read());
+        //pc.printf("Gyro: %3.3f rad/s\r\n",gyroCalc(gz.read()));
+        //pc.printf("Gyro filt: %3.3f, Gyro roh; %3.3f, accx filt: %3.3f, accy filt: %3.3f\r\n", gyroV,gz.read()*3.3f, accX, accY);
+        pc.printf("Phii: %3.3f\r\n", phi_mes);
     }
 }
 //******************************************************************************