Revision 21:cef093edb441, committed 2018-05-17

Comitter:

altb

Date:

Thu May 17 11:15:36 2018 +0000

Parent:

20:1d5e89b2f22e

Child:

23:1d24b1033371

Commit message:

- changed LinCharacteristics parametrisation; - changed Prefilter and K-gain according cuboid Lab FS2018

Changed in this revision

--- a/LinearCharacteristics.cpp	Tue Apr 10 12:25:13 2018 +0000
+++ b/LinearCharacteristics.cpp	Thu May 17 11:15:36 2018 +0000
@@ -2,35 +2,20 @@
 
 using namespace std;
 
-LinearCharacteristics::LinearCharacteristics(float k, float offset){
-    this->k = k;
-    this->offset = offset;
-    this->upper_limit = 9.99e19;
-    this->lower_limit = -9.99e19;
-    }
-LinearCharacteristics::LinearCharacteristics(float k, float offset,float lim){
-    this->k = k;
+LinearCharacteristics::LinearCharacteristics(float gain,float offset){    // standard lin characteristics
+    this->gain = gain;
     this->offset = offset;
-    this->upper_limit = lim;
-    this->lower_limit = -lim;
-    }
-LinearCharacteristics::LinearCharacteristics(float k, float offset,float ulim,float llim){
-    this->k = k;
-    this->offset = offset;
-    this->upper_limit = ulim;
-    this->lower_limit = llim;
-    }
+}
+
+LinearCharacteristics::LinearCharacteristics(float xmin,float xmax, float ymin, float ymax){    // standard lin characteristics
+    this->gain = (ymax - ymin)/(xmax - xmin);
+    this->offset = xmax - ymax/this->gain;
+}
+
+LinearCharacteristics::~LinearCharacteristics() {}
 
 
-LinearCharacteristics::~LinearCharacteristics() {} 
-    
-float LinearCharacteristics::eval(float u){
-    float val = k * (u-offset);
-    if(val > upper_limit)
-        return upper_limit;
-    else if(val < lower_limit)
-        return lower_limit;
-    else
-        return val;
+float LinearCharacteristics::evaluate(float x)
+{   
+return this->gain*(x - this->offset);
     }
-    

--- a/LinearCharacteristics.h	Tue Apr 10 12:25:13 2018 +0000
+++ b/LinearCharacteristics.h	Thu May 17 11:15:36 2018 +0000
@@ -1,18 +1,27 @@
+// Linear Characteristics for different purposes (map Voltage to acc etc.)
+
+
+#ifndef LINEAR_CHARACTERISTICS_H_
+#define LINEAR_CHARACTERISTICS_H_   
+
+
 class LinearCharacteristics{
      public:
-                    LinearCharacteristics(float k,float offset);
-                    LinearCharacteristics(float k,float offset,float);
-                    LinearCharacteristics(float k,float offset,float,float);
-        float operator()(float u){
-         return eval(u);
-         }
-        virtual     ~LinearCharacteristics();
-        float       eval(float);
+            LinearCharacteristics(float, float);
+            LinearCharacteristics(float, float, float, float);
+            float evaluate(float);
+            float operator()(float x){
+                return evaluate(x);
+                } 
+                //...
+                virtual     ~LinearCharacteristics();
+                // here: the calculation function
     
     private:
-    
-        float k;
+        // here: private functions and values...
+        float gain;
         float offset;
-        float lower_limit;
-        float upper_limit;
-};
\ No newline at end of file
+};
+
+
+#endif      // LINEAR_CHARACTERISTICS_H_
\ No newline at end of file

--- a/main.cpp	Tue Apr 10 12:25:13 2018 +0000
+++ b/main.cpp	Thu May 17 11:15:36 2018 +0000
@@ -97,13 +97,11 @@
 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.29890,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)
@@ -124,9 +122,9 @@
     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);
 
@@ -158,9 +156,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;
@@ -179,9 +177,9 @@
         ///*
         // balance, set n_soll = 0.0f
         // ---------------------------------------------------------------------
-        // K matrix: -5.2142   -0.6247  // from Matlab
+        // K matrix: -9.6910   -0.7119  // from Matlab
         float uPI = pi_w2zero(n_soll - omega); // needs further inverstigation
-        float uSS = (-5.2142f*ang - 0.6247f*gyrz);
+        float uSS = (-9.6910f*ang -0.7119f*gyrz);
         desTorque = uPI - uSS;     // state space controller for balance, calc desired Torque
         if(abs(desTorque) > maxTorque) {
             desTorque = copysign(maxTorque, desTorque);
@@ -217,10 +215,10 @@
     } else if(shouldGoDown) {
         // swing down
         // K matrix: -5.2142   -0.6247  // from Matlab
-        // V gain: -2.6080              // from Matlab
+        // V gain: -7.1191              // from Matlab
         float ref = FilterTrajectory(pi/4.0f);
         float uV  = V*ref;
-        float uSS = (-5.2142f*ang - 0.6247f*gyrz);
+        float uSS = (-9.6910f*ang -0.7119f*gyrz);
         desTorque = uV - uSS;     // state space controller for balance
         if(abs(desTorque) > maxTorque) {
             desTorque = copysign(maxTorque, desTorque);