Regler von Kellep15
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 float w = 1; 00039 // initiate PIDT1 controller objects 00040 PID_Cntrl dt1(0.0, 0.0, 0.0116, 0.000625, Ts, -3.0, 3.0); 00041 PID_Cntrl pi1(3.59, 13.5, 0.0, 1.0, Ts, -3.0, 3.0); 00042 // 00043 //****************************************************************************** 00044 //---------- main loop ------------- 00045 //****************************************************************************** 00046 int main() 00047 { 00048 pc.baud(115200); // for serial comm. 00049 counter1.reset(); // encoder reset 00050 out.write(i2u(0.0)); 00051 button.fall(&pressed); // attach key pressed function 00052 button.rise(&released); // attach key pressed function 00053 pc.printf("Start controller now or reset...\r\n"); 00054 ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...; 00055 while(1); 00056 } // END OF main 00057 //****************************************************************************** 00058 //---------- main loop (called via interrupt) ------------- 00059 //****************************************************************************** 00060 void updateLoop(void) 00061 { 00062 float x = (float)(counter1)/1000.0f; // get counts from Encoder 00063 float i_des = 0.0f; // default: set motor current to zero (will be overwritten) 00064 if(controller_active) { 00065 // controller update 00066 i_des = -dt1(x); 00067 //i_des = pi1(w-x)-dt1(x); 00068 } 00069 out.write(i2u(i_des)); 00070 if(++k>1000) { 00071 pc.printf("x: %1.3f, i: %1.4f\r\n",x,i_des); 00072 k = 0; 00073 w = -w; 00074 } 00075 } // END OF updateLoop(void) 00076 00077 //****************************************************************************** 00078 00079 00080 // start timer as soon as Button is pressed 00081 void pressed() 00082 { 00083 t_but.start(); 00084 } 00085 // Falling edge of button: enable/disable controller 00086 void released() 00087 { 00088 // readout, stop and reset timer 00089 float ButtonTime = t_but.read(); 00090 t_but.stop(); 00091 t_but.reset(); 00092 if(ButtonTime > 0.05f) { 00093 controller_active = !controller_active; 00094 if(controller_active) { 00095 pc.printf("Controller actived\r\n"); 00096 // reset controller here!!! 00097 dt1.reset(0.0); 00098 } else 00099 pc.printf("Controller disabled\r\n"); 00100 } 00101 }
Generated on Mon Jul 31 2023 05:47:57 by
1.7.2