Test of pmic GPA with filter
Dependencies: mbed
Fork of nucf446-cuboid-balance1_strong by
main.cpp
- Committer:
- pmic
- Date:
- 2018-04-09
- Revision:
- 22:715d351d0be7
- Parent:
- 12:93fd84766578
- Child:
- 26:492c7ab05e67
File content as of revision 22:715d351d0be7:
#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 // 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); // 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"); }