Regler von Kellep15

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }