altb_pmic / Mbed 2 deprecated GRT_VC_PIDT1_musterloesung

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

main.cpp@4:37df0f6a1bc3, 2019-05-10 (annotated)

Committer:
pmic
Date:
Fri May 10 18:15:41 2019 +0000
Revision:
4:37df0f6a1bc3
Parent:
3:477db0d9895e 
Introduce GPA and implement two sets of measurements which are apropriate for a frequency reasponse measurements, rember, the GPA is written to be used in CL, especially for weakly damped systems. It's just an example for future usage

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:4ed9a8952ddc 1 #include "mbed.h"
altb2 0:4ed9a8952ddc 2 #include "EncoderCounter.h"
altb2 1:314e2e3727f2 3 #include "PID_Cntrl.h"
altb2 0:4ed9a8952ddc 4 #include "LinearCharacteristics.h"
pmic 4:37df0f6a1bc3 5 #include "GPA.h"
altb2 0:4ed9a8952ddc 6 //------------------------------------------
altb2 0:4ed9a8952ddc 7 #define PI 3.1415927f
altb2 0:4ed9a8952ddc 8 //------------------------------------------
pmic 3:477db0d9895e 9
altb2 1:314e2e3727f2 10 /* GRT: Control of voice-coil with PID-T1 - Controller
altb2 0:4ed9a8952ddc 11 */
altb2 0:4ed9a8952ddc 12 Serial pc(SERIAL_TX, SERIAL_RX); // serial connection via USB - programmer
altb2 0:4ed9a8952ddc 13 InterruptIn button(USER_BUTTON); // User Button, short presses: reduce speed, long presses: increase speed
altb2 1:314e2e3727f2 14 AnalogOut out(PA_5); // Analog OUT on PA_5 1.6 V -> 0A 3.2A -> 4A (see ESCON)
altb2 0:4ed9a8952ddc 15
altb2 0:4ed9a8952ddc 16 bool key_was_pressed = false;
altb2 0:4ed9a8952ddc 17 bool controller_active = false;
altb2 0:4ed9a8952ddc 18 void pressed(void); // user Button pressed
altb2 0:4ed9a8952ddc 19 void released(void); // user Button released
pmic 3:477db0d9895e 20
altb2 0:4ed9a8952ddc 21 //------------------------------------------
altb2 0:4ed9a8952ddc 22 // ... here define variables like gains etc.
altb2 0:4ed9a8952ddc 23 //------------------------------------------
pmic 3:477db0d9895e 24 LinearCharacteristics i2u(-4.0f, 4.0f, 0.0f, 3.2f/3.3f); // output is normalized output
altb2 0:4ed9a8952ddc 25 //------------------------------------------
altb2 0:4ed9a8952ddc 26 Ticker ControllerLoopTimer; // interrupt for control loop
altb2 0:4ed9a8952ddc 27 EncoderCounter counter1(PB_6, PB_7); // initialize counter on PB_6 and PB_7
altb2 1:314e2e3727f2 28 Timer t_but; // define timer for button
altb2 0:4ed9a8952ddc 29 // ----- User defined functions -----------
altb2 0:4ed9a8952ddc 30 void updateLoop(void); // loop for State machine (via interrupt)
altb2 1:314e2e3727f2 31 float Ts = 0.0005f; // sample time of main loop
altb2 1:314e2e3727f2 32 uint16_t k = 0;
pmic 3:477db0d9895e 33 // initiate PIDT1 controller objects
pmic 3:477db0d9895e 34 // PID_Cntrl dt1(0.0f, 0.0f, 0.0109f, 0.000625f, Ts, -3.0f, 3.0f);
pmic 3:477db0d9895e 35 // PID_Cntrl pi1(1.35f, 13.5f, 0.0f, 1.0f, Ts, -3.0f, 3.0f);
pmic 3:477db0d9895e 36 PID_Cntrl dt1(0.0f, 0.0f, 0.0126f, 0.00067f, Ts, -3.0f, 3.0f);
pmic 3:477db0d9895e 37 PID_Cntrl pi1(3.16f, 15.1f, 0.0f, 1.0f, Ts, -3.0f, 3.0f);
pmic 3:477db0d9895e 38 float w = 0.5f;
pmic 4:37df0f6a1bc3 39 /*
pmic 4:37df0f6a1bc3 40 // low frequency region until 20 Hz with more amplitude
pmic 4:37df0f6a1bc3 41 float fMin = 0.3f;
pmic 4:37df0f6a1bc3 42 float fMax = 20.0f;
pmic 4:37df0f6a1bc3 43 int NfexcDes = 60;
pmic 4:37df0f6a1bc3 44 float Aexc0 = 3.0f;
pmic 4:37df0f6a1bc3 45 float Aexc1 = 0.8f; // Aexc0/fMax;
pmic 4:37df0f6a1bc3 46 GPA gpa(fMin, fMax, NfexcDes, Aexc0, Aexc1, Ts);
pmic 4:37df0f6a1bc3 47 float w = 0.0f;
pmic 4:37df0f6a1bc3 48 float exc = 0.0f;
pmic 4:37df0f6a1bc3 49 */
pmic 4:37df0f6a1bc3 50 /*
pmic 4:37df0f6a1bc3 51 // all frequencies with less amplitude (until approx. 20 Hz bad quality)
pmic 4:37df0f6a1bc3 52 float fMin = 1.0f;
pmic 4:37df0f6a1bc3 53 float fMax = 0.99f/2.0f/Ts;
pmic 4:37df0f6a1bc3 54 int NfexcDes = 200;
pmic 4:37df0f6a1bc3 55 float Aexc0 = 0.8f;
pmic 4:37df0f6a1bc3 56 float Aexc1 = 0.2f; // Aexc0/fMax;
pmic 4:37df0f6a1bc3 57 GPA gpa(fMin, fMax, NfexcDes, Aexc0, Aexc1, Ts);
pmic 4:37df0f6a1bc3 58 float w = 0.0f;
pmic 4:37df0f6a1bc3 59 float exc = 0.0f;
pmic 4:37df0f6a1bc3 60 */
altb2 0:4ed9a8952ddc 61 //******************************************************************************
altb2 0:4ed9a8952ddc 62 //---------- main loop -------------
altb2 0:4ed9a8952ddc 63 //******************************************************************************
altb2 0:4ed9a8952ddc 64 int main()
pmic 3:477db0d9895e 65 {
altb2 0:4ed9a8952ddc 66 pc.baud(115200); // for serial comm.
altb2 0:4ed9a8952ddc 67 counter1.reset(); // encoder reset
altb2 0:4ed9a8952ddc 68 out.write(i2u(0.0));
altb2 0:4ed9a8952ddc 69 button.fall(&pressed); // attach key pressed function
altb2 0:4ed9a8952ddc 70 button.rise(&released); // attach key pressed function
pmic 3:477db0d9895e 71 pc.printf("Start controller now or reset...\r\n");
altb2 1:314e2e3727f2 72 ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...;
altb2 1:314e2e3727f2 73 while(1);
altb2 0:4ed9a8952ddc 74 } // END OF main
altb2 0:4ed9a8952ddc 75 //******************************************************************************
altb2 0:4ed9a8952ddc 76 //---------- main loop (called via interrupt) -------------
altb2 0:4ed9a8952ddc 77 //******************************************************************************
pmic 3:477db0d9895e 78 void updateLoop(void)
pmic 3:477db0d9895e 79 {
altb2 1:314e2e3727f2 80 float x = (float)(counter1)/1000.0f; // get counts from Encoder
altb2 1:314e2e3727f2 81 float i_des = 0.0f; // default: set motor current to zero (will be overwritten)
pmic 3:477db0d9895e 82 if(controller_active) {
pmic 3:477db0d9895e 83 // controller update
pmic 3:477db0d9895e 84 i_des = pi1(w - x) - dt1(x);
pmic 4:37df0f6a1bc3 85 // i_des = pi1(exc - x) - dt1(x);
pmic 4:37df0f6a1bc3 86 // exc = gpa(i_des, x);
pmic 3:477db0d9895e 87 }
altb2 1:314e2e3727f2 88 out.write(i2u(i_des));
pmic 3:477db0d9895e 89 if(++k>1000) {
altb2 1:314e2e3727f2 90 pc.printf("x: %1.3f, i: %1.4f\r\n",x,i_des);
pmic 3:477db0d9895e 91 k = 0;
pmic 3:477db0d9895e 92 w = -w;
pmic 3:477db0d9895e 93 }
altb2 0:4ed9a8952ddc 94 } // END OF updateLoop(void)
pmic 3:477db0d9895e 95
altb2 0:4ed9a8952ddc 96 //******************************************************************************
pmic 3:477db0d9895e 97
pmic 3:477db0d9895e 98
altb2 0:4ed9a8952ddc 99 // start timer as soon as Button is pressed
altb2 0:4ed9a8952ddc 100 void pressed()
altb2 0:4ed9a8952ddc 101 {
altb2 0:4ed9a8952ddc 102 t_but.start();
altb2 0:4ed9a8952ddc 103 }
pmic 3:477db0d9895e 104 // Falling edge of button: enable/disable controller
altb2 0:4ed9a8952ddc 105 void released()
altb2 0:4ed9a8952ddc 106 {
altb2 0:4ed9a8952ddc 107 // readout, stop and reset timer
altb2 0:4ed9a8952ddc 108 float ButtonTime = t_but.read();
altb2 0:4ed9a8952ddc 109 t_but.stop();
altb2 0:4ed9a8952ddc 110 t_but.reset();
pmic 3:477db0d9895e 111 if(ButtonTime > 0.05f) {
altb2 1:314e2e3727f2 112 controller_active = !controller_active;
pmic 3:477db0d9895e 113 if(controller_active) {
pmic 3:477db0d9895e 114 pc.printf("Controller actived\r\n");
pmic 3:477db0d9895e 115 // reset controller here!!!
pmic 3:477db0d9895e 116 dt1.reset(0.0);
altb2 2:0b64504024fe 117 pi1.reset(0.0);
pmic 4:37df0f6a1bc3 118 // gpa.reset();
pmic 4:37df0f6a1bc3 119 // exc = 0.0f;
pmic 3:477db0d9895e 120 } else
altb2 1:314e2e3727f2 121 pc.printf("Controller disabled\r\n");
altb2 1:314e2e3727f2 122 }
altb2 0:4ed9a8952ddc 123 }