gpa in double prescision

Dependencies:   mbed

Revision:
8:3b98b6e1ead3
Parent:
7:b3c5116e9fab
Child:
9:4e6f3404d473
--- a/main.cpp	Fri Mar 02 17:11:34 2018 +0000
+++ b/main.cpp	Fri Mar 09 17:39:49 2018 +0000
@@ -1,22 +1,22 @@
 #include "mbed.h"
 #include "math.h"
-
-#define PI 3.1415927f
+#define   pi 3.1415927f
 
 #include "EncoderCounter.h"
 #include "DiffCounter.h"
 #include "PI_Cntrl.h"
 #include "IIR_filter.h"
 #include "LinearCharacteristics.h"
+#include "GPA.h"
 
 /* Cuboid balance on one edge on Nucleo F446RE
 // -----------------------------------------------------------------------------
 
 IMPORTANT: use ..\T-RT\Messen_Ausstellungen\Praesentationen_im_Labor\Wuerfel_nucleo\Escon_Parameter_4nucleo_stark.edc
            settings for Maxon ESCON controller (upload via ESCON Studio)
-           
+
 hardware Connections:
- 
+
  CN7                    CN10
   :                         :
   :                         :
@@ -30,12 +30,12 @@
  o. ENC CH B               ..
  ..                        ..
  ..                        ..
- .o AIN acx (PA_0)         ..  
+ .o AIN acx (PA_0)         ..
  .o AIN acy (PA_1)         ..                  5.
  .o AIN Gyro(PA_4)         .o Analog GND
  ..                        ..
- ..                        ..              
- ..                        ..                  1. 
+ ..                        ..
+ ..                        ..                  1.
  ----------------------------
  CN7               CN10
  */
@@ -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 = 0.0f;                    // nominal speed for speed control tests
+float n_soll = 12.0f;                    // nominal speed for speed control tests
 float tau = 1.0f;            // time constant of complementary filter
 float fg = 300.0f;
 
@@ -64,7 +64,7 @@
 bool doesStand = 0;                     // state if the cube is standing or not
 bool shouldBalance = 0;                 // state if the controller is active
 
-// set up encoder         
+// 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
 
@@ -75,6 +75,53 @@
 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 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);
+float inpWobble = 0.0f;
+float outWobble = 0.0f;
+float excWobble = 0.0f;
+short counts = 0;           
+float omega = 0.0f;
+float desTorque = 0.0f;        
+
 // IIR_filter FilterANG(1.0f/(2.0f*PI*fg), Ts, 1.0f);
 // IIR_filter FilterDiffANG(1.0f/(2.0f*PI*fg), Ts);
 
@@ -97,47 +144,78 @@
 int main()
 {
     // for serial comm.
-    pc.baud(2000000);   
-    
+    pc.baud(2000000);
+
     // reset encoder, controller and filters
     MotorEncoder.reset();
     MotorDiff.reset(0.0f,0.0f);
     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()));
-    
+
+    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);
     // FilterDiffANG.reset(0.0f);
-    
+
     // reset output
-    out.write(u3k3_TO_1V(i2u(0.0f))); 
-    
+    out.write(u3k3_TO_1V(i2u(0.0f)));
+
     // attach controller loop to timer interrupt
     ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz;
     Button.fall(&pressed);          // attach key pressed function
     Button.rise(&released);         // attach key pressed function
 }
 
