Diff: main.cpp

Revision:

23:1d24b1033371

Parent:

21:cef093edb441

Child:

27:8f1ab2a2267b

--- a/main.cpp	Thu May 17 11:35:16 2018 +0000
+++ b/main.cpp	Thu May 31 13:51:07 2018 +0000
@@ -8,6 +8,7 @@
 #include "IIR_filter.h"
 #include "LinearCharacteristics.h"
 #include "GPA.h"
+#include "PRBS.h"
 
 /* Cuboid balance on one edge on Nucleo F446RE
 // -----------------------------------------------------------------------------
@@ -52,7 +53,7 @@
 // controller parameters etc.
 float Ts = 0.0025f;                     // sample time
 float v_max = 200.0f;                   // maximum speed rad/s
-float n_soll = 15.0f;                   // nominal speed for speed control tests
+float n_soll = 15.0f;                    // nominal speed for speed control tests
 float tau = 1.00f;                      // time constant of complementary filter
 
 // output and statemachine
@@ -77,16 +78,23 @@
 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;
+float Aexc0 = 3.0f;
+float Aexc1 = 0.5f; //Aexc0/fMax;
+int NperMin = 10;
+float TmeasMin = 2.0f;
 int NmeasMin = (int)ceil(TmeasMin/Ts);
 GPA Wobble(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1);
 float inpWobble = 0.0f;
 float outWobble = 0.0f;
 float excWobble = 0.0f;
 
+// set up prbs generator
+int n = 9;
+int Aprbs = 1.0f;
+int Nprbs = (int)pow((float)2, (float)n) - 1;
+int Nexp = 50;
+PRBS prbs(n);
+
 // set up filters for complementary filter
 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
@@ -94,22 +102,20 @@
 
 // set up filter for swing down process
 IIR_filter FilterTrajectory(10.0f, 1.0f, Ts, 1.0f);
-float V = -2.6080f;
 
 // linear characteristics
-LinearCharacteristics i2u(0.1067f, -15.0f);          // full range, convert desired current (Amps)  -> voltage 0..3.3V
-LinearCharacteristics u2n(312.5f, 1.6f);             // convert input voltage (0..3.3V) -> speed (1/min)
-LinearCharacteristics u2w(32.725, 1.6f);             // convert input voltage (0..3.3V) -> speed (rad/sec)
-LinearCharacteristics u2ax(14.67f, 1.6378f);         // convert input voltage (0..3.3V) -> acc_x m/s^2
-LinearCharacteristics u2ay(15.02f, 1.6673f);        // convert input voltage (0..3.3V) -> acc_y m/s^2
-LinearCharacteristics u2gz(-4.652f, 1.4949f);        // convert input voltage (0..3.3V) -> w_x rad/s
-LinearCharacteristics u3k3_TO_1V(0.303030303f, 0.0f, 3.3f, 0.0f);// normalize output voltage (0..3.3)V -> (0..1) V
+LinearCharacteristics i2u(-15.0f,15.0f,0.0f,3.2f / 3.3f);       // output is normalized output
+LinearCharacteristics u2ax(0.29719f,0.69768f,-9.81f,9.81f);     // use normalized input
+LinearCharacteristics u2ay(0.29890f,0.70330f,-9.81f,9.81f);      // use normalized input
+LinearCharacteristics u2gz(-4.6517f * 3.3f,0.45495f);           // use normalized input, change sign
+                // 4.6517f = 1/3.752e-3 [V / °/s] * pi/180
 
 // user defined functions
 void updateControllers(void);   // speed controller loop (via interrupt)
 void pressed(void);             // user Button pressed
 void released(void);            // user Button released
 void printLine();
+//void printStart();
 
 // main program and control loop
 // -----------------------------------------------------------------------------
@@ -124,21 +130,26 @@
     pi_w2zero.reset(0.0f);
     pi_w.reset(0.0f);
 
-    FilterACCx.reset(u2ax(3.3f*ax.read()));
-    FilterACCy.reset(u2ay(3.3f*ay.read()));
-    FilterGYRZ.reset(u2gz(3.3f*gz.read()));
+    FilterACCx.reset(u2ax(ax.read()));
+    FilterACCy.reset(u2ay(ay.read()));
+    FilterGYRZ.reset(u2gz(gz.read()));
 
     FilterTrajectory.reset(0.0f);
 
     Wobble.reset();
-    Wobble.printGPAmeasPara();
+    //Wobble.printGPAmeasPara();
     inpWobble = 0.0f;
     outWobble = 0.0f;
     excWobble = 0.0f;
-
+    
+    prbs.reset();
+    // prbs.printPRBSind();
+    // prbs.printPRBSregister();
+    // printLine();
+    
     // reset output
     desTorque = 0.0f;
-    out.write(u3k3_TO_1V(i2u(desTorque*Km)));
+    out.write(i2u(desTorque*Km));
 
     // reset statemachine
     shouldGoDown = 0;
@@ -158,9 +169,9 @@
     float omega = MotorDiff(counts);        // angular velofity motor
 
     // read imu data
-    float accx = u2ax(3.3f*ax.read());
-    float accy = u2ay(3.3f*ay.read());
-    float gyrz = u2gz(3.3f*gz.read());
+    float accx = u2ax(ax.read());
+    float accy = u2ay(ay.read());
+    float gyrz = u2gz(gz.read());
 
     // perform complementary filter
     float ang = atan2(-FilterACCx(accx), FilterACCy(accy)) + FilterGYRZ(gyrz) + pi/4.0f;
@@ -175,20 +186,20 @@
 
     // update controllers
     if(shouldBalance) {
-        
-        ///*
+               
+        /*
         // balance, set n_soll = 0.0f
         // ---------------------------------------------------------------------
-        // K matrix: -5.2142   -0.6247  // from Matlab
+        // K matrix: [-5.2142 -0.6247] // from Matlab
         float uPI = pi_w2zero(n_soll - omega); // needs further inverstigation
-        float uSS = (-5.2142f*ang - 0.6247f*gyrz);
+        float uSS = (-5.2142f*ang -0.6247f*gyrz);
         desTorque = uPI - uSS;     // state space controller for balance, calc desired Torque
         if(abs(desTorque) > maxTorque) {
             desTorque = copysign(maxTorque, desTorque);
         }
         if(k == 0) printLine();
         if(k++ < 2000) pc.printf("%6.4f %6.4f %6.4f %6.4f\r\n", uPI, uSS, ang, omega);
-        //*/
+        */
         
         /*
         // step response velocity controller, set n_soll = 0.0f
@@ -205,22 +216,31 @@
         desTorque = pi_w(n_soll + excWobble - omega);
         inpWobble = desTorque;
         outWobble = omega;
-        excWobble = Wobble(excWobble, outWobble);
+        excWobble = Wobble(inpWobble, outWobble);
         // measuring the controller C and the closed loop tf SC = C/(1 + PC)
         // desTorque = pi_w(n_soll + excWobble - omega);
         // inpWobble = n_soll + excWobble - omega;
         // outWobble = desTorque;
         // excWobble = Wobble(inpWobble, outWobble);
-        if(++k == 73000) Wobble.printGPAmeasTime();
+        // //if(++k == 73000) Wobble.printGPAmeasTime();
         */
         
