Revision 13:724759951a6f, committed 2018-05-07

Comitter:

Kiwicjam

Date:

Mon May 07 09:06:54 2018 +0000

Parent:

12:1aa5ff8333e8

Commit message:

fertig komentiert

Changed in this revision

--- a/main.cpp	Mon May 07 09:00:43 2018 +0000
+++ b/main.cpp	Mon May 07 09:06:54 2018 +0000
@@ -52,33 +52,28 @@
 // ... here define variables like gains etc.
 //------------------------------------------
 //LinearCharacteristics i2u(0.8f,-2.0f);
-//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 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);
 
-
-
 //------------------------------------------
 Ticker  ControllerLoopTimer;            // interrupt for control loop
 EncoderCounter counter1(PB_6, PB_7);    // initialize counter on PB_6 and PB_7
 DiffCounter diff(0.01,Ts);              // discrete differentiate, based on encoder data
+
 //------------------------------------------
 // ... here define instantiate classes
 //------------------------------------------
 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;
+float tau = 1.0f;                   //Zeitkonstante Filter
 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)
 // ... define some linear characteristics -----------------------------------------
 
 // ----- User defined functions -----------
@@ -104,15 +99,10 @@
 void updateControllers(void){
     short counts = counter1;            // get counts from Encoder
     float vel = diff(counts);           // motor velocity 
-      //desTorque = vel_cntrl(w_soll - vel);
-     /*       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 gyro_dir = (u2gyro(gz.read()*3.3f));
-    float gyroV = LPF_gyro.filter(gyro_dir);
+    // filterung der Input Daten
+    float gyro = (u2gyro(gz.read()*3.3f));
+    float gyroV = LPF_gyro.filter(gyro);
     float accX = LPF_accX.filter(u2ax(ax.read()));
     float accY = LPF_accY.filter(u2ay(ay.read()));
     
@@ -120,17 +110,14 @@
     float phi_mes = ((PI/4) + atan2(-accX, accY) + gyroV);
     
     // regler
-    float torque = omega2zero(0-vel)-(-9.6910f*phi_mes - 0.71190f * gyro_dir);
+    float torque = omega2zero(0-vel)-(-9.6910f*phi_mes - 0.71190f * gyro);
     out.write(i2u(torque/0.217f)/3.3f);
     
     
     // OUTPUT
     if(++k >= 150){
         k = 0;
-        //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);
     }
 }