-void updateControllers(void){
-    
+void updateControllers(void)
+{
+
+    counts = MotorEncoder;            // counts in 1
+    omega = MotorDiff(counts);        // angular velocity motor
+
+    if(shouldBalance) {
+        inpWobble = desTorque;
+        outWobble = omega;
+        excWobble = Wobble(inpWobble, outWobble);
+    }
+    desTorque = pi_w((n_soll + excWobble)-omega);
+    out.write(u3k3_TO_1V(i2u(desTorque/0.217f)));
+
+    /*
+    // if(k<=100) pc.printf("%3.2f %3.2f \r\n", u, y);
+    u = Wobble(u, y) + Oexc;
+    y = SysSecOrder(u);
+    // k += 1;
+
+    if(k == 70000) {
+        Wobble.printGPAmeasTime();
+    }
+
+    */
+
+    /*
     // read encoder data
     short counts = MotorEncoder;            // counts in 1
-    float omega = MotorDiff(counts);        // angular velofity motor
-    
+    float omega = MotorDiff(counts);        // angular velocity 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());
-    
+
     // perform complementary filter
     float ang = atan2(-FilterACCx(accx), FilterACCy(accy)) + FilterGYRZ(gyrz) + PI/4.0f;
-    
+
     // float angf = FilterANG(ang);
     // float dangf = FilterDiffANG(ang);
-    
+
     // get current state of the cube
     float actualAngleDegree = ang*180.0f/PI;
     if(actualAngleDegree > -10.0f && actualAngleDegree < 10.0f){
@@ -146,71 +224,72 @@
     else{
         doesStand = 0;
     }
-    
+
     // update controllers
     float desTorque = 0.0f;
-    if(shouldBalance){       
+    if(shouldBalance){
         // K matrix: -5.2142   -0.6247  // from Matlab
-        desTorque = pi_w2zero(n_soll-omega)-(-5.2142f*ang-0.6247f*gyrz);     // state space controller for balance, calc desired Torque 
+        desTorque = pi_w2zero(n_soll-omega)-(-5.2142f*ang-0.6247f*gyrz);     // state space controller for balance, calc desired Torque
     }
     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/0.217f)));                
-    
-    //out.write(u3k3_TO_1V(i2u(pi_w(n_soll-omega))));          // test speed controller 
+    // convert Nm -> A and write to AOUT
+    out.write(u3k3_TO_1V(i2u(desTorque/0.217f)));
+
+    //out.write(u3k3_TO_1V(i2u(pi_w(n_soll-omega))));          // test speed controller
     if(++k >= 199){
         k = 0;
         pc.printf("phi=%3.2f omega=%3.2f \r\n", actualAngleDegree, omega);
         //pc.printf("ax=%3.2f ay=%3.2f gz=%3.2f \r\n",u2ax(3.3f*ax.read()),u2ay(3.3f*ay.read()),wz);
     }
+    */
 }
 
 // Buttonhandling and statemachine
 // -----------------------------------------------------------------------------
 // start timer as soon as Button is pressed
-void pressed(){   
+void pressed()
+{
     t.start();
 }
 
 // evaluating statemachine
-void released(){
-    
+void released()
+{
+
     // readout, stop and reset timer
     float ButtonTime = t.read();
     t.stop();
     t.reset();
-    
+
     // if the cube doesStand
-    if(doesStand)
-    {
-        // in - or decrease speed 
-        if(ButtonTime < 2.0f){
-            // press Button long -> increase speed 5 rev/min 
-            if(ButtonTime > 0.5f){
+    if(doesStand) {
+        // in - or decrease speed
+        if(ButtonTime < 2.0f) {
+            // press Button long -> increase speed 5 rev/min
+            if(ButtonTime > 0.5f) {
                 n_soll -= 5.0f;
             }
             // press Button short -> decrease speed 5 rev/min
-            else{
+            else {
                 n_soll += 5.0f;
             }
             // constrain n_soll
             if(n_soll >  v_max)
-                n_soll = v_max; 
+                n_soll = v_max;
             if(n_soll < -v_max)
                 n_soll = -v_max;
         }
         // stop balancing
-        else{
-                n_soll = 0.0f;
+        else {
+            n_soll = 0.0f;
             shouldBalance = 0;
             pi_w2zero.reset(0.0f);
         }
-    }
-    else{
+    } else {
         if(ButtonTime > 2.0f)
-           shouldBalance = 1;
-           pi_w.reset(0.0f);
+            shouldBalance = 1;
+        pi_w.reset(0.0f);
     }
 }