main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "HIDScope.h"
00004 
00005 //////////////////////////////////////CONSTANTS, I/O, FREQUENCIES AND TICKERS//////////////////////////////////////
00006 //info uit
00007 HIDScope scope(3);
00008 
00009 //encoders
00010 QEI encoder1 (D12,D13,NC,32); // first b then a for clockwise +
00011 
00012 //ingangen
00013 AnalogIn pot1(A2);
00014 
00015 //uitgangen
00016 DigitalOut motor1_direction(D7);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
00017 PwmOut motor1_speed_control(D6);//aanstuursnelheid motor 1
00018 //PwmOut motor2_speed_control(D5);
00019 //DigitalOut motor2_direction(D4);
00020 const int CW=1; //clockwise
00021 const int CCW=0; //counterclockwise
00022 
00023 //frequencies
00024 //const float pwm_frequency=1000;
00025 const double hidscope_frequency=100;//frequentie waarop data naar HIDScope wordt gestuurd
00026 const double pi_control_frequency=5;//frequentie waarop er een control actie wordt uitgevoerd. kan hoger, voorzichtig bij ruis!
00027 
00028 //tickers
00029 Ticker hidscope_ticker;
00030 Ticker pi_control_ticker;
00031 
00032 //constants
00033 const int cpr=32*131;
00034 const float PI=3.1415;
00035 const float counttorad=((2*PI)/cpr);
00036 
00037 ///////////////////////////////////////////////////CONTROLLER CONSTANTS////////////////////////////////
00038 
00039 //DEZE WAARDES ZIJN ZOMAAR RANDOM WAARDES!!!!
00040 const float motor1_pi_kp=0.5;
00041 const double motor1_pi_ki=0.01;
00042 double motor1_error_int=0;
00043 ////////////////////////////////////////////GO FLAGS AND ACTIVATION FUNCTIONS//////////////////////////////////
00044 //go flags
00045 volatile bool scopedata_go=false, pi_control_go=false;
00046 
00047 //acvitator functions
00048 
00049 void scopedata_activate()
00050 {
00051     scopedata_go=true;
00052 }
00053 void pi_control_activate()
00054 {
00055     pi_control_go=true;
00056 }
00057 ///////////////////////////////////////////////////////FUNCTIONS//////////////////////////////////////////////////////////////////////////
00058 
00059 //scopedata
00060 void scopedata()
00061 {
00062     scope.set(0,2*PI*pot1.read());//gewenste hoek in rad van potmeter
00063     scope.set(1,counttorad*encoder1.getPulses());//hoek in rad van outputshaft
00064     scope.set(2,motor1_speed_control.read());//pwm signaal naar motor toe
00065     scope.send();
00066 }
00067 //////////////////////////////////////////////////////////CONTROLLER///////////////////////////////////////
00068 // Reusable PI controller
00069 double pi_control( double e, const double Kp, const double Ki, double Ts, double& motor1_error_int)
00070 {
00071     motor1_error_int = motor1_error_int + Ts * e; // e_int is changed globally because it’s ’by reference’ (&)
00072     return Kp*e+Ki*motor1_error_int;
00073 }
00074 
00075 //////////////////////////////////////////////////MAIN///////////////////////////////////
00076 int main()
00077 {
00078     //set initial shizzle
00079     //motor1_speed_control.period(1.0/pwm_frequency);
00080     motor1_speed_control.write(0);
00081 
00082     //tickers
00083     hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency);
00084     pi_control_ticker.attach(&pi_control_activate,1.0/pi_control_frequency);
00085 
00086     while(1) {
00087         //control motor 1 with a pi controller
00088         if (pi_control_go==true) {
00089             double error=2*PI*pot1.read()-counttorad*encoder1.getPulses();
00090             double signal_motor1=pi_control(error,motor1_pi_kp,motor1_pi_ki,
00091                                             1/pi_control_frequency,motor1_error_int);//send error to p controller
00092             if (signal_motor1>=0) {//determine CW or CCW rotation
00093                 motor1_direction.write(CW);
00094             } else {
00095                 motor1_direction.write(CCW);
00096             }
00097 
00098             if (fabs(signal_motor1)>=1) { //check if signal is <1
00099                 signal_motor1=1;//if signal >1 make it 1 to not damage motor
00100             } else {
00101                 signal_motor1=fabs(signal_motor1);// if signal<1 use signal
00102             }
00103 
00104             motor1_speed_control.write(fabs(signal_motor1));//write signal to motor
00105             pi_control_go=false;
00106         }
00107         //call scopedata
00108         if (scopedata_go==true) {
00109             scopedata();
00110             scopedata_go=false;
00111         }
00112     }
00113     return 0;
00114 }