Revision 0:78ca29b4c49e, committed 2018-04-03

Comitter:

altb

Date:

Tue Apr 03 08:47:41 2018 +0000

Child:

1:a30512c3ac73

Commit message:

Cuboid Lab RT2 FS 2018

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DiffCounter.cpp	Tue Apr 03 08:47:41 2018 +0000
@@ -0,0 +1,35 @@
+#include "DiffCounter.h"
+
+using namespace std;
+/*      1/tau*(z-1)        
+G(z) = ------------
+         z - a0
+*/
+
+DiffCounter::DiffCounter(float time_constant, float SampleTime){
+    Ts = SampleTime;
+    alpha = 1/time_constant;            // scaling
+    a0 = -(1-Ts/time_constant);         // a0=-exp(-Ts/tau) ~= -(1-Ts/tau) 
+    inc_old = 0;                        // old values
+    v_old = 0;                          //   "  "
+    inc2rad = 2.0f*3.1415927f/(4.0f*6400.0);   // incr encoder with 6400inc/rev
+    }
+
+DiffCounter::~DiffCounter() {} 
+    
+void DiffCounter::reset(float initValue,short inc) {
+    v_old = initValue;
+    inc_old = inc;
+}
+
+float DiffCounter::doStep(short inc){
+    del = (long)(inc - inc_old);
+    if(del < -16000)
+        del += 0xFFFF;
+    if(del > 16000)
+        del -= 0xFFFF;
+    float vel = alpha * (float)del * inc2rad - a0 * v_old;
+    v_old = vel;
+    inc_old = inc;
+    return vel;
+    }
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DiffCounter.h	Tue Apr 03 08:47:41 2018 +0000
@@ -0,0 +1,28 @@
+/*  DiffCounter Class, differentiate encoder counts for cuboid based on LP filter
+    and unwrapping
+    
+        1/tau*(z-1)        
+G(z) = ------------
+         z - a0
+*/
+
+class DiffCounter{
+     public:
+                    DiffCounter(float a,float b);
+        float operator()(short inc){
+         return doStep(inc);
+         }
+        virtual     ~DiffCounter();
+        void        reset(float,short);
+        float       doStep(short inc);
+        float       Ts;
+    
+    private:
+    
+        float alpha;
+        float a0;
+        short inc_old;
+        float v_old;
+        long del;
+        float inc2rad;
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter.cpp	Tue Apr 03 08:47:41 2018 +0000
@@ -0,0 +1,139 @@
+/*
+ * EncoderCounter.cpp
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#include "EncoderCounter.h"
+
+using namespace std;
+
+/**
+ * Creates and initializes the driver to read the quadrature
+ * encoder counter of the STM32 microcontroller.
+ * @param a the input pin for the channel A.
+ * @param b the input pin for the channel B.
+ */
+EncoderCounter::EncoderCounter(PinName a, PinName b) {
+    
+    // check pins
+    
+    if ((a == PA_6) && (b == PC_7)) {
+        
+        // pinmap OK for TIM3 CH1 and CH2
+        
+        TIM = TIM3;
+        
+        // configure reset and clock control registers
+        
+        RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;    // manually enable port C (port A enabled by mbed library)
+        
+        // configure general purpose I/O registers
+        
+        GPIOA->MODER &= ~GPIO_MODER_MODER6;     // reset port A6
+        GPIOA->MODER |= GPIO_MODER_MODER6_1;    // set alternate mode of port A6
+        GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR6;     // reset pull-up/pull-down on port A6
+        GPIOA->PUPDR |= GPIO_PUPDR_PUPDR6_1;    // set input as pull-down
+        GPIOA->AFR[0] &= ~(0xF << 4*6);         // reset alternate function of port A6
+        GPIOA->AFR[0] |= 2 << 4*6;              // set alternate funtion 2 of port A6
+        
+        GPIOC->MODER &= ~GPIO_MODER_MODER7;     // reset port C7
+        GPIOC->MODER |= GPIO_MODER_MODER7_1;    // set alternate mode of port C7
+        GPIOC->PUPDR &= ~GPIO_PUPDR_PUPDR7;     // reset pull-up/pull-down on port C7
+        GPIOC->PUPDR |= GPIO_PUPDR_PUPDR7_1;    // set input as pull-down
+        GPIOC->AFR[0] &= ~0xF0000000;           // reset alternate function of port C7
+        GPIOC->AFR[0] |= 2 << 4*7;              // set alternate funtion 2 of port C7
+        
+        // configure reset and clock control registers
+        
+        RCC->APB1RSTR |= RCC_APB1RSTR_TIM3RST;  //reset TIM3 controller
+        RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM3RST;
+        
+        RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;     // TIM3 clock enable
+        
+    } else if ((a == PB_6) && (b == PB_7)) {
+        
+        // pinmap OK for TIM4 CH1 and CH2
+        
+        TIM = TIM4;
+        
+        // configure reset and clock control registers
+        
+        RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;    // manually enable port B (port A enabled by mbed library)
+        
+        // configure general purpose I/O registers
+        
+        GPIOB->MODER &= ~GPIO_MODER_MODER6;     // reset port B6
+        GPIOB->MODER |= GPIO_MODER_MODER6_1;    // set alternate mode of port B6
+        GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR6;     // reset pull-up/pull-down on port B6
+        GPIOB->PUPDR |= GPIO_PUPDR_PUPDR6_1;    // set input as pull-down
+        GPIOB->AFR[0] &= ~(0xF << 4*6);         // reset alternate function of port B6
+        GPIOB->AFR[0] |= 2 << 4*6;              // set alternate funtion 2 of port B6
+        
+        GPIOB->MODER &= ~GPIO_MODER_MODER7;     // reset port B7
+        GPIOB->MODER |= GPIO_MODER_MODER7_1;    // set alternate mode of port B7
+        GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR7;     // reset pull-up/pull-down on port B7
+        GPIOB->PUPDR |= GPIO_PUPDR_PUPDR7_1;    // set input as pull-down
+        GPIOB->AFR[0] &= ~0xF0000000;           // reset alternate function of port B7
+        GPIOB->AFR[0] |= 2 << 4*7;              // set alternate funtion 2 of port B7
+        
+        // configure reset and clock control registers
+        
+        RCC->APB1RSTR |= RCC_APB1RSTR_TIM4RST;  //reset TIM4 controller
+        RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM4RST;
+        
+        RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;     // TIM4 clock enable
+        
+    } else {
+        
+        printf("pinmap not found for peripheral\n");
+    }
+    
+    // configure general purpose timer 3 or 4
+    
+    TIM->CR1 = 0x0000;          // counter disable
+    TIM->CR2 = 0x0000;          // reset master mode selection
+    TIM->SMCR = TIM_SMCR_SMS_1 | TIM_SMCR_SMS_0; // counting on both TI1 & TI2 edges
+    TIM->CCMR1 = TIM_CCMR1_CC2S_0 | TIM_CCMR1_CC1S_0;
+    TIM->CCMR2 = 0x0000;        // reset capture mode register 2
+    TIM->CCER = TIM_CCER_CC2E | TIM_CCER_CC1E;
+    TIM->CNT = 0x0000;          // reset counter value
+    TIM->ARR = 0xFFFF;          // auto reload register
+    TIM->CR1 = TIM_CR1_CEN;     // counter enable
+}
+
+EncoderCounter::~EncoderCounter() {}
+
+/**
+ * Resets the counter value to zero.
+ */
+void EncoderCounter::reset() {
+    
+    TIM->CNT = 0x0000;
+}
+
+/**
+ * Resets the counter value to a given offset value.
+ * @param offset the offset value to reset the counter to.
+ */
+void EncoderCounter::reset(short offset) {
+    
+    TIM->CNT = -offset;
+}
+
+/**
+ * Reads the quadrature encoder counter value.
+ * @return the quadrature encoder counter as a signed 16-bit integer value.
+ */
+short EncoderCounter::read() {
+    
+    return (short)(-TIM->CNT);
+}
+
+/**
+ * The empty operator is a shorthand notation of the <code>read()</code> method.
+ */
+EncoderCounter::operator short() {
+    
+    return read();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter.h	Tue Apr 03 08:47:41 2018 +0000
@@ -0,0 +1,33 @@
+/*
+ * EncoderCounter.h
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef ENCODER_COUNTER_H_
+#define ENCODER_COUNTER_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+/**
+ * This class implements a driver to read the quadrature
+ * encoder counter of the STM32 microcontroller.
+ */
+class EncoderCounter {
+    
+    public:
+        
+                    EncoderCounter(PinName a, PinName b);
+        virtual     ~EncoderCounter();
+        void        reset();
+        void        reset(short offset);
+        short       read();
+                    operator short();
+        
+    private:
+        
+        TIM_TypeDef*    TIM;
+};
+
+#endif /* ENCODER_COUNTER_H_ */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GPA.cpp	Tue Apr 03 08:47:41 2018 +0000
@@ -0,0 +1,267 @@
+/*
+    GPA: frequency point wise gain and phase analyser to measure the frequency
+         respone of dynamical system
+    
+        hint:       the measurements should only be perfomed in closed loop
+        assumption: the system is at the desired steady state of interest when
+                    the measurment starts
+    
+                             exc(2)                 C: controller
+                               |                    P: plant
+                               v                     
+     exc(1) --> o   ->| C |--->o------->| P |----------> out
+                ^                  |             |
+                |                   --> inp      |  exc: excitation signal (E)
+                |                                |  inp: input plant (U)
+                 --------------------------------   out: output plant (Y)
+  
+    instantiate option 1:
+    GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
+    
+        fMin:     minimal desired frequency that should be measured in Hz
+        fMax:     maximal desired frequency that should be measured in Hz
+        NfexcDes: number of logarithmic equaly spaced frequency points
+        NperMin:  minimal number of periods that are used for each frequency point
+        NmeasMin: maximal number of samples that are used for each frequency point
+        Ts:       sampling time
+        Aexc0:    excitation amplitude at fMin 
+        Aexc1:    excitation amplitude at fMax
+    
+        hints:    the amplitude drops with 1/fexc, if you're using
+                  Axc1 = Aexc0/fMax the d/dt exc = const., this is recommended
+                  if your controller does not have a rolloff.
+                  if a desired frequency point is not measured try to increase Nmeas.
+    
+    pseudo code for a closed loop measurement with a proportional controller Kp:
+    
+        float inp = "measurement of inputsignal";
+        float out = "mesurement of outputsignal";
+        float exc = myGPA(inp, out);
+        float off = "desired steady state off the system";
+        
+        excitation input at (1):
+        inp = Kp*(exc + off - out);
+        
+        excitation input at (2):
+        inp = Kp*(off - out) + exc;
+        
+    usage:
+        exc = myGPA(inp, out) does update the internal states of the gpa at the
+        timestep k and returns the excitation signal for the timestep k+1. the 
+        results are plotted to a terminal (putty) over serial cennection and look
+        as follows:
+ -----------------------------------------------------------------------------------------
+   fexc[Hz]    |Gyu|     ang(Gyu)    |Gye|     ang(Gye)     |E|        |U|        |Y|
+ -----------------------------------------------------------------------------------------
+   7.01e-01   1.08e+01  -7.45e-02   1.08e+01  -7.38e-02   9.99e-01   9.99e-01   1.08e+01
+        
+    in matlab you can use:
+        dataNucleo = [... insert measurement data here ...];
+        g = frd(dataNucleo(:,2).*exp(1i*dataNucleo(:,3)), dataNucleo(:,1), Ts, 'Units', 'Hz');
+        gcl = frd(dataNucleo(:,4).*exp(1i*dataNucleo(:,5)), dataNucleo(:,1), Ts, 'Units', 'Hz');
+        
+    if you're evaluating more than one measurement which contain equal frequency points try:
+        dataNucleo = [dataNucleo1; dataNucleo2];
+        [~, ind] = unique(dataNucleo(:,1),'stable');
+        dataNucleo = dataNucleo(ind,:);
+         
+        autor: M.E. Peter
+*/ 
+
+#include "GPA.h"
+#include "mbed.h"
+#include "math.h"
+#define   pi 3.1415927f
+
+using namespace std;
+
+GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
+{
+    this->NfexcDes = NfexcDes;
+    this->NperMin = NperMin;
+    this->NmeasMin = NmeasMin;
+    this->Ts = Ts;
+
+    // calculate logarithmic spaced frequency points
+    fexcDes = (float*)malloc(NfexcDes*sizeof(float));
+    fexcDesLogspace(fMin, fMax, NfexcDes);
+
+    // calculate coefficients for decreasing amplitude (1/fexc)
+    this->aAexcDes = (Aexc1 - Aexc0)/(1.0f/fexcDes[NfexcDes-1] - 1.0f/fexcDes[0]);
+    this->bAexcDes = Aexc0 - aAexcDes/fexcDes[0];
+
+    fnyq = 1/2.0f/Ts;
+    pi2 = 2.0f*pi;
+    pi2Ts = pi2*Ts;
+    piDiv2 = pi/2.0f;
+
+    sU = (float*)malloc(3*sizeof(float));
+    sY = (float*)malloc(3*sizeof(float));
+    reset();
+}
+
+GPA::~GPA() {}
+
+void GPA::reset()
+{
+    Nmeas = 0;
+    Nper = 0;
+    fexc = 0.0f;
+    fexcPast = 0.0f;
+    ii = 1; // iterating through desired frequency points
+    jj = 1; // iterating through measurement points w.r.t. reachable frequency
+    scaleG = 0.0f;
+    cr = 0.0f;
+    ci = 0.0f;
+    for(int i = 0; i < 3; i++) {
+        sU[i] = 0.0f;
+        sY[i] = 0.0f;
+    }
+    sinarg = 0.0f;
+    NmeasTotal = 0;
+    Aexc = 0.0f;
+    pi2Tsfexc = 0.0f;
+}
+
+float GPA::update(float inp, float out)
+{
+    // a new frequency point has been reached
+    if(jj == 1) {
+        // get a new unique frequency point
+        while(fexc == fexcPast) {
+            // measurement finished
+            if(ii > NfexcDes) {
+                return 0.0f;
+            }
+            calcGPAmeasPara(fexcDes[ii - 1]);
+            // secure fexc is not higher or equal to nyquist frequency
+            if(fexc >= fnyq) {
+                fexc = fexcPast;
+            }
+            // no frequency found
+            if(fexc == fexcPast) {
+                ii += 1;
+            } else {
+                Aexc = aAexcDes/fexc + bAexcDes;
+                pi2Tsfexc = pi2Ts*fexc;
+            }
+        }
+        fexcPast = fexc;
+        // filter scaling
+        scaleG = 1.0f/sqrt((float)Nmeas);
+        // filter coefficients
+        cr = cos(pi2Tsfexc);
+        ci = sin(pi2Tsfexc);
+        // filter storage
+        for(int i = 0; i < 3; i++) {
+            sU[i] = 0.0f;
+            sY[i] = 0.0f;
+        }
+    }
+    // filter step for signal su
+    sU[0] = scaleG*inp + 2.0f*cr*sU[1] - sU[2];
+    sU[2] = sU[1];
+    sU[1] = sU[0];
+    // filter step for signal sy
+    sY[0] = scaleG*out + 2.0f*cr*sY[1] - sY[2];
+    sY[2] = sY[1];
+    sY[1] = sY[0];
+    // measurement of frequencypoint is finished
+    if(jj == Nmeas) {
+        jj = 1;
+        ii += 1;
+        // calculate the one point dft
+        float Ureal = 2.0f*scaleG*(cr*sU[1] - sU[2]);
+        float Uimag = 2.0f*scaleG*ci*sU[1];
+        float Yreal = 2.0f*scaleG*(cr*sY[1] - sY[2]);
+        float Yimag = 2.0f*scaleG*ci*sY[1];
+        // calculate magnitude and angle
+        float Umag = sqrt(Ureal*Ureal + Uimag*Uimag);
+        float Ymag = sqrt(Yreal*Yreal + Yimag*Yimag);
+        float absGyu = Ymag/Umag;
+        float angGyu = atan2(Yimag, Yreal) - atan2(Uimag, Ureal);
+        float absGye = Ymag/Aexc;
+        float angGye = (atan2(Yimag, Yreal) + piDiv2);
+        // user info
+        if(ii == 1) {
+            printLine();
+            printf("   fexc[Hz]    |Gyu|     ang(Gyu)    |Gye|     ang(Gye)     |E|        |U|        |Y|\r\n");
+            printLine();
+        }
+        printf("%11.2e %10.2e %10.2e %10.2e %10.2e %10.2e %10.2e %10.2e\r\n", fexc, absGyu, angGyu, absGye, angGye, Aexc, Umag, Ymag);
+    } else {
+        jj += 1;
+    }
+    sinarg = fmod(sinarg + pi2Tsfexc, pi2);
+    NmeasTotal += 1;
+    return Aexc*sin(sinarg);
+}
+
+void GPA::fexcDesLogspace(float fMin, float fMax, int NfexcDes)
+{
+    // calculate logarithmic spaced frequency points
+    float Gain = log10(fMax/fMin)/((float)NfexcDes - 1.0f);
+    float expon = 0.0f;
+    for(int i = 0; i < NfexcDes; i++) {
+        fexcDes[i] = fMin*pow(10.0f, expon);
+        expon += Gain;
+    }
+}
+
+void GPA::calcGPAmeasPara(float fexcDes_i)
+{
+    // Nmeas has to be an integer
+    Nper = NperMin;
+    Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f);
+    //  secure that the minimal number of measurements is fullfilled
+    int Ndelta = NmeasMin - Nmeas;
+    if(Ndelta > 0) {
+        Nper = (int)ceil((float)NmeasMin*fexcDes_i*Ts);
+        Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f);
+    }
+    // evaluating reachable frequency
+    fexc = (float)Nper/(float)Nmeas/Ts;
+}
+
+void GPA::printLine()
+{
+    printf("-----------------------------------------------------------------------------------------\r\n");
+}
+
+void GPA::printGPAfexcDes()
+{
+    printLine();
+    for(int i = 0; i < NfexcDes; i++) {
+        printf("%9.4f\r\n", fexcDes[i]);
+    }
+}
+
+void GPA::printGPAmeasPara()
+{
+    printLine();
+    printf(" fexcDes[Hz]  fexc[Hz]       Aexc   Nmeas   Nper\r\n");
+    printLine();
+    for(int i = 0; i < NfexcDes; i++) {
+        calcGPAmeasPara(fexcDes[i]);
+        if(fexc == fexcPast || fexc >= fnyq) {
+            fexc = 0.0f;
+            Nmeas = 0;
+            Nper = 0;
+            Aexc = 0;
+        } else {
+            Aexc = aAexcDes/fexc + bAexcDes;
+            fexcPast = fexc;
+        }
+        NmeasTotal += Nmeas;
+        printf("%12.2e %9.2e %10.2e %7i %6i \r\n", fexcDes[i], fexc, Aexc, Nmeas, Nper);
+    }
+    printGPAmeasTime();
+    reset();
+}
+
+void GPA::printGPAmeasTime()
+{
+    printLine();
+    printf(" number of data points:  %9i\r\n", NmeasTotal);
+    printf(" measurment time in sec: %9.2f\r\n", (float)NmeasTotal*Ts);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GPA.h	Tue Apr 03 08:47:41 2018 +0000
@@ -0,0 +1,55 @@
+class GPA
+{
+public:
+
+    GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1);
+
+    float operator()(float inp, float out) {
+        return update(inp, out);
+    }
+
+    virtual ~GPA();
+
+    void     reset();
+    float    update(float inp, float out);
+
+    void     printGPAfexcDes();
+    void     printGPAmeasPara();
+    void     printGPAmeasTime();
+
+private:
+
+    int    NfexcDes;
+    int    NperMin;
+    int    NmeasMin;
+    float  Ts;
+    float *fexcDes;
+    float  aAexcDes;
+    float  bAexcDes;
+
+    float  fnyq;
+    float  pi2;
+    float  pi2Ts;
+    float  piDiv2;
+
+    int    Nmeas;
+    int    Nper;
+    float  fexc;
+    float  fexcPast;
+    int    ii;
+    int    jj;
+    float  scaleG;
+    float  cr;
+    float  ci;
+    float *sU;
+    float *sY;
+    float  sinarg;
+    int    NmeasTotal;
+    float  Aexc;
+    float  pi2Tsfexc;
+
+    void   fexcDesLogspace(float fMin, float fMax, int NfexcDes);
+    void   calcGPAmeasPara(float fexcDes_i);
+    void   printLine();
+
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IIR_filter.cpp	Tue Apr 03 08:47:41 2018 +0000
@@ -0,0 +1,171 @@
+#include "IIR_filter.h"
+#include "mbed.h"
+using namespace std;
+
+/*
+  IIR filter implemention for the following filter types:
+  init for: first order differentiatior:   G(s) = s/(T*s + 1)
+            first order lowpass with gain  G(s) = K/(T*s + 1)
+            second order lowpass with gain G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0*w0)        
+            nth order, with arbitrary values
+  the billinear transformation is used for s -> z
+  reseting the filter only makes sence for static signals, whatch out if you're using the differnetiator
+*/
+
+// G(s) = s/(T*s + 1)
+IIR_filter::IIR_filter(float T,float Ts){
+           
+    // filter orders
+    nb = 1; // Filter Order
+    na = 1; // Filter Order
+    
+    // filter coefficients
+    B = (float*)malloc((nb+1)*sizeof(float));
+    A = (float*)malloc(na*sizeof(float));    
+    B[0] = 2.0f/(2.0f*T + Ts);
+    B[1] = -B[0];
+    A[0] = -(2.0f*T - Ts)/(2.0f*T + Ts);
+    
+    // signal arrays
+    uk = (float*)malloc((nb+1)*sizeof(float));
+    yk = (float*)malloc(na*sizeof(float));
+    uk[0]= uk[1] = 0.0f;
+    yk[0] = 0.0f;
+    
+    // dc-gain
+    this->K = 0.0f;
+}
+
+// G(s) = K/(T*s + 1)
+IIR_filter::IIR_filter(float T,float Ts,float K){
+    
+    // filter orders
+    nb = 1; // Filter Order
+    na = 1; // Filter Order
+    
+    // filter coefficients
+    B = (float*)malloc((nb+1)*sizeof(float));
+    A = (float*)malloc(na*sizeof(float));      
+    B[0] = Ts/(Ts + 2.0f*T);
+    B[1] = B[0];
+    A[0] = (Ts - 2.0f*T)/(Ts + 2.0f*T); 
+    
+    // signal arrays
+    uk = (float*)malloc((nb+1)*sizeof(float));
+    yk = (float*)malloc(na*sizeof(float));
+    uk[0]= uk[1] = 0.0f;
+    yk[0] = 0.0f;
+    
+    // dc-gain
+    this->K = K;
+}
+
+// G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0^2) 
+IIR_filter::IIR_filter(float w0,float D, float Ts, float K){
+    
+    // filter orders
+    nb = 2; // Filter Order
+    na = 2; // Filter Order
+    
+    // filter coefficients
+    B = (float*)malloc((nb+1)*sizeof(float));
+    A = (float*)malloc(na*sizeof(float));
+    float k0 = Ts*Ts*w0*w0;
+    float k1 = 4.0f*D*Ts*w0;
+    float k2 = k0 + k1 + 4.0f;    
+    B[0] = K*k0/k2;
+    B[1] = 2.0f*B[0];
+    B[2] = B[0]; 
+    A[0] = (2.0f*k0 - 8.0f)/k2;
+    A[1] = (k0 - k1 + 4.0f)/k2;
+    
+    // signal arrays
+    uk = (float*)malloc((nb+1)*sizeof(float));
+    yk = (float*)malloc(na*sizeof(float));
+    uk[0]= uk[1] = uk[2] = 0.0f;
+    yk[0] = yk[1] = 0.0f;
+    
+    // dc-gain
+    this->K = K;
+}
+
+IIR_filter::IIR_filter(float *b,float *a,int nb_, int na_){
+    
+    // filter orders
+    this->nb = nb_-1;    // Filter Order
+    this->na = na_;      // Filter Order
+    
+    // filter coefficients
+    B = (float*)malloc((nb+1)*sizeof(float));
+    A = (float*)malloc(na*sizeof(float));
+    uk = (float*)malloc((nb+1)*sizeof(float));
+    yk = (float*)malloc(na*sizeof(float));
+    
+    for(int k=0;k<=nb;k++){
+        B[k]=b[k];
+        uk[k]=0.0f;
+        }
+    for(int k=0;k<na;k++){
+        A[k] = a[k];
+        yk[k] = 0.0f;
+        }
+    //B[0] = K*k0/k2;
+    //B[1] = 2.0f*B[0];
+    //B[2] = B[0]; 
+    //A[0] = (2.0f*k0 - 8.0f)/k2;
+    //A[1] = (k0 - k1 + 4.0f)/k2;
+    
+    // dc-gain
+    this->K = 1.0f;
+}
+
+    
+IIR_filter::~IIR_filter() {} 
+    
+void IIR_filter::reset(float val) {
+    for(int k=0;k < nb;k++)
+        uk[k] = val;
+    for(int k=0;k < na;k++)
+        yk[k] = val*K;
+        
+}
+
+/* 
+    the filter is operating as follows: 
+    (B[0] + B[1]*z^-1 + ... + B[nb]*z^-nb)*U(z) = (1 + A[0]*z^-1 + ... + A[na-1]*z^-na))*Y(z)
+    y(n) =  B[0]*u(k)   + B[1]*u(k-1) + ... + B[nb]*u(k-nb) + ...
+          - A[0]*y(k-1) - A[1]*y(k-2) - ... - A[na]*y(n-na)
+*/
+float IIR_filter::filter(float input){
+    for(int k = nb;k > 0;k--)    // shift input values back
+        uk[k] = uk[k-1];
+    uk[0] = input;
+    float ret = 0.0f;
+    for(int k = 0;k <= nb;k++)
+        ret += B[k] * uk[k];
+    for(int k = 0;k < na;k++)
+        ret -= A[k] * yk[k];
+    for(int k = na;k > 1;k--)
+        yk[k-1] = yk[k-2];
+    yk[0] = ret;
+    return ret;
+}
+
+// (B[0] + B[1]*z^-1 + ... + B[nb]*z^-nb)*U(z) = (1 + A[0]*z^-1 + ... + A[na-1]*z^-na))*Y(z)
+/*
+IIR_filter::IIR_filter(float *a[], float *b[], float K){
+    
+    this->A = A[0];
+    this->B = B[0];   
+    nb = sizeof(B)/sizeof(B[0]);
+    na = sizeof(A)/sizeof(A[0]);
+    uk = (float*)malloc((nb+1)*sizeof(float));
+    yk = (float*)malloc(na*sizeof(float));
+    for(int ii=0; ii<nb; ii++){
+        uk[ii] = 0.0f;
+    }
+    for(int ii=0; ii<na; ii++){
+        yk[ii] = 0.0f;
+    }
+    this->K = K;       %%% THIS IMPLEMENTATION SUITS NOT THE RESET PROCESS %%%
+}*/
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IIR_filter.h	Tue Apr 03 08:47:41 2018 +0000
@@ -0,0 +1,26 @@
+class IIR_filter{
+     public:
+     
+        //IIR_filter(float *A[],float *B[]);       
+        IIR_filter(float T,float Ts);
+        IIR_filter(float T,float Ts,float K);
+        IIR_filter(float w0,float D, float Ts, float K);
+        IIR_filter(float *,float *, int,int);
+                    
+        float operator()(float u){
+            return filter(u);
+         }
+        virtual     ~IIR_filter();
+        void        reset(float);
+        float       filter(float);
+    
+    private:
+
+        unsigned int nb;
+        unsigned int na;
+        float *B;
+        float *A;
+        float *uk;
+        float *yk;
+        float K;
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LinearCharacteristics.cpp	Tue Apr 03 08:47:41 2018 +0000
@@ -0,0 +1,8 @@
+#include "LinearCharacteristics.h"
+
+using namespace std;
+
+LinearCharacteristics::LinearCharacteristics(float k, float offset){    // standard lin characteristics
+
+// ...
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LinearCharacteristics.h	Tue Apr 03 08:47:41 2018 +0000
@@ -0,0 +1,11 @@
+// Linear Characteristics for different purposes (map Voltage to acc etc.)
+class LinearCharacteristics{
+     public:
+                    //...
+                    //...
+        virtual     ~LinearCharacteristics();
+        // here: the calculation function
+    
+    private:
+        // here: private functions and values...
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PI_Cntrl.cpp	Tue Apr 03 08:47:41 2018 +0000
@@ -0,0 +1,31 @@
+#include "PI_Cntrl.h"
+
+using namespace std;
+
+PI_Cntrl::PI_Cntrl(float Kp, float Tn){
+    this->Kp = Kp;
+    this->Tn = Tn_;
+    
+    //...
+    
+    
+    }
+
+
+PI_Cntrl::~PI_Cntrl() {} 
+    
+void PI_Cntrl::reset(float initValue) {
+//...
+
+}
+
+float PI_Cntrl::doStep(float error){
+    
+    // ...
+    
+    // -------- Anti-Windup -------- 
+    // ...
+    // -------- Timeshift --------
+    
+    
+    }
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PI_Cntrl.h	Tue Apr 03 08:47:41 2018 +0000
@@ -0,0 +1,11 @@
+class PI_Cntrl{
+     public:
+
+// ....    
+
+
+    private:
+    
+    // ....
+    
+    };
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 03 08:47:41 2018 +0000
@@ -0,0 +1,96 @@
+#include "mbed.h"
+#include "math.h"
+//------------------------------------------
+#define PI 3.1415927f
+//------------------------------------------
+#include "EncoderCounter.h"
+#include "DiffCounter.h"
+#include "IIR_filter.h"
+#include "LinearCharacteristics.h"
+#include "PI_Cntrl.h"
+/* Cuboid balance on one edge on Nucleo F446RE
+
+ **** IMPORTANT: use ..\Labormodelle\RT-MOD054 - Würfel\Escon_Parameter_4nucleo.edc 
+                 settings for Maxon ESCON controller (upload via ESCON Studio) ****
+hardware Connections:
+ 
+ CN7                    CN10
+  :                         :
+  :                         :
+ ..                        ..
+ ..                        ..                  15.
+ ..    AOUT i_des on (PA_5)o.
+ ..                        ..
+ ..                        ..
+ ..               ENC CH A o.
+ o. GND                    ..                  10.
+ o. ENC CH B               ..
+ ..                        ..
+ ..                        ..
+ .o AIN acx (PA_0)         ..
+ .o AIN acy (PA_1)         ..                  5.
+ .o AIN Gyro(PA_4)         .o Analog GND
+ ..                        ..
+ ..                        ..
+ ..                        ..                  1.
+ ----------------------------
+ CN7               CN10
+ */
+Serial pc(SERIAL_TX, SERIAL_RX);        // serial connection via USB - programmer
+InterruptIn button(USER_BUTTON);        // User Button, short presses: reduce speed, long presses: increase speed
+AnalogIn ax(PA_0);                      // Analog IN (acc x) on PA_0
+AnalogIn ay(PA_1);                      // Analog IN (acc y) on PA_1
+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
+//------------------------------------------
+// ... here define variables like gains etc.
+//------------------------------------------
+
+//------------------------------------------
+Ticker  ControllerLoopTimer;            // interrupt for control loop
+EncoderCounter counter1(PB_6, PB_7);    // initialize counter on PB_6 and PB_7
+DiffCounter diff(0.01,Ts);              // discrete differentiate, based on encoder data
+//------------------------------------------
+// ... here define instantiate classes
+//------------------------------------------
+
+// ... define some linear characteristics -----------------------------------------
+
+// ----- User defined functions -----------
+void updateControllers(void);   // speed controller loop (via interrupt)
+// ------ END User defined functions ------
+
+//******************************************************************************
+//---------- main loop -------------
+//******************************************************************************
+int main()
+{
+    //attach controller loop to timer interrupt
+    pc.baud(2000000);   // for serial comm.
+    counter1.reset();   // encoder reset
+    diff.reset(0.0f,0.0f);
+    ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = ...;
+}
+//******************************************************************************
+//---------- control loop (called via interrupt) -------------
+//******************************************************************************
+void updateControllers(void){
+    short counts = counter1;            // get counts from Encoder
+    float vel = diff(counts);           // motor velocity 
+    
+    // ... your code 
+    
+    if(++k >= 199){
+        k = 0;
+        pc.printf("omega_soll=%3.2f omega=%3.2f \r\n",w_soll,vel);
+    }
+}
+//******************************************************************************
+//********** User functions like buttens handle etc. **************
+//******************************************************************************
+// pressed button
+//******************************************************************************
+
+//...
\ No newline at end of file