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 }