main.cpp

00001 ///////////////////////////////////////////////#INCLUDES//////////////////////////////////////////
00002 #include "mbed.h"
00003 #include "QEI.h"
00004 #include "HIDScope.h"
00005 
00006 //////////////////////////////////////////////CONSTANTS AND SHIZZLE////////////////////////////////////////////////////////////////////////////
00007 //info uit
00008 HIDScope scope(3);
00009 //encoders
00010 QEI encoder1 (D12,D13,NC,32); // first b then a for clockwise +
00011 
00012 //ingangen
00013 AnalogIn pot1(A2);
00014 InterruptIn changecontrollervalues(PTA4);
00015 Serial pc(USBTX, USBRX);
00016 
00017 //uitgangen
00018 DigitalOut motor1_direction(D7);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
00019 PwmOut motor1_speed_control(D6);//aanstuursnelheid motor 1
00020 //PwmOut motor2_speed_control(D5);
00021 //DigitalOut motor2_direction(D4);
00022 const int CW=1; //clockwise
00023 const int CCW=0; //counterclockwise
00024 
00025 //frequencies
00026 //const float pwm_frequency=1000;
00027 const double hidscope_frequency=100;
00028 const double pid_control_frequency=5;
00029 
00030 //tickers
00031 Ticker hidscope_ticker;
00032 Ticker pid_control_ticker;
00033 
00034 //constants
00035 const int cpr=32*131;
00036 const float PI=3.1415;
00037 const float counttorad=((2*PI)/cpr);
00038 
00039 //////////////////////////////////////////////////////////FILTERS///////////////////////////////////////////////////////
00040 
00041 //IK WEET NIET HOE GOED DEZE WAARDES KLOPPEN, VOORZICHTIG DUS!! WAARDES ZIJN TE VERKRIJGEN UIT ASN
00042 //Biquad #1
00043 const double gain_f1 = 0.004756;
00044 const double B_f1[] = { 1.00000000000, -0.34966580719,  1.00000000000};
00045 const double A_f1[] = { 1.00000000000, -1.86848759822,  0.87558390333};
00046 
00047 //Biquad #2
00048 const double gain_f2 = 0.036454;
00049 const double B_f2[] = { 1.00000000000, -1.56455717372,  1.00000000000};
00050 const double A_f2[] = { 1.00000000000, -1.93013247482,  0.94815823632};
00051 
00052 
00053 //filterwaardes uit asn naar aparte waardes omschrijven
00054 double v1 = 0, v2 = 0, u = 0, y = 0;
00055 const double a1_f1 = gain_f1*A_f1[1], a2_f1 = gain_f1*A_f1[2], b0_f1 = gain_f1*B_f1[0], b1_f1 = gain_f1*B_f1[1], b2_f1 = gain_f1*B_f1[2]; //filter coefficients filter 1
00056 const double a1_f2 = gain_f2*A_f1[1], a2_f2 = gain_f2*A_f2[2], b0_f2 = gain_f2*B_f2[0], b1_f2 = gain_f2*B_f2[1], b2_f2 = gain_f2*B_f2[2]; //filter coefficients filter 1
00057 
00058 
00059 //biquad filter
00060 double biquad( double u, double &v1, double &v2, const double a1, const double a2,
00061                const double b0, const double b1, const double b2 )
00062 {
00063     double v = u - a1*v1 - a2*v2;
00064     double y = b0*v + b1*v1 + b2*v2;
00065     v2 = v1;
00066     v1 = v;
00067     return y;
00068 }
00069 ///////////////////////////////////////////////////////////CONTROLLERS//////////////////////////////////////////////////////////
00070 
00071 //DIT ZIJN ZOMAAR WAARDEN, AANPASSEN DUS WAARDES BEREKENEN MET MATLAB
00072 float motor1_pid_kp=0.5;
00073 float motor1_pid_ki=0.01;
00074 float motor1_pid_kd=0.01;
00075 
00076 double motor1_error_int=0;
00077 double motor1_error_prev=0;
00078 
00079 // Reusable PID controller with filter
00080 double pid_control( double e, const double Kp, const double Ki, const double Kd, double Ts,//Ts is dus sampletijd
00081                     double &motor1_error_int, double &motor1_error_prev)
00082 {
00083 // Derivative
00084     double e_der = (e - motor1_error_prev)/Ts;
00085     e_der = biquad( e_der, v1, v2, a1_f1, a2_f1, b0_f1, b1_f1, b2_f1);//twee keer hetzelfde met andere filterwaardes, dit klopt
00086     e_der = biquad( e_der, v1, v2, a1_f2, a2_f2, b0_f2, b1_f2, b2_f2);//is omdat een biquad filter maar 6 polen en nulpunten heeft
00087     motor1_error_prev = e;
00088 // Integral
00089     motor1_error_int = motor1_error_int + Ts * e;
00090 // PID
00091     return Kp * e + Ki * motor1_error_int + Kd * e_der;
00092 }
00093 
00094 ///////////////////////////////////////////////////////////GO-FLAGS AND ACTIVATION FUNCTIONS///////////////////////////////
00095 //go flags
00096 volatile bool scopedata_go=false, pid_control_go=false;
00097 
00098 //acvitator functions
00099 
00100 void scopedata_activate()
00101 {
00102     scopedata_go=true;
00103 }
00104 void pid_control_activate()
00105 {
00106     pid_control_go=true;
00107 }
00108 ///////////////////////////////////////////////////////FUNCTIONS//////////////////////////////////////////////////////////////////////////
00109 
00110 //scopedata
00111 void scopedata()
00112 {
00113     scope.set(0,2*PI*pot1.read());
00114     scope.set(1,counttorad*encoder1.getPulses());
00115     scope.set(2,motor1_speed_control.read());
00116     scope.send();
00117 }
00118 
00119 //////////////////////////////////////////////////MAIN////////////////////////////////////////////////////////////////////
00120 int main()
00121 {
00122     //set initial shizzle
00123     motor1_speed_control.write(0);
00124 
00125     //tickers
00126     hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency);
00127     pid_control_ticker.attach(&pid_control_activate,1.0/pid_control_frequency);
00128 
00129     //make variables
00130     volatile float signal_motor1=0;
00131     while(1) {
00132 
00133         //control motor 1 with a pID controller
00134         if (pid_control_go==true) {
00135             double error=2*PI*pot1.read()-counttorad*encoder1.getPulses();
00136             double signal_motor1=pid_control(error,motor1_pid_kp,motor1_pid_ki,motor1_pid_kd,
00137                                              1.0/pid_control_frequency,motor1_error_int,motor1_error_prev);//send error to pid controller 1.0/pid_control_frequency=sampletijd
00138             if (signal_motor1>=0) {//determine CW or CCW rotation
00139                 motor1_direction.write(CW);
00140             } else {
00141                 motor1_direction.write(CCW);
00142             }
00143 
00144             if (fabs(signal_motor1)>=1) { //check if signal is <1
00145                 signal_motor1=1;//if signal >1 make it 1 to not damage motor
00146             } else {
00147                 signal_motor1=fabs(signal_motor1);// if signal<1 use signal
00148             }
00149             motor1_speed_control.write(fabs(signal_motor1));//write signal to motor
00150             pid_control_go=false;
00151         }
00152         //call scopedata
00153         if (scopedata_go==true) {
00154             scopedata();
00155             scopedata_go=false;
00156         }
00157         //intterrupt in to change controller values
00158         if (changecontrollervalues==0) {
00159             motor1_speed_control.write(0);
00160             pc.printf("KP is now %f, enter new value\n",motor1_pid_kp);
00161             pc.scanf("%f", &motor1_pid_kp);
00162 
00163             pc.printf("KI is now %f, enter new value\n",motor1_pid_ki);
00164             pc.scanf("%f", &motor1_pid_ki);
00165 
00166             pc.printf("KD is now %f, enter new value\n",motor1_pid_kd);
00167             pc.scanf("%f", &motor1_pid_kd);
00168         }
00169     }
00170     return 0; //gives warning: unreachable
00171 }