BA
sfbsg
Dependencies: mbed
Revision 0:8ab621116ccd, committed 2018-04-03
- Comitter:
- borlanic
- Date:
- Tue Apr 03 15:17:11 2018 +0000
- Commit message:
- fg
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/DiffCounter.cpp Tue Apr 03 15:17:11 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 15:17:11 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 15:17:11 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 15:17:11 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 15:17:11 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 15:17:11 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 15:17:11 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 15:17:11 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 15:17:11 2018 +0000
@@ -0,0 +1,29 @@
+#include "LinearCharacteristics.h"
+
+using namespace std;
+
+LinearCharacteristics::LinearCharacteristics(float k, float offset, float min, float max) // standard lin characteristics
+{
+ this -> k = k;
+ this -> offset = offset;
+ this -> min = min;
+ this -> max = max;
+// ...
+}
+
+LinearCharacteristics::~LinearCharacteristics(){}
+
+float LinearCharacteristics::trans(float input)
+{
+
+ output = k*input + offset;
+
+ if(output >= max) {
+ output = max;
+ }
+ if(output <= min) {
+ output = min;
+ }
+ return output;
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/LinearCharacteristics.h Tue Apr 03 15:17:11 2018 +0000
@@ -0,0 +1,34 @@
+#ifndef LINEAR_CHARACTERISTICS_H_
+#define LINEAR_CHARACTERISTICS_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+// Linear Characteristics for different purposes (map Voltage to acc etc.)
+class LinearCharacteristics
+{
+public:
+
+ LinearCharacteristics(float k, float offset, float min, float max);
+ float trans(float input);
+
+// float operator()(float input) {
+// return trans(input);
+// }
+ //...
+ //...
+ virtual ~LinearCharacteristics();
+ // here: the calculation function
+
+
+private:
+ // here: private functions and values...
+ float k;
+ float offset;
+ float output;
+ float min;
+ float max;
+
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Apr 03 15:17:11 2018 +0000
@@ -0,0 +1,34 @@
+#include "mbed.h"
+#include "math.h"
+//------------------------------------------
+#define PI 3.1415927f
+//------------------------------------------
+#include "EncoderCounter.h"
+#include "DiffCounter.h"
+#include "IIR_filter.h"
+#include "LinearCharacteristics.h"
+
+PwmOut pwm1(PA_5);
+
+float inc2rad = 2.0f*3.1415927f/(4.0f*6400.0);
+
+int main()
+{
+
+ EncoderCounter counter1(PB_6, PB_7);
+ DiffCounter diffc(0.9f,0.05f);
+
+
+ pwm1.period(0.00005f);
+
+ while(1) {
+ short inc = counter1.read();
+ float velocity = diffc(inc);
+ printf("angle = %.2f velocity = %.2f\r\n",inc*inc2rad,velocity);
+
+ pwm1.write(0.5f);
+ wait(0.05f);
+ }
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Apr 03 15:17:11 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/994bdf8177cb \ No newline at end of file