pr7
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "EncoderCounter.h" 00003 #include "PID_Cntrl.h" 00004 #include "LinearCharacteristics.h" 00005 //------------------------------------------ 00006 #define PI 3.1415927f 00007 //------------------------------------------ 00008 00009 /* GRT: Control of voice-coil with PID-T1 - Controller 00010 00011 Eigther reseting the Nucleo via the black button or save a new software on 00012 the Nucleo sets the analog output to zero. Zero is equal to -4 Ampere!!! 00013 Therefor: NEVER !!! reset or save a new software while the VC is powered on 00014 (the green button on the VC is glowing green) 00015 00016 */ 00017 Serial pc(SERIAL_TX, SERIAL_RX); // serial connection via USB - programmer 00018 InterruptIn button(USER_BUTTON); // User Button, short presses: reduce speed, long presses: increase speed 00019 AnalogOut out(PA_5); // Analog OUT on PA_5 1.6 V -> 0A 3.2A -> 4A (see ESCON) 00020 00021 bool key_was_pressed = false; 00022 bool controller_active = false; 00023 void pressed(void); // user Button pressed 00024 void released(void); // user Button released 00025 00026 //------------------------------------------ 00027 // ... here define variables like gains etc. 00028 //------------------------------------------ 00029 LinearCharacteristics i2u(-4.0f, 4.0f, 0.0f, 3.2f/3.3f); // output is normalized output 00030 //------------------------------------------ 00031 Ticker ControllerLoopTimer; // interrupt for control loop 00032 EncoderCounter counter1(PB_6, PB_7); // initialize counter on PB_6 and PB_7 00033 Timer t_but; // define timer for button 00034 // ----- User defined functions ----------- 00035 void updateLoop(void); // loop for State machine (via interrupt) 00036 float Ts = 0.0005f; // sample time of main loop 00037 uint16_t k = 0; 00038 // initiate PIDT1 controller objects 00039 PID_Cntrl dt1( 0.0f, 0.0f, 0.0186f, 0.000667f, Ts, -3.0f, 3.0f); 00040 PID_Cntrl pi( 8.13f, 15.0f, 0.0f, 0.0f, Ts, -3.0f, 3.0f); // Ki 38.8 00041 float w = 0.5f; 00042 //****************************************************************************** 00043 //---------- main loop ------------- 00044 //****************************************************************************** 00045 int main() 00046 { 00047 pc.baud(115200); // for serial comm. 00048 counter1.reset(); // encoder reset 00049 out.write(i2u(0.0)); 00050 button.fall(&pressed); // attach key pressed function 00051 button.rise(&released); // attach key pressed function 00052 pc.printf("Start controller now or reset...\r\n"); 00053 ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...; 00054 while(1); 00055 } // END OF main 00056 //****************************************************************************** 00057 //---------- main loop (called via interrupt) ------------- 00058 //****************************************************************************** 00059 void updateLoop(void) 00060 { 00061 float x = (float)(counter1)/1000.0f; // get counts from Encoder 00062 float i_des = 0.0f; // default: set motor current to zero (will be overwritten) 00063 if(controller_active) { 00064 // controller update 00065 i_des = pi(w - x) - dt1(x); 00066 } 00067 out.write(i2u(i_des)); 00068 if(++k>1000) { 00069 pc.printf("x: %1.3f, i: %1.4f\r\n",x,i_des); 00070 k = 0; 00071 w = -w; 00072 } 00073 } // END OF updateLoop(void) 00074 00075 //****************************************************************************** 00076 00077 00078 // start timer as soon as Button is pressed 00079 void pressed() 00080 { 00081 t_but.start(); 00082 } 00083 // Falling edge of button: enable/disable controller 00084 void released() 00085 { 00086 // readout, stop and reset timer 00087 float ButtonTime = t_but.read(); 00088 t_but.stop(); 00089 t_but.reset(); 00090 if(ButtonTime > 0.05f) { 00091 controller_active = !controller_active; 00092 if(controller_active) { 00093 pc.printf("Controller actived\r\n"); 00094 // reset controller here!!! 00095 dt1.reset(0.0f); 00096 pi.reset(0.0f); 00097 } else 00098 pc.printf("Controller disabled\r\n"); 00099 } 00100 }
Generated on Thu Jul 21 2022 14:11:08 by
1.7.2