GRT_FS2019_VoiceCoil / Mbed 2 deprecated GRT_VC_PIDT1

Control of VoiceCoil Lab#7

Dependencies:   mbed

main.cpp@2:91678e836872, 2019-05-24 (annotated)

Committer:
altb2
Date:
Fri May 24 10:06:51 2019 +0000
Revision:
2:91678e836872
Parent:
1:92f175969d90 
Final G3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:05dd1de8cc3f 1 #include "mbed.h"
altb2 0:05dd1de8cc3f 2 #include "EncoderCounter.h"
pmic 1:92f175969d90 3 #include "PID_Cntrl.h"
altb2 0:05dd1de8cc3f 4 #include "LinearCharacteristics.h"
altb2 0:05dd1de8cc3f 5 //------------------------------------------
altb2 0:05dd1de8cc3f 6 #define PI 3.1415927f
altb2 0:05dd1de8cc3f 7 //------------------------------------------
pmic 1:92f175969d90 8
altb2 0:05dd1de8cc3f 9 /* GRT: Control of voice-coil with PID-T1 - Controller
pmic 1:92f175969d90 10
pmic 1:92f175969d90 11 Eigther reseting the Nucleo via the black button or save a new software on
pmic 1:92f175969d90 12 the Nucleo sets the analog output to zero. Zero is equal to -4 Ampere!!!
pmic 1:92f175969d90 13 Therefor: NEVER !!! reset or save a new software while the VC is powered on
pmic 1:92f175969d90 14 (the green button on the VC is glowing green)
pmic 1:92f175969d90 15
altb2 0:05dd1de8cc3f 16 */
altb2 0:05dd1de8cc3f 17 Serial pc(SERIAL_TX, SERIAL_RX); // serial connection via USB - programmer
altb2 0:05dd1de8cc3f 18 InterruptIn button(USER_BUTTON); // User Button, short presses: reduce speed, long presses: increase speed
altb2 0:05dd1de8cc3f 19 AnalogOut out(PA_5); // Analog OUT on PA_5 1.6 V -> 0A 3.2A -> 4A (see ESCON)
altb2 0:05dd1de8cc3f 20
altb2 0:05dd1de8cc3f 21 bool key_was_pressed = false;
altb2 0:05dd1de8cc3f 22 bool controller_active = false;
altb2 0:05dd1de8cc3f 23 void pressed(void); // user Button pressed
altb2 0:05dd1de8cc3f 24 void released(void); // user Button released
pmic 1:92f175969d90 25
altb2 0:05dd1de8cc3f 26 //------------------------------------------
altb2 0:05dd1de8cc3f 27 // ... here define variables like gains etc.
altb2 0:05dd1de8cc3f 28 //------------------------------------------
pmic 1:92f175969d90 29 LinearCharacteristics i2u(-4.0f, 4.0f, 0.0f, 3.2f/3.3f); // output is normalized output
altb2 0:05dd1de8cc3f 30 //------------------------------------------
altb2 0:05dd1de8cc3f 31 Ticker ControllerLoopTimer; // interrupt for control loop
altb2 0:05dd1de8cc3f 32 EncoderCounter counter1(PB_6, PB_7); // initialize counter on PB_6 and PB_7
altb2 0:05dd1de8cc3f 33 Timer t_but; // define timer for button
altb2 0:05dd1de8cc3f 34 // ----- User defined functions -----------
altb2 0:05dd1de8cc3f 35 void updateLoop(void); // loop for State machine (via interrupt)
altb2 0:05dd1de8cc3f 36 float Ts = 0.0005f; // sample time of main loop
altb2 0:05dd1de8cc3f 37 uint16_t k = 0;
altb2 2:91678e836872 38 float w = 0.50;
pmic 1:92f175969d90 39 // initiate PIDT1 controller objects
altb2 2:91678e836872 40 PID_Cntrl dt1(0.0,0.0,0.0116,0.000625,Ts,-4.0,4.0);
altb2 2:91678e836872 41 PID_Cntrl pi1(3.59,13.5,0.0,1.0,Ts,-4.0,4.0);
altb2 2:91678e836872 42
altb2 0:05dd1de8cc3f 43 //******************************************************************************
altb2 0:05dd1de8cc3f 44 //---------- main loop -------------
altb2 0:05dd1de8cc3f 45 //******************************************************************************
altb2 0:05dd1de8cc3f 46 int main()
pmic 1:92f175969d90 47 {
altb2 0:05dd1de8cc3f 48 pc.baud(115200); // for serial comm.
altb2 0:05dd1de8cc3f 49 counter1.reset(); // encoder reset
altb2 0:05dd1de8cc3f 50 out.write(i2u(0.0));
altb2 0:05dd1de8cc3f 51 button.fall(&pressed); // attach key pressed function
altb2 0:05dd1de8cc3f 52 button.rise(&released); // attach key pressed function
pmic 1:92f175969d90 53 pc.printf("Start controller now or reset...\r\n");
altb2 0:05dd1de8cc3f 54 ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...;
altb2 0:05dd1de8cc3f 55 while(1);
altb2 0:05dd1de8cc3f 56 } // END OF main
altb2 0:05dd1de8cc3f 57 //******************************************************************************
altb2 0:05dd1de8cc3f 58 //---------- main loop (called via interrupt) -------------
altb2 0:05dd1de8cc3f 59 //******************************************************************************
pmic 1:92f175969d90 60 void updateLoop(void)
pmic 1:92f175969d90 61 {
altb2 0:05dd1de8cc3f 62 float x = (float)(counter1)/1000.0f; // get counts from Encoder
altb2 0:05dd1de8cc3f 63 float i_des = 0.0f; // default: set motor current to zero (will be overwritten)
pmic 1:92f175969d90 64 if(controller_active) {
pmic 1:92f175969d90 65 // controller update
altb2 2:91678e836872 66 i_des = pi1(w-x)-dt1(x);
pmic 1:92f175969d90 67 }
altb2 0:05dd1de8cc3f 68 out.write(i2u(i_des));
pmic 1:92f175969d90 69 if(++k>1000) {
altb2 0:05dd1de8cc3f 70 pc.printf("x: %1.3f, i: %1.4f\r\n",x,i_des);
pmic 1:92f175969d90 71 k = 0;
altb2 2:91678e836872 72 w = -w;
pmic 1:92f175969d90 73
pmic 1:92f175969d90 74 }
altb2 0:05dd1de8cc3f 75 } // END OF updateLoop(void)
pmic 1:92f175969d90 76
altb2 0:05dd1de8cc3f 77 //******************************************************************************
pmic 1:92f175969d90 78
pmic 1:92f175969d90 79
altb2 0:05dd1de8cc3f 80 // start timer as soon as Button is pressed
altb2 0:05dd1de8cc3f 81 void pressed()
altb2 0:05dd1de8cc3f 82 {
altb2 0:05dd1de8cc3f 83 t_but.start();
altb2 0:05dd1de8cc3f 84 }
pmic 1:92f175969d90 85 // Falling edge of button: enable/disable controller
altb2 0:05dd1de8cc3f 86 void released()
altb2 0:05dd1de8cc3f 87 {
altb2 0:05dd1de8cc3f 88 // readout, stop and reset timer
altb2 0:05dd1de8cc3f 89 float ButtonTime = t_but.read();
altb2 0:05dd1de8cc3f 90 t_but.stop();
altb2 0:05dd1de8cc3f 91 t_but.reset();
pmic 1:92f175969d90 92 if(ButtonTime > 0.05f) {
altb2 0:05dd1de8cc3f 93 controller_active = !controller_active;
pmic 1:92f175969d90 94 if(controller_active) {
pmic 1:92f175969d90 95 pc.printf("Controller actived\r\n");
pmic 1:92f175969d90 96 // reset controller here!!!
altb2 2:91678e836872 97 dt1.reset(0.0);
altb2 2:91678e836872 98 pi1.reset(0.0);
pmic 1:92f175969d90 99 } else
altb2 0:05dd1de8cc3f 100 pc.printf("Controller disabled\r\n");
altb2 0:05dd1de8cc3f 101 }
altb2 0:05dd1de8cc3f 102 }