Test of pmic GPA with filter
Dependencies: mbed
Fork of nucf446-cuboid-balance1_strong by
main.cpp
- Committer:
- pmic
- Date:
- 2018-04-10
- Revision:
- 26:492c7ab05e67
- Parent:
- 22:715d351d0be7
File content as of revision 26:492c7ab05e67:
#include "mbed.h" #include "math.h" #define pi 3.1415927f #include "PI_Cntrl.h" #include "IIR_filter.h" #include "LinearCharacteristics.h" #include "GPA.h" // Software Test for nucleo GPA. GPA pointwise GPA made bei pmic March 2018 // ----------------------------------------------------------------------------- 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 Ticker ControllerLoopTimer; // interrupt for control loop Timer t; // timer to analyse Button float Ts = 0.0025; // sample time PI_Cntrl PI1(1,.01,Ts); // controller parameters etc. /* float B[3] ={1.515974984336e-3,3.031949968672e-3,1.515974984336e-3}; // 2nd order System low Damping, Ts =1/400 // MATLAB: w0=5*2*pi;D=.2;Ts=1/400;G=tf(w0^2,[1 2*D*w0 w0^2]);Gd=c2d(G,Ts,'tustin');format long;[n,d]=tfdata(Gd,'v') float A[2]={ -1.963052911280484,0.969116811217828}; IIR_filter pt2(B,A,sizeof(B)/4,sizeof(A)/4); // length of float is 4 */ float w0 = 2.0f*pi*5.0f; float D = 0.2f; IIR_filter pt2(w0, D, Ts, 1.0f); // GPA gpa1(1.0f, 200.0f, 50, 5, 100, Ts, 1.0f, 1.0f); // init GPA, see references there float fexcDes[5] = {10.0f, 20.0f, 30.0f, 40.0f, 50.0f}; // GPA gpa1(1.0f, 200.0f, fexcDes, sizeof(fexcDes)/sizeof(fexcDes[0]), 5, 100, Ts, 1.0f, 1.0f); GPA gpa1(fexcDes, sizeof(fexcDes)/sizeof(fexcDes[0]), 5, 100, Ts, 1.0f, 1.0f); // user defined functions void updateControllers(void); // speed controller loop (via interrupt) void pressed(void); // user Button pressed void released(void); // user Button released void printLine(); void output (float *); unsigned int k = 0; // counter for serial output // for GPA float inp = 0.0f; float out = 0.0f; // main program and control loop // ----------------------------------------------------------------------------- int main() { // for serial comm. pc.baud(2000000); gpa1.reset(); pt2.reset(0.0f); gpa1.printGPAmeasPara(); /* gpa1.printNfexcDes(); printLine(); for(int i = 0; i < 5; i++) { printf(" frequancy points: %9.3f\r\n", fexcDes[i]); } printLine(); printf(" number of frequancy points: %2i\r\n", sizeof(fexcDes)/sizeof(fexcDes[0])); */ // attach controller loop to timer interrupt ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz; } void updateControllers(void) { inp = gpa1(inp,out); out = pt2(inp); // Test GPA with pt2 system // u = Wobble(u, y) + Oexc; // y = SysSecOrder(u); // desTorque = pi_w(omega_desired - omega + excWobble); // inpWobble = desTorque; // outWobble = omega; // excWobble = Wobble(excWobble, outWobble); } // Buttonhandling // ----------------------------------------------------------------------------- // start timer as soon as Button is pressed void pressed() { t.start(); } // evaluating statemachine void released() { // readout, stop and reset timer float ButtonTime = t.read(); t.stop(); t.reset(); // if the cube doesStand } void printLine() { printf("-----------------------------------------------------------------------------------------\r\n"); }