Revision 12:e18fdd404660, committed 2018-05-15

Comitter:

sipru

Date:

Tue May 15 19:01:38 2018 +0000

Parent:

11:67af6d24c588

Commit message:

Stefan

Changed in this revision

--- a/LinearCharacteristics.cpp	Fri Apr 27 06:54:18 2018 +0000
+++ b/LinearCharacteristics.cpp	Tue May 15 19:01:38 2018 +0000
@@ -2,3 +2,22 @@
 
 using namespace std;
 
+    LinearCharacteristics::LinearCharacteristics(float gain, float offset) {
+        this->gain = gain;
+        this->offset = offset;
+        
+    }
+    LinearCharacteristics::LinearCharacteristics(float xmin, float xmax,float ymin,float ymax) {
+        this->gain = (ymax-ymin)/(xmax-xmin);
+        this->offset = xmax-(ymax/gain);
+    }
+    
+    LinearCharacteristics::~LinearCharacteristics() {}
+    
+    float LinearCharacteristics::eval(float x) {
+        return gain * (x-offset);
+        
+    }
+
+
+

--- a/LinearCharacteristics.h	Fri Apr 27 06:54:18 2018 +0000
+++ b/LinearCharacteristics.h	Tue May 15 19:01:38 2018 +0000
@@ -8,11 +8,21 @@
 class LinearCharacteristics{
      public:
                 // here: the calculation function
+    LinearCharacteristics(float gain, float offset);
+    LinearCharacteristics(float xmin, float xmax,float ymin,float ymax);
     
+    float eval(float x);
+    virtual ~LinearCharacteristics();
+    
+    float operator () (float x){
+        return eval(x);
+        } 
     private:
         // here: private functions and values...
 
-
+        float gain;
+        float offset;
+        
 
 };
 

--- a/PI_Cntrl.cpp	Fri Apr 27 06:54:18 2018 +0000
+++ b/PI_Cntrl.cpp	Tue May 15 19:01:38 2018 +0000
@@ -11,3 +11,47 @@
 #include "PI_Cntrl.h"
 
 using namespace std;
+
+
+    PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts) {
+        this->Kp = Kp;
+        this->Tn = Tn;
+        this->Ts = Ts;   
+        
+        this->iMin = -0.4f;
+        this->iMax = 0.4f;
+        this->iPartOld = 0;
+        
+    }
+    
+    PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float iMin, float iMax) {
+        this->Kp = Kp;
+        this->Tn = Tn;
+        this->Ts = Ts;  
+        this->iMin = iMin;
+        this->iMax = iMax;
+        this->iPartOld = 0;
+        
+    }
+    
+    PI_Cntrl::~PI_Cntrl() {}
+    
+    float PI_Cntrl::eval(float val) {
+        
+        float pPart = Kp*val;
+        float iPart = iPartOld+(Ts*Kp/Tn)*val;
+        
+        
+        // Begrenze I
+        if(iPart > iMax) {
+            iPart = iMax;   
+        } else if(iPart < iMin) {
+            iPart = iMin;   
+        }
+        
+        iPartOld = iPart;
+        
+        return pPart+iPart;   
+    }
+    
+    

--- a/PI_Cntrl.h	Fri Apr 27 06:54:18 2018 +0000
+++ b/PI_Cntrl.h	Tue May 15 19:01:38 2018 +0000
@@ -5,10 +5,26 @@
 class PI_Cntrl
 {
 public:
-
+    PI_Cntrl(float Kp, float Tn, float Ts);
+    PI_Cntrl(float Kp, float Tn, float Ts, float iMin, float iMax);
+    
+    
+    float eval(float val);
+    virtual ~PI_Cntrl();
+    
+    float operator() (float e) {
+        return eval(e);
+    }
 
 private:
 
+    float Kp;
+    float Tn;
+    float Ts;
+    float iMin;
+    float iMax;
+    float iPartOld;
+    
 
 };
     

