RT2_Cuboid_demo / Mbed 2 deprecated nucf446-test_GPA

Test of pmic GPA with filter

Dependencies:   mbed

Fork of nucf446-cuboid-balance1_strong by RT2_Cuboid_demo

main.cpp@26:492c7ab05e67, 2018-04-10 (annotated)

Committer:
pmic
Date:
Tue Apr 10 12:24:57 2018 +0000
Revision:
26:492c7ab05e67
Parent:
22:715d351d0be7 
correct new functinality

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rtlabor 0:15be70d21d7c 1 #include "mbed.h"
rtlabor 0:15be70d21d7c 2 #include "math.h"
pmic 5:d6c7ccbbce78 3 #define pi 3.1415927f
pmic 5:d6c7ccbbce78 4
rtlabor 0:15be70d21d7c 5 #include "PI_Cntrl.h"
rtlabor 0:15be70d21d7c 6 #include "IIR_filter.h"
rtlabor 0:15be70d21d7c 7 #include "LinearCharacteristics.h"
altb 10:600d7cf652e7 8 #include "GPA.h"
pmic 5:d6c7ccbbce78 9
altb 10:600d7cf652e7 10 // Software Test for nucleo GPA. GPA pointwise GPA made bei pmic March 2018
pmic 5:d6c7ccbbce78 11 // -----------------------------------------------------------------------------
altb 3:a951d699878b 12
pmic 5:d6c7ccbbce78 13
pmic 5:d6c7ccbbce78 14 Serial pc(SERIAL_TX, SERIAL_RX); // serial connection via USB - programmer
pmic 5:d6c7ccbbce78 15 InterruptIn Button(USER_BUTTON); // User Button, short presses: reduce speed, long presses: increase speed
pmic 5:d6c7ccbbce78 16 Ticker ControllerLoopTimer; // interrupt for control loop
pmic 5:d6c7ccbbce78 17 Timer t; // timer to analyse Button
altb 10:600d7cf652e7 18 float Ts = 0.0025; // sample time
pmic 12:93fd84766578 19 PI_Cntrl PI1(1,.01,Ts);
pmic 5:d6c7ccbbce78 20 // controller parameters etc.
pmic 5:d6c7ccbbce78 21
pmic 22:715d351d0be7 22 /*
altb 10:600d7cf652e7 23 float B[3] ={1.515974984336e-3,3.031949968672e-3,1.515974984336e-3}; // 2nd order System low Damping, Ts =1/400
altb 11:ea8a91528ecf 24 // 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')
altb 10:600d7cf652e7 25 float A[2]={ -1.963052911280484,0.969116811217828};
altb 10:600d7cf652e7 26 IIR_filter pt2(B,A,sizeof(B)/4,sizeof(A)/4); // length of float is 4
pmic 22:715d351d0be7 27 */
pmic 22:715d351d0be7 28
pmic 22:715d351d0be7 29 float w0 = 2.0f*pi*5.0f;
pmic 22:715d351d0be7 30 float D = 0.2f;
pmic 22:715d351d0be7 31 IIR_filter pt2(w0, D, Ts, 1.0f);
pmic 22:715d351d0be7 32
pmic 26:492c7ab05e67 33 // GPA gpa1(1.0f, 200.0f, 50, 5, 100, Ts, 1.0f, 1.0f); // init GPA, see references there
pmic 26:492c7ab05e67 34 float fexcDes[5] = {10.0f, 20.0f, 30.0f, 40.0f, 50.0f};
pmic 26:492c7ab05e67 35 // GPA gpa1(1.0f, 200.0f, fexcDes, sizeof(fexcDes)/sizeof(fexcDes[0]), 5, 100, Ts, 1.0f, 1.0f);
pmic 26:492c7ab05e67 36 GPA gpa1(fexcDes, sizeof(fexcDes)/sizeof(fexcDes[0]), 5, 100, Ts, 1.0f, 1.0f);
pmic 5:d6c7ccbbce78 37 // user defined functions
rtlabor 0:15be70d21d7c 38 void updateControllers(void); // speed controller loop (via interrupt)
pmic 5:d6c7ccbbce78 39 void pressed(void); // user Button pressed
pmic 5:d6c7ccbbce78 40 void released(void); // user Button released
pmic 8:d68e177e2571 41 void printLine();
altb 10:600d7cf652e7 42 void output (float *);
altb 10:600d7cf652e7 43 unsigned int k = 0; // counter for serial output
altb 10:600d7cf652e7 44 // for GPA
altb 10:600d7cf652e7 45 float inp = 0.0f;
altb 10:600d7cf652e7 46 float out = 0.0f;
pmic 5:d6c7ccbbce78 47 // main program and control loop
pmic 5:d6c7ccbbce78 48 // -----------------------------------------------------------------------------
rtlabor 0:15be70d21d7c 49 int main()
pmic 22:715d351d0be7 50 {
pmic 5:d6c7ccbbce78 51 // for serial comm.
pmic 5:d6c7ccbbce78 52 pc.baud(2000000);
pmic 22:715d351d0be7 53
pmic 22:715d351d0be7 54 gpa1.reset();
pmic 22:715d351d0be7 55 pt2.reset(0.0f);
pmic 22:715d351d0be7 56
pmic 26:492c7ab05e67 57 gpa1.printGPAmeasPara();
pmic 26:492c7ab05e67 58 /*
pmic 26:492c7ab05e67 59 gpa1.printNfexcDes();
pmic 26:492c7ab05e67 60 printLine();
pmic 26:492c7ab05e67 61 for(int i = 0; i < 5; i++) {
pmic 26:492c7ab05e67 62 printf(" frequancy points: %9.3f\r\n", fexcDes[i]);
pmic 26:492c7ab05e67 63 }
pmic 26:492c7ab05e67 64 printLine();
pmic 26:492c7ab05e67 65 printf(" number of frequancy points: %2i\r\n", sizeof(fexcDes)/sizeof(fexcDes[0]));
pmic 26:492c7ab05e67 66 */
pmic 26:492c7ab05e67 67
pmic 5:d6c7ccbbce78 68 // attach controller loop to timer interrupt
rtlabor 0:15be70d21d7c 69 ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz;
rtlabor 0:15be70d21d7c 70 }
pmic 5:d6c7ccbbce78 71
pmic 5:d6c7ccbbce78 72 void updateControllers(void)
pmic 22:715d351d0be7 73 {
altb 10:600d7cf652e7 74 inp = gpa1(inp,out);
pmic 22:715d351d0be7 75 out = pt2(inp); // Test GPA with pt2 system
pmic 22:715d351d0be7 76
pmic 22:715d351d0be7 77 // u = Wobble(u, y) + Oexc;
pmic 22:715d351d0be7 78 // y = SysSecOrder(u);
pmic 22:715d351d0be7 79
pmic 22:715d351d0be7 80 // desTorque = pi_w(omega_desired - omega + excWobble);
pmic 22:715d351d0be7 81 // inpWobble = desTorque;
pmic 22:715d351d0be7 82 // outWobble = omega;
pmic 22:715d351d0be7 83 // excWobble = Wobble(excWobble, outWobble);
pmic 22:715d351d0be7 84
pmic 5:d6c7ccbbce78 85 }
pmic 5:d6c7ccbbce78 86
altb 10:600d7cf652e7 87
altb 10:600d7cf652e7 88 // Buttonhandling
pmic 5:d6c7ccbbce78 89 // -----------------------------------------------------------------------------
pmic 5:d6c7ccbbce78 90 // start timer as soon as Button is pressed
pmic 5:d6c7ccbbce78 91 void pressed()
pmic 5:d6c7ccbbce78 92 {
pmic 5:d6c7ccbbce78 93 t.start();
pmic 5:d6c7ccbbce78 94 }
pmic 5:d6c7ccbbce78 95
pmic 5:d6c7ccbbce78 96 // evaluating statemachine
pmic 5:d6c7ccbbce78 97 void released()
pmic 5:d6c7ccbbce78 98 {
pmic 5:d6c7ccbbce78 99
pmic 5:d6c7ccbbce78 100 // readout, stop and reset timer
pmic 5:d6c7ccbbce78 101 float ButtonTime = t.read();
pmic 5:d6c7ccbbce78 102 t.stop();
pmic 5:d6c7ccbbce78 103 t.reset();
pmic 5:d6c7ccbbce78 104
pmic 5:d6c7ccbbce78 105 // if the cube doesStand
rtlabor 0:15be70d21d7c 106 }
pmic 8:d68e177e2571 107 void printLine()
pmic 8:d68e177e2571 108 {
pmic 8:d68e177e2571 109 printf("-----------------------------------------------------------------------------------------\r\n");
pmic 8:d68e177e2571 110 }