RT2_Cuboid_demo / Mbed 2 deprecated nucf446-test_GPA

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of nucf446-cuboid-balance1_strong by RT2_Cuboid_demo

main.cpp@22:715d351d0be7, 2018-04-09 (annotated)

Committer:
pmic
Date:
Mon Apr 09 14:48:55 2018 +0000
Revision:
22:715d351d0be7
Parent:
12:93fd84766578
Child:
26:492c7ab05e67
correct simulation

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
altb 10:600d7cf652e7 33 GPA gpa1(1.0f, 200.0f,50, 5, 100, Ts, 1.0f,1.0f); // init GPA, see references there
pmic 5:d6c7ccbbce78 34 // user defined functions
rtlabor 0:15be70d21d7c 35 void updateControllers(void); // speed controller loop (via interrupt)
pmic 5:d6c7ccbbce78 36 void pressed(void); // user Button pressed
pmic 5:d6c7ccbbce78 37 void released(void); // user Button released
pmic 8:d68e177e2571 38 void printLine();
altb 10:600d7cf652e7 39 void output (float *);
altb 10:600d7cf652e7 40 unsigned int k = 0; // counter for serial output
altb 10:600d7cf652e7 41 // for GPA
altb 10:600d7cf652e7 42 float inp = 0.0f;
altb 10:600d7cf652e7 43 float out = 0.0f;
pmic 5:d6c7ccbbce78 44 // main program and control loop
pmic 5:d6c7ccbbce78 45 // -----------------------------------------------------------------------------
rtlabor 0:15be70d21d7c 46 int main()
pmic 22:715d351d0be7 47 {
pmic 5:d6c7ccbbce78 48 // for serial comm.
pmic 5:d6c7ccbbce78 49 pc.baud(2000000);
pmic 22:715d351d0be7 50
pmic 22:715d351d0be7 51 gpa1.reset();
pmic 22:715d351d0be7 52 pt2.reset(0.0f);
pmic 22:715d351d0be7 53
pmic 5:d6c7ccbbce78 54 // attach controller loop to timer interrupt
rtlabor 0:15be70d21d7c 55 ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz;
rtlabor 0:15be70d21d7c 56 }
pmic 5:d6c7ccbbce78 57
pmic 5:d6c7ccbbce78 58 void updateControllers(void)
pmic 22:715d351d0be7 59 {
altb 10:600d7cf652e7 60 inp = gpa1(inp,out);
pmic 22:715d351d0be7 61 out = pt2(inp); // Test GPA with pt2 system
pmic 22:715d351d0be7 62
pmic 22:715d351d0be7 63 // u = Wobble(u, y) + Oexc;
pmic 22:715d351d0be7 64 // y = SysSecOrder(u);
pmic 22:715d351d0be7 65
pmic 22:715d351d0be7 66 // desTorque = pi_w(omega_desired - omega + excWobble);
pmic 22:715d351d0be7 67 // inpWobble = desTorque;
pmic 22:715d351d0be7 68 // outWobble = omega;
pmic 22:715d351d0be7 69 // excWobble = Wobble(excWobble, outWobble);
pmic 22:715d351d0be7 70
pmic 5:d6c7ccbbce78 71 }
pmic 5:d6c7ccbbce78 72
altb 10:600d7cf652e7 73
altb 10:600d7cf652e7 74 // Buttonhandling
pmic 5:d6c7ccbbce78 75 // -----------------------------------------------------------------------------
pmic 5:d6c7ccbbce78 76 // start timer as soon as Button is pressed
pmic 5:d6c7ccbbce78 77 void pressed()
pmic 5:d6c7ccbbce78 78 {
pmic 5:d6c7ccbbce78 79 t.start();
pmic 5:d6c7ccbbce78 80 }
pmic 5:d6c7ccbbce78 81
pmic 5:d6c7ccbbce78 82 // evaluating statemachine
pmic 5:d6c7ccbbce78 83 void released()
pmic 5:d6c7ccbbce78 84 {
pmic 5:d6c7ccbbce78 85
pmic 5:d6c7ccbbce78 86 // readout, stop and reset timer
pmic 5:d6c7ccbbce78 87 float ButtonTime = t.read();
pmic 5:d6c7ccbbce78 88 t.stop();
pmic 5:d6c7ccbbce78 89 t.reset();
pmic 5:d6c7ccbbce78 90
pmic 5:d6c7ccbbce78 91 // if the cube doesStand
rtlabor 0:15be70d21d7c 92 }
pmic 8:d68e177e2571 93 void printLine()
pmic 8:d68e177e2571 94 {
pmic 8:d68e177e2571 95 printf("-----------------------------------------------------------------------------------------\r\n");
pmic 8:d68e177e2571 96 }