--- a/main.cpp	Fri Apr 27 06:54:18 2018 +0000
+++ b/main.cpp	Tue May 15 19:01:38 2018 +0000
@@ -44,7 +44,7 @@
 AnalogIn gz(PA_4);                      // Analog IN (gyr z) on PA_4
 AnalogOut out(PA_5);                    // Analog OUT on PA_5   1.6 V -> 0A 3.2A -> 2A (see ESCON)
 float out_value = 1.6f;                 // set voltage on 1.6 V (0 A current)
-float w_soll = 10.0f;                   // desired velocity
+float w_soll = 80.0f;                   // desired velocity
 float Ts = 0.002f;                      // sample time of main loops
 int k = 0;
 
@@ -61,6 +61,23 @@
 // ... here define instantiate classes
 //------------------------------------------
 
+LinearCharacteristics i2u(3.2f/4.0f,-2.0f);
+LinearCharacteristics i2u2(-2.0f,2.0f,0.0f,3.2f);
+
+LinearCharacteristics u2g(-4.622f*3.3f,0.45275f);
+LinearCharacteristics u2a(0.3072f,0.70281f,-9.81f,9.81f);
+
+// Filter
+float tau = 1.0f;
+IIR_filter LPF_gz(tau, Ts, 1.0f);
+IIR_filter LPF_ax(tau, Ts, 1.0f);
+IIR_filter LPF_ay(tau, Ts, 1.0f);
+
+// Tn unknown, Kp grätlet 3.0
+PI_Cntrl regler(3.0f, 0.1f, Ts, -0.4f, 0.4f);
+PI_Cntrl vel2zero(-0.2f, 4.0, Ts, -0.4f, 0.4f);
+
+
 // ----- User defined functions -----------
 void updateControllers(void);   // speed controller loop (via interrupt)
 // ------ END User defined functions ------
@@ -85,13 +102,44 @@
     short counts = counter1;            // get counts from Encoder
     float vel = diff(counts);           // motor velocity 
 
-    
+        // Motor still
+        //out = i2u(0.0f)/3.3f;
+        
+        // Motor geregelt
+        float value = regler(w_soll-vel);
+        
+        // /0.217f kein plan wieso aber het komisch tönt
+        //out.write(i2u(value/0.217f)/3.3f);
+        
+        //pc.printf("test \r\n");
+        
+        float gyro = (u2g(gz.read()));
+        
+        float phi_mes = (PI/4.0f) + atan2( -LPF_ax(u2a(ax.read())), LPF_ay(u2a(ay.read())) ) + LPF_gz(gyro);
+        
+        float Mu = vel2zero(0.0f - vel);
+        
+        // values are from matlab script init_cub1.m (variable K)
+        float torque = Mu - (-9.6910f * phi_mes -0.7119f * gyro);
+        
+        // versuch um geschwindigkeit die immer höher wird zu begrenzen... funktioniert nicht
+        /*if(vel >= 20) {
+            out.write(i2u(0)/3.3f);
+        } else {
+            out.write(i2u(torque/0.217f)/3.3f);
+        }*/
+        
+        out.write(i2u(torque/0.217f)/3.3f);
+        
+        
         if(++k >= 249){
         k = 0;
-        pc.printf("Some Output: %1.5f \r\n",PI);
+        //pc.printf("Velocity: %2.2f \r\n",vel);
+        //pc.printf("ax: %1.5f ay: %1.5f gz: %1.5f \r\n",u2a(ax.read()),u2a(ay.read()),u2g(gz.read()));
+        pc.printf("winkel: %2.5f torque: %2.5f Mu: %2.5f vel: %2.5f \r\n",phi_mes, torque, Mu, vel);
+        //pc.printf("Motor Still in A: %f \r\n",i2u(0.0f)/3.3f);
     }
 }
 //******************************************************************************
 //********** User functions like buttens handle etc. **************
 
-//...
\ No newline at end of file