gpa in double prescision

Dependencies:   mbed

Revision:
9:4e6f3404d473
Parent:
8:3b98b6e1ead3
Child:
10:b7f142952df9
diff -r 3b98b6e1ead3 -r 4e6f3404d473 main.cpp
--- a/main.cpp	Fri Mar 09 17:39:49 2018 +0000
+++ b/main.cpp	Tue Mar 13 20:23:44 2018 +0000
@@ -55,7 +55,7 @@
 // float Tn = 0.10f;                       // integral time       "     "        "
 float Ts = 0.0025;                      // sample time
 float v_max = 200;                      // maximum speed rad/s
-float n_soll = 12.0f;                    // nominal speed for speed control tests
+float n_soll = 15.0f;                    // nominal speed for speed control tests
 float tau = 1.0f;            // time constant of complementary filter
 float fg = 300.0f;
 
@@ -66,61 +66,49 @@
 
 // set up encoder
 EncoderCounter MotorEncoder(PB_6, PB_7);    // initialize counter on PB_6 and PB_7
-DiffCounter MotorDiff(0.01, Ts);             // discrete differentiate, based on encoder data
+DiffCounter MotorDiff(1/(2.0f*pi*30.0f), Ts);             // discrete differentiate, based on encoder data
 
 PI_Cntrl pi_w2zero(-.01f, 1.0f, 0.4f);  // controller to bring motor speed to zero while balancing
-PI_Cntrl pi_w(1.0f, 0.1f, 2.0f);              // PI controller for test purposes motor speed (no balance)
+PI_Cntrl pi_w(0.5f, 0.2f, 12.0f);              // PI controller for test purposes motor speed (no balance)
 
 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
 IIR_filter FilterGYRZ(tau, Ts, tau);     // 1st order LP for complementary filter gyro
 
-/*
-float Aexc = 1.0f;
-float Oexc = 1.0f;
 float fMin = 1.0f;
 float fMax = 1.0f/2.0f/Ts*0.9f;
-int Ndata = 50;
-int NperMin = 10;
-float TmeasMin = 2.0;
-int NmeasMin = (int)ceil(TmeasMin/Ts);
-GPA Wobble(fMin, fMax, Ndata, NperMin, NmeasMin, Ts, Aexc);
-*/
-/*
-float Aexc = 1.0f;
-float Oexc = 1.0f;
-float fMin = 20.0f;
-float fMax = 1.0f/2.0f/Ts*0.9f;
-int Ndata = 20;
-int NperMin = 1;
-float TmeasMin = 0.0025;
+int NfexcDes = 100;
+float Aexc0 = 6.0f;
+float Aexc1 = 0.5f; //Aexc0/fMax;
+int NperMin = 3;
+float TmeasMin = 1.0;
 int NmeasMin = (int)ceil(TmeasMin/Ts);
-GPA Wobble(fMin, fMax, Ndata, NperMin, NmeasMin, Ts, Aexc);
-*/
-/*
-float w0 = 2.0f*pi*3.0f;
-float D = 0.02f;
-float K = 10.0f;
-IIR_filter SysSecOrder(w0, D, Ts, 10);
-float u = 1.0f;
-float y = K*u;
-float exc = 0.0f;
-*/
-
-float Aexc = 6.0f;
-float fMin = 0.3f;
-float fMax = 1.0f/2.0f/Ts*0.9f;
-int Ndata = 30;
-int NperMin = 10;
-float TmeasMin = 2.0;
-int NmeasMin = (int)ceil(TmeasMin/Ts);
-GPA Wobble(fMin, fMax, Ndata, NperMin, NmeasMin, Ts, Aexc);
+GPA Wobble(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1);
 float inpWobble = 0.0f;
 float outWobble = 0.0f;
 float excWobble = 0.0f;
 short counts = 0;           
 float omega = 0.0f;
-float desTorque = 0.0f;        
+float desCurrent = 0.0f;
+
+/* // simulation on nucleo
+float Oexc = 1.0f;
+float fMin = 0.7f;
+float fMax = 1.0f/2.0f/Ts*0.72f;
+int NfexcDes = 100;
+float Aexc0 = 1.0f;
+float Aexc1 = Aexc0/fMax;
+int NperMin = 1;
+float TmeasMin = 1.0;
+int NmeasMin = 1;//(int)ceil(TmeasMin/Ts);
+GPA Wobble(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1);
+float w0 = 2.0f*pi*3.0f;
+float D = 0.02f;
+IIR_filter SysSecOrder(w0, D, Ts, 10.0f);
+float u = Oexc;
+float y = 10.0f*u;
+float exc = 0.0f;
+*/
 
 // IIR_filter FilterANG(1.0f/(2.0f*PI*fg), Ts, 1.0f);
 // IIR_filter FilterDiffANG(1.0f/(2.0f*PI*fg), Ts);
@@ -159,7 +147,6 @@
     Wobble.reset();
     // SysSecOrder.reset(u);
 
-    Wobble.printGPAfexcDes();
     Wobble.printGPAmeasPara();
 
     // FilterANG.reset(atan2(-u2ax(3.3f*ax.read()), u2ay(3.3f*ay.read())) + PI/4.0f);
@@ -176,30 +163,28 @@
 
 void updateControllers(void)
 {
-
+    
     counts = MotorEncoder;            // counts in 1
     omega = MotorDiff(counts);        // angular velocity motor
 
     if(shouldBalance) {
-        inpWobble = desTorque;
+        inpWobble = desCurrent;
         outWobble = omega;
         excWobble = Wobble(inpWobble, outWobble);
     }
-    desTorque = pi_w((n_soll + excWobble)-omega);
-    out.write(u3k3_TO_1V(i2u(desTorque/0.217f)));
-
+    desCurrent = pi_w((n_soll + excWobble - omega)/0.217f);
+    out.write(u3k3_TO_1V(i2u(desCurrent)));
+    
     /*
-    // if(k<=100) pc.printf("%3.2f %3.2f \r\n", u, y);
     u = Wobble(u, y) + Oexc;
     y = SysSecOrder(u);
-    // k += 1;
+    k += 1;
 
-    if(k == 70000) {
+    if(k == 11000) {
         Wobble.printGPAmeasTime();
     }
-
     */
-
+    
     /*
     // read encoder data
     short counts = MotorEncoder;            // counts in 1