Diff: main.cpp

Revision:

12:1aa5ff8333e8

Parent:

11:a0074351e154

Child:

13:724759951a6f

diff -r a0074351e154 -r 1aa5ff8333e8 main.cpp
--- a/main.cpp	Mon May 07 06:14:56 2018 +0000
+++ b/main.cpp	Mon May 07 09:00:43 2018 +0000
@@ -52,9 +52,10 @@
 // ... here define variables like gains etc.
 //------------------------------------------
 //LinearCharacteristics i2u(0.8f,-2.0f);
-LinearCharacteristics i2u(-2.0f,2.0f,0.0f,3.2f);
-LinearCharacteristics u2ax(0.69670f,0.29817,-9.81f,9.81f);
-LinearCharacteristics u2ay(0.69988f,0.29792,-9.81f,9.81f);
+//LinearCharacteristics i2u(-2.0f,2.0f,0.0f,3.2f);    // schwach parametrierung
+LinearCharacteristics i2u(-15.0f,15.0f,0.0f,3.2f);    // starke parametrierung
+LinearCharacteristics u2ax(0.69630f,0.29792,9.81f,-9.81f);
+LinearCharacteristics u2ay(0.70012f,0.29750,9.81f,-9.81f);
 LinearCharacteristics u2gyro(-4.65f,1.5f);
 
 
@@ -66,7 +67,8 @@
 //------------------------------------------
 // ... here define instantiate classes
 //------------------------------------------
-PI_Cntrl vel_cntrl(0.5f,.05f,Ts,0.4f);
+PI_Cntrl vel_cntrl(0.5f,0.05f,Ts,0.4f);
+PI_Cntrl omega2zero(-0.002f,2.0f,Ts,0.5);
 
 float tau = 1.0f;
 IIR_filter LPF_gyro(tau,Ts,tau);
@@ -114,9 +116,11 @@
     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;
+    // komplementärfilter
+    float phi_mes = ((PI/4) + atan2(-accX, accY) + gyroV);
     
-    float torque = -(-3.7f*phi_mes - 0.42f * gyro_dir);
+    // regler
+    float torque = omega2zero(0-vel)-(-9.6910f*phi_mes - 0.71190f * gyro_dir);
     out.write(i2u(torque/0.217f)/3.3f);
     
     
@@ -136,4 +140,6 @@
 // pressed button
 //******************************************************************************
 
-//...
\ No newline at end of file
+//...
+
+