Tobias Vögeli / Mbed 2 deprecated GRT_VC_PIDT1

pr7

Dependencies:   mbed

main.cpp@2:1ded9d10f322, 2019-05-14 (annotated)

Committer:
voegetob
Date:
Tue May 14 15:29:24 2019 +0000
Revision:
2:1ded9d10f322
Parent:
1:92f175969d90 
pr7

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;
pmic 1:92f175969d90 38 // initiate PIDT1 controller objects
voegetob 2:1ded9d10f322 39 PID_Cntrl dt1( 0.0f, 0.0f, 0.0186f, 0.000667f, Ts, -3.0f, 3.0f);
voegetob 2:1ded9d10f322 40 PID_Cntrl pi( 8.13f, 15.0f, 0.0f, 0.0f, Ts, -3.0f, 3.0f); // Ki 38.8
voegetob 2:1ded9d10f322 41 float w = 0.5f;
altb2 0:05dd1de8cc3f 42 //******************************************************************************
altb2 0:05dd1de8cc3f 43 //---------- main loop -------------
altb2 0:05dd1de8cc3f 44 //******************************************************************************
altb2 0:05dd1de8cc3f 45 int main()
pmic 1:92f175969d90 46 {
altb2 0:05dd1de8cc3f 47 pc.baud(115200); // for serial comm.
altb2 0:05dd1de8cc3f 48 counter1.reset(); // encoder reset
altb2 0:05dd1de8cc3f 49 out.write(i2u(0.0));
altb2 0:05dd1de8cc3f 50 button.fall(&pressed); // attach key pressed function
altb2 0:05dd1de8cc3f 51 button.rise(&released); // attach key pressed function
pmic 1:92f175969d90 52 pc.printf("Start controller now or reset...\r\n");
altb2 0:05dd1de8cc3f 53 ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...;
altb2 0:05dd1de8cc3f 54 while(1);
altb2 0:05dd1de8cc3f 55 } // END OF main
altb2 0:05dd1de8cc3f 56 //******************************************************************************
altb2 0:05dd1de8cc3f 57 //---------- main loop (called via interrupt) -------------
altb2 0:05dd1de8cc3f 58 //******************************************************************************
pmic 1:92f175969d90 59 void updateLoop(void)
pmic 1:92f175969d90 60 {
altb2 0:05dd1de8cc3f 61 float x = (float)(counter1)/1000.0f; // get counts from Encoder
altb2 0:05dd1de8cc3f 62 float i_des = 0.0f; // default: set motor current to zero (will be overwritten)
pmic 1:92f175969d90 63 if(controller_active) {
pmic 1:92f175969d90 64 // controller update
voegetob 2:1ded9d10f322 65 i_des = pi(w - x) - dt1(x);
pmic 1:92f175969d90 66 }
voegetob 2:1ded9d10f322 67 out.write(i2u(i_des));
pmic 1:92f175969d90 68 if(++k>1000) {
altb2 0:05dd1de8cc3f 69 pc.printf("x: %1.3f, i: %1.4f\r\n",x,i_des);
pmic 1:92f175969d90 70 k = 0;
voegetob 2:1ded9d10f322 71 w = -w;
pmic 1:92f175969d90 72 }
altb2 0:05dd1de8cc3f 73 } // END OF updateLoop(void)
pmic 1:92f175969d90 74
altb2 0:05dd1de8cc3f 75 //******************************************************************************
pmic 1:92f175969d90 76
pmic 1:92f175969d90 77
altb2 0:05dd1de8cc3f 78 // start timer as soon as Button is pressed
altb2 0:05dd1de8cc3f 79 void pressed()
altb2 0:05dd1de8cc3f 80 {
altb2 0:05dd1de8cc3f 81 t_but.start();
altb2 0:05dd1de8cc3f 82 }
pmic 1:92f175969d90 83 // Falling edge of button: enable/disable controller
altb2 0:05dd1de8cc3f 84 void released()
altb2 0:05dd1de8cc3f 85 {
altb2 0:05dd1de8cc3f 86 // readout, stop and reset timer
altb2 0:05dd1de8cc3f 87 float ButtonTime = t_but.read();
altb2 0:05dd1de8cc3f 88 t_but.stop();
altb2 0:05dd1de8cc3f 89 t_but.reset();
pmic 1:92f175969d90 90 if(ButtonTime > 0.05f) {
altb2 0:05dd1de8cc3f 91 controller_active = !controller_active;
pmic 1:92f175969d90 92 if(controller_active) {
pmic 1:92f175969d90 93 pc.printf("Controller actived\r\n");
pmic 1:92f175969d90 94 // reset controller here!!!
voegetob 2:1ded9d10f322 95 dt1.reset(0.0f);
voegetob 2:1ded9d10f322 96 pi.reset(0.0f);
pmic 1:92f175969d90 97 } else
altb2 0:05dd1de8cc3f 98 pc.printf("Controller disabled\r\n");
altb2 0:05dd1de8cc3f 99 }
altb2 0:05dd1de8cc3f 100 }