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@10:600d7cf652e7, 2018-03-29 (annotated)

Committer:
altb
Date:
Thu Mar 29 19:15:31 2018 +0000
Revision:
10:600d7cf652e7
Parent:
9:30ee1d386a1d
Child:
11:ea8a91528ecf
Test GPA with 2nd order filter

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
altb 10:600d7cf652e7 19 PI_Cntrl PI1(1,.01);
pmic 5:d6c7ccbbce78 20 // controller parameters etc.
pmic 5:d6c7ccbbce78 21
altb 10:600d7cf652e7 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 10:600d7cf652e7 24 float A[2]={ -1.963052911280484,0.969116811217828};
altb 10:600d7cf652e7 25 IIR_filter pt2(B,A,sizeof(B)/4,sizeof(A)/4); // length of float is 4
altb 10:600d7cf652e7 26 GPA gpa1(1.0f, 200.0f,50, 5, 100, Ts, 1.0f,1.0f); // init GPA, see references there
pmic 5:d6c7ccbbce78 27 // user defined functions
rtlabor 0:15be70d21d7c 28 void updateControllers(void); // speed controller loop (via interrupt)
pmic 5:d6c7ccbbce78 29 void pressed(void); // user Button pressed
pmic 5:d6c7ccbbce78 30 void released(void); // user Button released
pmic 8:d68e177e2571 31 void printLine();
altb 10:600d7cf652e7 32 void output (float *);
altb 10:600d7cf652e7 33 unsigned int k = 0; // counter for serial output
altb 10:600d7cf652e7 34 // for GPA
altb 10:600d7cf652e7 35 float inp = 0.0f;
altb 10:600d7cf652e7 36 float out = 0.0f;
pmic 5:d6c7ccbbce78 37 // main program and control loop
pmic 5:d6c7ccbbce78 38 // -----------------------------------------------------------------------------
rtlabor 0:15be70d21d7c 39 int main()
rtlabor 0:15be70d21d7c 40 {
pmic 5:d6c7ccbbce78 41 // for serial comm.
pmic 5:d6c7ccbbce78 42 pc.baud(2000000);
pmic 5:d6c7ccbbce78 43 // attach controller loop to timer interrupt
rtlabor 0:15be70d21d7c 44 ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz;
rtlabor 0:15be70d21d7c 45 }
pmic 5:d6c7ccbbce78 46
pmic 5:d6c7ccbbce78 47 void updateControllers(void)
pmic 5:d6c7ccbbce78 48 {
altb 10:600d7cf652e7 49 inp = gpa1(inp,out);
altb 10:600d7cf652e7 50 out=pt2(inp); // Test GPA with pt2 system
pmic 5:d6c7ccbbce78 51 }
pmic 5:d6c7ccbbce78 52
altb 10:600d7cf652e7 53
altb 10:600d7cf652e7 54 // Buttonhandling
pmic 5:d6c7ccbbce78 55 // -----------------------------------------------------------------------------
pmic 5:d6c7ccbbce78 56 // start timer as soon as Button is pressed
pmic 5:d6c7ccbbce78 57 void pressed()
pmic 5:d6c7ccbbce78 58 {
pmic 5:d6c7ccbbce78 59 t.start();
pmic 5:d6c7ccbbce78 60 }
pmic 5:d6c7ccbbce78 61
pmic 5:d6c7ccbbce78 62 // evaluating statemachine
pmic 5:d6c7ccbbce78 63 void released()
pmic 5:d6c7ccbbce78 64 {
pmic 5:d6c7ccbbce78 65
pmic 5:d6c7ccbbce78 66 // readout, stop and reset timer
pmic 5:d6c7ccbbce78 67 float ButtonTime = t.read();
pmic 5:d6c7ccbbce78 68 t.stop();
pmic 5:d6c7ccbbce78 69 t.reset();
pmic 5:d6c7ccbbce78 70
pmic 5:d6c7ccbbce78 71 // if the cube doesStand
rtlabor 0:15be70d21d7c 72 }
pmic 8:d68e177e2571 73 void printLine()
pmic 8:d68e177e2571 74 {
pmic 8:d68e177e2571 75 printf("-----------------------------------------------------------------------------------------\r\n");
pmic 8:d68e177e2571 76 }