Diff: main.cpp

Revision:

10:a28f393c6716

Parent:

9:30ee1d386a1d

Child:

11:ed2638662dfa

--- a/main.cpp	Thu Mar 22 17:34:18 2018 +0000
+++ b/main.cpp	Wed Apr 04 14:18:43 2018 +0000
@@ -7,6 +7,7 @@
 #include "PI_Cntrl.h"
 #include "IIR_filter.h"
 #include "LinearCharacteristics.h"
+#include "GPA.h"
 
 /* Cuboid balance on one edge on Nucleo F446RE
 // -----------------------------------------------------------------------------
@@ -50,8 +51,8 @@
 
 // controller parameters etc.
 float out_value = 1.6f;                 // set voltage on 1.6 V (0 A current)
-float Ts = 0.0025;                      // sample time
-float v_max = 200;                      // maximum speed rad/s
+float Ts = 0.0025f;                      // sample time
+float v_max = 200.0f;                      // maximum speed rad/s
 float n_soll = 0.0f;                    // nominal speed for speed control tests
 float tau = 1.05f;            // time constant of complementary filter
 float fg = 300.0f;
@@ -69,8 +70,19 @@
 float maxCurrent = 15.0f;
 float Km = 1/0.217f;   // Motorgain: Torque -> km -> Current in A/Nm
 float maxTorque = maxCurrent/Km;
-PI_Cntrl pi_w2zero(-.01f, 0.8f, maxTorque);  // controller to bring motor speed to zero while balancing
-PI_Cntrl pi_w(0.5f, 0.2f, maxTorque);              // PI controller for test purposes motor speed (no balance)
+PI_Cntrl pi_w2zero(-.01f, 0.8f, Ts, maxTorque);  // controller to bring motor speed to zero while balancing
+PI_Cntrl pi_w(0.6f, 0.4f, Ts, maxTorque);              // PI controller for test purposes motor speed (no balance)
+float desTorque = 0.0f;
+
+float fMin = 1.0f;
+float fMax = 1.0f/2.0f/Ts*0.9f;
+int NfexcDes = 150;
+float Aexc0 = 5.0f;
+float Aexc1 = 0.3f; //Aexc0/fMax;
+int NperMin = 3;
+float TmeasMin = 1.0f;
+int NmeasMin = (int)ceil(TmeasMin/Ts);
+GPA Wobble(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1);
 
 IIR_filter FilterACCx(tau, Ts, 1.0f);                // 1st order LP for complementary filter acc_x
 IIR_filter FilterACCy(tau, Ts, 1.0f);                // 1st order LP for complementary filter acc_y
@@ -111,9 +123,13 @@
     FilterGYRZ.reset(u2gz(3.3f*gz.read()));
     
     // FilterDiffANG.reset(u2gz(0.0f));
+    
+    Wobble.reset();
+    Wobble.printGPAmeasPara();
 
     // reset output
-    out.write(u3k3_TO_1V(i2u(0.0f)));
+    desTorque = 0.0f;
+    out.write(u3k3_TO_1V(i2u(desTorque*Km)));
 
     // attach controller loop to timer interrupt
     ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz;
@@ -146,10 +162,11 @@
     }
 
     // update controllers
-    float desTorque = 0.0f;
     if(shouldBalance) {
+        ///*
+        // balance, set n_soll = 0.0f
         // K matrix: -5.2142   -0.6247  // from Matlab
-        float uPiC = pi_w2zero(n_soll - omega - 24.9f); // needs further inverstigation
+        float uPiC = pi_w2zero(n_soll - omega - 0*24.9f); // needs further inverstigation
         float uSsC = (-5.2142f*ang - 0.6247f*gyrz);
         desTorque = uPiC - uSsC;     // state space controller for balance, calc desired Torque
         if(abs(desTorque) > maxTorque) {
@@ -157,8 +174,23 @@
         }
         if(k == 0) printLine();
         if(k++ < 2000) pc.printf("%6.4f %6.4f %6.4f %6.4f\r\n", uPiC, uSsC, ang, omega);
+        //*/
+        /* 
+        // step response, set n_soll = 0.0f
+        desTorque = pi_w(10.0f - omega); 
+        if(k == 0) printLine();
+        if(k++ < 2000) pc.printf("%6.4f %6.4f %6.4f\r\n", 10.0f, omega, desTorque);
+        */
+        /*
+        // wobble, set n_soll = 15.0f
+        float inpWobble = desTorque;
+        float outWobble = omega;
+        float excWobble = Wobble(inpWobble, outWobble);
+        desTorque = pi_w(n_soll + excWobble - omega);
+        if(++k == 73000) Wobble.printGPAmeasTime();
+        */
     } else {
-        desTorque = pi_w(n_soll-omega);     // state space controller for balance, calc desired Torque
+        desTorque = pi_w(n_soll - omega);     // state space controller for balance, calc desired Torque
     }
     // convert Nm -> A and write to AOUT
     out.write(u3k3_TO_1V(i2u(desTorque*Km)));