Diff: GPA.cpp

Revision:

2:769ce5f06d3e

Parent:

0:78ca29b4c49e

Child:

4:2cc56521aa16

--- a/GPA.cpp	Mon Apr 09 05:50:04 2018 +0000
+++ b/GPA.cpp	Mon Apr 09 08:01:29 2018 +0000
@@ -1,72 +1,83 @@
 /*
     GPA: frequency point wise gain and phase analyser to measure the frequency
-         respone of dynamical system
-    
+         respone of a dynamical system
+
         hint:       the measurements should only be perfomed in closed loop
         assumption: the system is at the desired steady state of interest when
                     the measurment starts
-    
+
                              exc(2)                 C: controller
                                |                    P: plant
-                               v                     
+                               v
      exc(1) --> o   ->| C |--->o------->| P |----------> out
                 ^                  |             |
                 |                   --> inp      |  exc: excitation signal (E)
                 |                                |  inp: input plant (U)
                  --------------------------------   out: output plant (Y)
-  
+
     instantiate option 1:
     GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
-    
+
         fMin:     minimal desired frequency that should be measured in Hz
         fMax:     maximal desired frequency that should be measured in Hz
         NfexcDes: number of logarithmic equaly spaced frequency points
         NperMin:  minimal number of periods that are used for each frequency point
-        NmeasMin: maximal number of samples that are used for each frequency point
+        NmeasMin: minimal number of samples that are used for each frequency point
         Ts:       sampling time
-        Aexc0:    excitation amplitude at fMin 
+        Aexc0:    excitation amplitude at fMin
         Aexc1:    excitation amplitude at fMax
-    
+
         hints:    the amplitude drops with 1/fexc, if you're using
-                  Axc1 = Aexc0/fMax the d/dt exc = const., this is recommended
+                  Axc1 = Aexc0/fMax then d/dt exc = const., this is recommended
                   if your controller does not have a rolloff.
                   if a desired frequency point is not measured try to increase Nmeas.
-    
-    pseudo code for a closed loop measurement with a proportional controller Kp:
-    
-        float inp = "measurement of inputsignal";
-        float out = "mesurement of outputsignal";
-        float exc = myGPA(inp, out);
-        float off = "desired steady state off the system";
-        
+
+    pseudo code for a closed loop measurement with a controller C:
+
         excitation input at (1):
-        inp = Kp*(exc + off - out);
-        
+       
+        - measuring the plant P and the closed loop tf T = PC/(1 + PC):
+            desTorque = pi_w(omega_desired - omega + excWobble);
+            inpWobble = desTorque;
+            outWobble = omega;
+            excWobble = Wobble(excWobble, outWobble);
+                        
+        - measuring the controller C and the closed loop tf SC = C/(1 + PC)
+            desTorque = pi_w(omega_desired - omega + excWobble);
+            inpWobble = n_soll + excWobble - omega;
+            outWobble = desTorque;
+            excWobble = Wobble(inpWobble, outWobble);
+
         excitation input at (2):
-        inp = Kp*(off - out) + exc;
         
+        - measuring the plant P and the closed loop tf SP = P/(1 + PC):
+            desTorque = pi_w(omega_desired - omega) + excWobble;
+            inpWobble = desTorque;
+            outWobble = omega;
+            excWobble = Wobble(excWobble, outWobble);    
+
     usage:
-        exc = myGPA(inp, out) does update the internal states of the gpa at the
-        timestep k and returns the excitation signal for the timestep k+1. the 
-        results are plotted to a terminal (putty) over serial cennection and look
-        as follows:
+        exc(k+1) = myGPA(inp(k), out(k)) does update the internal states of the 
+        gpa at the timestep k and returns the excitation signal for the timestep
+        k+1. the results are plotted to a terminal (putty) over a serial 
+        connection and look as follows:
  -----------------------------------------------------------------------------------------
    fexc[Hz]    |Gyu|     ang(Gyu)    |Gye|     ang(Gye)     |E|        |U|        |Y|
  -----------------------------------------------------------------------------------------
-   7.01e-01   1.08e+01  -7.45e-02   1.08e+01  -7.38e-02   9.99e-01   9.99e-01   1.08e+01
-        
+   1.000e+00  2.924e+01 -1.572e+00  1.017e+00 -4.983e-02  5.000e+00  1.739e-01  5.084e+00
+
     in matlab you can use:
         dataNucleo = [... insert measurement data here ...];
         g = frd(dataNucleo(:,2).*exp(1i*dataNucleo(:,3)), dataNucleo(:,1), Ts, 'Units', 'Hz');
         gcl = frd(dataNucleo(:,4).*exp(1i*dataNucleo(:,5)), dataNucleo(:,1), Ts, 'Units', 'Hz');
-        
+
     if you're evaluating more than one measurement which contain equal frequency points try:
         dataNucleo = [dataNucleo1; dataNucleo2];
         [~, ind] = unique(dataNucleo(:,1),'stable');
         dataNucleo = dataNucleo(ind,:);
-         
+
         autor: M.E. Peter
-*/ 
+*/
 
 #include "GPA.h"
 #include "mbed.h"
@@ -146,7 +157,8 @@
                 pi2Tsfexc = pi2Ts*fexc;
             }
         }
-        fexcPast = fexc;
+        // secure sinarg starts at 0 (numerically maybe not given)
+        sinarg = 0.0f;
         // filter scaling
         scaleG = 1.0f/sqrt((float)Nmeas);
         // filter coefficients
@@ -170,6 +182,7 @@
     if(jj == Nmeas) {
         jj = 1;
         ii += 1;
+        fexcPast = fexc;
         // calculate the one point dft
         float Ureal = 2.0f*scaleG*(cr*sU[1] - sU[2]);
         float Uimag = 2.0f*scaleG*ci*sU[1];
@@ -183,12 +196,12 @@
         float absGye = Ymag/Aexc;
         float angGye = (atan2(Yimag, Yreal) + piDiv2);
         // user info
-        if(ii == 1) {
+        if(ii == 2) {
             printLine();
             printf("   fexc[Hz]    |Gyu|     ang(Gyu)    |Gye|     ang(Gye)     |E|        |U|        |Y|\r\n");
             printLine();
         }
-        printf("%11.2e %10.2e %10.2e %10.2e %10.2e %10.2e %10.2e %10.2e\r\n", fexc, absGyu, angGyu, absGye, angGye, Aexc, Umag, Ymag);
+        printf("%11.3e %10.3e %10.3e %10.3e %10.3e %10.3e %10.3e %10.3e\r\n", fexc, absGyu, angGyu, absGye, angGye, Aexc, Umag, Ymag);
     } else {
         jj += 1;
     }