NR_method_1.cpp

00001 #include "mbed.h"
00002 #include "BiQuad.h"
00003 #include <math.h>
00004 #include <stdio.h>
00005 #include <iostream>
00006 #include <stdlib.h>
00007 #include <ctime>
00008 #include <QEI.h>
00009 #include "PID_controler.h"
00010 //#include <HIDScope.h>
00011 
00012 //Define in/outputs
00013 Serial pc(USBTX, USBRX);
00014 AnalogIn   emg1(A0);// emg1 input
00015 AnalogIn   emg2(A1); // emg2 input
00016 InterruptIn sw2(SW2);
00017 InterruptIn button(SW3);
00018 InterruptIn emgbutton(D0);
00019 InterruptIn emgbutton2(D1);
00020 DigitalOut Led(LED1);
00021 DigitalOut Led2(LED2);
00022 PwmOut PMW1(D5); // Motor 1
00023 DigitalOut M1(D4); // direction of motor 1
00024 PwmOut PMW2(D6); // Motor 2
00025 DigitalOut M2(D7); // direction of motor 2
00026 
00027 // hidscope
00028 //HIDScope    scope( 4 ); // 4 to check the different
00029 
00030 // tickers
00031 Ticker sample_timer;
00032 Ticker position_controll;
00033 
00034 //Define Variables
00035 double pi = 3.14159265359;
00036 int bb;
00037 int bc;
00038 float z;
00039 int i;
00040 double angle_a = 0; //in rad
00041 double angle_b = 0.5 * pi;  //in rad
00042 double X0[2][1] = {{angle_a},{angle_b}};
00043 double X[2][1];
00044 double Xold[2][1];
00045 double fval[2][1];
00046 double J[2][2];
00047 double err[2][1];
00048 double MaxIter = 20;
00049 double tolX = 1e-4;
00050 double A = 20;
00051 double B = 30;
00052 double C = 20;
00053 double D = 27;
00054 double E = 35;
00055 double ex = -40; // current position
00056 double ey = -30; // current position
00057 double Cxx = -35; // Goal position
00058 double Cyy = 27; // Goal position
00059 double Kp = 4;
00060 double Ki = 4.5;
00061 double Kd = 0.7;
00062 double Ts = 0.002;
00063 bool startCalc;
00064 bool calpos1;
00065 bool calpos2;
00066 bool bas;
00067 int waiting;
00068 int count1;
00069 int count2;
00070 int count3;
00071 int count4;
00072 float counts_a;
00073 float counts_b;
00074 int counts = 8400;
00075 float x1;
00076 float y1;
00077 double emgFiltered3;
00078 double emgFiltered23;
00079 bool dir = true;
00080 int countsemg = 0;
00081 bool calibration;
00082 double sumemgx;
00083 double sumemgy;
00084 double threshholdx = 5;
00085 double threshholdy = 5;
00086 // filtering
00087 //filter coeffiecents
00088 // highpass
00089 double b01h =  0.956543225556877;
00090 double b02h = -1.91308645111375;
00091 double b03h =  0.956543225556877;
00092 double a01h = -1.91119706742607;
00093 double a02h =  0.914975834801434;
00094 // notchfilter
00095 double b01n =  0.991103635646810;
00096 double b02n = -1.60363936885013;
00097 double b03n =  0.991103635646810;
00098 double a01n = -1.60363936885013;
00099 double a02n =  0.982207271293620;
00100 //lowpass 1
00101 double b01l =  0.000346041337639103;
00102 double b02l =  0.000692082675278205;
00103 double b03l =  0.000346041337639103;
00104 double a01l = -1.94669754075618;
00105 double a02l =  0.948081706106740;
00106 BiQuadChain bqc;
00107 BiQuad bq1( b01h, b02h, b03h, a01h, a02h ); //highpass
00108 BiQuad bq2( b01n, b02n, b03n, a01n, a02n ); //notch
00109 // than we need to rectifie
00110 // and lowpass afterwards
00111 BiQuadChain bqc2;
00112 BiQuad bq3( b01l, b02l, b03l, a01l, a02l);  //lowpass
00113 // optional is doing a movingaverage after
00114 
00115 BiQuadChain bqc3;
00116 BiQuad bq4( b01h, b02h, b03h, a01h, a02h ); //highpass
00117 BiQuad bq5( b01n, b02n, b03n, a01n, a02n ); //notch
00118 // than we need to rectifie
00119 // and lowpass afterwards
00120 BiQuadChain bqc4;
00121 BiQuad bq6( b01l, b02l, b03l, a01l, a02l);  //lowpass
00122 
00123 //initializing Encoders
00124 QEI Enc1(D13,D12, NC , counts, QEI::X4_ENCODING); //Motor 1 encoder
00125 QEI Enc2(D11,D10, NC , counts, QEI::X4_ENCODING); // Motor 3 encoder this checks whetehter the motor has rotated
00126 
00127 //Functions
00128 void filteren ()
00129 {
00130     double emgSignal1 = emg1.read();
00131     double emgSignal2 = emg2.read();
00132 
00133     double emgFiltered1 = bqc.step(emgSignal1);
00134     double emgFiltered2 = fabs(emgFiltered1);
00135     emgFiltered3 = bqc2.step(emgFiltered2);
00136 
00137     double emgFiltered21 =  bqc3.step(emgSignal2);
00138     double emgFiltered22 = fabs(emgFiltered21);
00139     emgFiltered23 = bqc4.step(emgFiltered22);
00140 
00141 }
00142 
00143 void Position1x(double b)
00144 {
00145     if (b > threshholdx) {
00146         Led2 = 0;
00147         i = 0;
00148         Cxx =x1;
00149         pc.printf("Moving in the x direction\n\r");
00150         if (dir == true) {
00151             if(x1 > -45.7) {
00152                 x1 = x1-4.2;
00153 
00154 
00155             } else if ( x1 <= -45.7) {
00156                 x1 =-17;
00157 
00158             } else {
00159             }
00160         } else {
00161             if(x1 < -17) {
00162                 x1 = x1+4.2;
00163 
00164             } else if ( x1 >= -17) {
00165                 x1 = -45.7;
00166             } else {
00167             }
00168         }
00169         pc.printf("%f",x1);
00170     }
00171 }
00172 
00173 void Position1y(double b)
00174 {
00175     if (b > threshholdy) {
00176         Led2 = 0;
00177         i = 0;
00178         Cyy=y1;
00179         pc.printf("Moving in the y direction\r\n");
00180         if(dir == true) {
00181             if(y1 < 31.7) {
00182                 y1 = y1+4.1;
00183                 //return y1;
00184             } else if ( y1 >= 31.7) {
00185                 y1 = 3;
00186                 //return y1;
00187             } else {
00188             }
00189         } else {
00190             if(y1 > 3) {
00191                 y1 = y1-4.1;
00192                 //return y1;
00193             } else if ( y1 <= 3) {
00194                 y1 = 31.7;
00195                 //return y1;
00196             } else {
00197             }
00198         }
00199         pc.printf("%f",y1);
00200     }
00201 }
00202 void change()
00203 {
00204     dir = !dir;
00205 }
00206 void NR() //Newton Rapshon Calculation
00207 {
00208     //Variables
00209     double Hoa = X[0][0];
00210     double Hob = X[1][0];
00211 
00212     double meuk1 = cos(Hoa) * A - ((E + C)/E) * (cos(Hob)*D - ex) - ex;
00213     double meuk2 = sin(Hoa) * A - ((E + C)/E) * (sin(Hob)*D - ey) - ey;
00214 
00215     //Define f(x)
00216     fval[0][0] = pow((ex - D * cos(Hob)),2) + pow((ey - D * sin(Hob)),2) - pow((E),2);
00217     fval[1][0] = pow((meuk1),2) + pow((meuk2),2) - pow((B),2);
00218     //Jacobian
00219 
00220 
00221 
00222     J[0][0]= 0;
00223     J[0][1]= 2 * D * sin(Hob) * (ex - D * cos(Hob)) - 2 * D * cos(Hob) * (ey - D * sin(Hob));
00224     J[1][0]=  - 2 * A * sin(Hoa) * meuk1 + 2 * A * cos(Hoa)* meuk2;
00225     J[1][1]= 2 * ((E + C)/E) * D * sin(Hob) * meuk1 - 2 * ((E + C)/E) * D * cos(Hob) * meuk2;
00226 }
00227 
00228 void angle_define() //define the angle needed.
00229 {
00230     for(int i=1 ; i <= MaxIter; i++) {
00231         NR();
00232 
00233         X[0][0] = X[0][0] - ((-J[1][1]/(J[0][1] *J[1][0]))*fval[0][0] + (1/J[1][0])* fval[0][1]);
00234         X[1][0] = X[1][0] - ((1/J[0][1])*fval[0][0]);
00235 
00236         err[0][0] = abs(X[0][0] - Xold[0][0]);
00237         err[1][0] = abs(X[1][0] - Xold[1][0]);
00238 
00239         Xold[0][0] = X[0][0];
00240         Xold[1][0] = X[1][0];
00241 
00242         counts_a = ((X[0][0]) / (2* pi)) * 8400;
00243         counts_b = ((X[1][0]) / (2* pi)) * 8400;
00244 
00245         if(err[0][0] <= tolX) {
00246             if(err[1][0] <= tolX) {
00247                 break;
00248             }
00249         }
00250     }
00251 }
00252 
00253 void position_define()
00254 {
00255     if (ex >= Cxx - 0.01 && ex <= Cxx + 0.01) {
00256         if (ey >= Cyy - 0.01 && ey <= Cyy + 0.01) {
00257         } else {
00258             if (ey > Cyy) {
00259                 ey = ey - 0.004;
00260             }
00261             if (ey < Cyy) {
00262                 ey = ey + 0.004;
00263             }
00264         }
00265     } else {
00266         if (ex > Cxx) {
00267             ex = ex - 0.004;
00268         }
00269         if (ex < Cxx) {
00270             ex = ex + 0.004;
00271         }
00272     }
00273 }
00274 
00275 
00276 
00277 
00278 
00279 
00280 
00281 void position_controll_void()
00282 {
00283     bas = true;
00284 }
00285 
00286 
00287 void motor_controler()
00288 {
00289     bb = -(Enc1.getPulses()) - 816;
00290     bc = Enc2.getPulses() + 4316;
00291 
00292     if (bb >= counts_a) {
00293         z = PID_controller((counts_a - bb),Kp, Ki, Kd, Ts);
00294         PMW1.write(abs(z));
00295         M1 = 1;
00296     }
00297     if (bb <= counts_a) {
00298         z = PID_controller((counts_a - bb),Kp, Ki, Kd, Ts);
00299         PMW1.write(abs(z));
00300         M1 = 0;
00301     }
00302     if (bc >= counts_b) {
00303         M2 = 0;
00304         z = PID_controller((counts_b - bc),Kp, Ki, Kd, Ts);
00305         PMW2.write(abs(z));
00306     }
00307     if (bc <= counts_b) {
00308         M2 = 1;
00309         z = PID_controller((counts_b - bc),Kp, Ki, Kd, Ts);
00310         PMW2.write(abs(z));
00311     }
00312 }
00313 
00314 
00315 void change_wait()
00316 {
00317     waiting = waiting++;
00318 }
00319 
00320 
00321 void initializeren()
00322 {
00323     waiting = 1;
00324     while(waiting <=2)
00325         if (bas == true) {
00326             if (waiting == 1) {
00327                 Cxx = -17;
00328                 Cyy = 3;
00329 
00330                 position_define();
00331                 angle_define();
00332                 motor_controler();
00333 
00334             }
00335 
00336             if(waiting == 2) {
00337                 Cxx = -45.8;
00338                 Cyy = 3;
00339                 position_define();
00340                 angle_define();
00341                 motor_controler();
00342             }
00343         }
00344 }
00345 
00346 void setCalibration()
00347 {
00348     if (startCalc == false) {
00349 
00350         if (calpos1 == false) {
00351             while(abs(count2-count1) > 0) {
00352                 PMW1.write(0.1f);
00353                 wait(0.1);
00354                 PMW1.write(0);
00355                 count2 = count1;
00356                 count1 = Enc1.getPulses();
00357             }
00358             Enc1.reset();
00359             bb= Enc1.getPulses();
00360             calpos1 = true;
00361         }
00362         if(calpos2 == false) {
00363             while(abs(count4-count3) > 0) {
00364                 PMW2.write(0.1f);
00365                 M1=0;
00366                 PMW1.write(0.1f);
00367                 wait(0.1);
00368                 PMW2.write(0);
00369                 PMW1.write(0);
00370                 count4 = count3;
00371                 count3 = Enc2.getPulses();
00372             }
00373             Enc2.reset();
00374             bc= Enc2.getPulses();
00375             calpos2 = true;
00376         }
00377     }
00378 }
00379 
00380 void emgCalibration()
00381 {
00382     calibration = true;
00383     sumemgx = 0;
00384     sumemgy = 0;
00385 }
00386 
00387 void emgprint()
00388 {
00389     pc.printf("%f %f %f %f ", threshholdx,threshholdy,sumemgx,sumemgy) ;
00390 }
00391 
00392 
00393 int main()
00394 {
00395     pc.baud(115200);
00396     pc.printf("Hello World!\r\n");
00397     Led = 1;
00398     Led2 = 0;
00399     M1 = 1;
00400     M2 = 1;
00401     startCalc = false;
00402     calpos1 = false;
00403     calpos2 = false;
00404     count2 = 10000;
00405     count4 = 10000;
00406     Led = 1;
00407     PMW1.write(0.1f);
00408     wait(0.1);
00409     PMW1.write(0);
00410     count1 = Enc1.getPulses();
00411     PMW2.write(0.1f);
00412     wait(0.1);
00413     PMW2.write(0);
00414     count3 = Enc2.getPulses();
00415     emgbutton.fall(&emgCalibration);
00416     emgbutton2.fall(&emgprint);
00417     setCalibration();
00418     pc.printf("Calibration is done\r\n");
00419     pc.printf("Please press button SW3\n\r");
00420     Led2 = 1;
00421     button.fall(&change_wait);
00422     PMW1.period_us(60);
00423     PMW2.period_us(60);
00424     position_controll.attach(position_controll_void,0.002);
00425     X[0][0] = X0[0][0];
00426     X[1][0] = X0[1][0];
00427     Xold[0][0] = X0[0][0];
00428     Xold[1][0] = X0[1][0];
00429     initializeren();
00430     pc.printf("Initialization step done. Enjoy your game!\n\r");
00431     x1 = Cxx;
00432     y1= Cyy;
00433     bqc.add( &bq1 ).add( &bq2 );
00434     bqc2.add( &bq3 );
00435     bqc3.add( &bq4 ).add( &bq5 );
00436     bqc4.add( &bq6 );
00437     Cxx = -35;
00438     Cyy = 27;
00439     i = 251;
00440     while(true) {
00441 
00442 
00443 
00444         if(bas == true) {
00445             Led = 1;
00446             if(calibration == true) {
00447                 Led2 = 0;
00448                 filteren();
00449 
00450                 if (countsemg <= 2500) {
00451                     sumemgx = sumemgx + emgFiltered3;
00452                     sumemgy = sumemgy + emgFiltered23;
00453                     countsemg ++;
00454                // scope.set(0, emgFiltered3); // filtered 1
00455                 //scope.set(1, emgFiltered23); //filtered signal 2
00456                 
00457                 //scope.send();
00458                 } else {
00459                     calibration = false;
00460                     countsemg = 0;
00461                 }
00462 
00463             }
00464                 if (countsemg == 2500)
00465                 {
00466                 threshholdx = (sumemgx / 2500) * 0.9;
00467                 threshholdy = (sumemgy / 2500)  * 0.9;
00468                 }
00469 
00470 
00471                 Led2 = 1;
00472                 filteren();
00473                 position_define();
00474                 angle_define();
00475                 motor_controler();
00476 
00477                 //scope.set(0, ex); // filtered 1
00478                 //scope.set(1, Cxx); //filtered signal 2
00479                 //scope.set(2, emgFiltered3);
00480                 //scope.set(3, emgFiltered23);
00481                 //scope.send();
00482                 
00483 
00484                 if (i <= 250) {
00485                     emgFiltered3 = 0;
00486                     emgFiltered23 = 0;
00487                     i++;
00488                 }
00489 
00490 
00491                 sw2.fall(change);
00492                 Position1x(emgFiltered3);
00493                 Position1y(emgFiltered23);
00494                 Cxx = x1;
00495                 Cyy = y1;
00496                 Led2 = 1;
00497                 Led = 0;
00498                 bas= false;
00499             
00500         }
00501     }
00502 }
00503