+        ///*
+        // prbs measurement, set n_soll = 10.0f
+        // ---------------------------------------------------------------------
+        // measuring the closed loop tf T = PC/(1 + PC)
+        float excPrbs = Aprbs*prbs();
+        desTorque = pi_w(n_soll - omega + excPrbs);
+        if(k++ < Nexp*Nprbs) pc.printf("%10i %10.3e %10.3e\r\n", k, excPrbs, omega);
+        //*/
+        
     } else if(shouldGoDown) {
         // swing down
-        // K matrix: -5.2142   -0.6247  // from Matlab
-        // V gain: -2.6080              // from Matlab
+        // K matrix: [-5.2142 -0.6247] // from Matlab
+        // V gain:    -2.6080          // from Matlab
         float ref = FilterTrajectory(pi/4.0f);
-        float uV  = V*ref;
-        float uSS = (-5.2142f*ang - 0.6247f*gyrz);
+        float uV  = -2.6080f*ref;
+        float uSS = (-5.2142f*ang -0.6247f*gyrz);
         desTorque = uV - uSS;     // state space controller for balance
         if(abs(desTorque) > maxTorque) {
             desTorque = copysign(maxTorque, desTorque);
@@ -230,10 +250,10 @@
             FilterTrajectory.reset(omega);
         }
     } else {
-        desTorque = pi_w(FilterTrajectory(0.0f) - omega);     // state space controller for balance
+        desTorque = pi_w(FilterTrajectory(0.0f)*0 + n_soll - omega);     // state space controller for balance
     }
     // convert Nm -> A and write to AOUT
-    out.write(u3k3_TO_1V(i2u(desTorque*Km)));
+    out.write(i2u(desTorque*Km));
 }
 
 // Buttonhandling and statemachine
@@ -290,5 +310,12 @@
 
 void printLine()
 {
-    printf("-----------------------------------------------------------------------------------------\r\n");
+    printf("----------------------------------------------------------------------------------------- \r\n");
 }
+
+/*
+void printStart)
+{
+    printf("starting \r\n");
+}
+*/