main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "encoder.h"
00004 #include "QEI.h"
00005 #include "BiQuad.h"
00006  
00007 Serial          pc(USBTX, USBRX);
00008  
00009 //Defining all in- and outputs
00010 //EMG input
00011 AnalogIn        emgBR( A0 );    //Right Biceps
00012 AnalogIn        emgBL( A1 );    //Left Biceps
00013  
00014 //Output motor 1 and reading Encoder motor 1
00015 DigitalOut      motor1DirectionPin(D4);
00016 PwmOut          motor1MagnitudePin(D5);
00017 QEI             Encoder1(D12,D13,NC,32);
00018   
00019 //Output motor 2 and reading Encoder motor 2
00020 DigitalOut      motor2DirectionPin(D7);
00021 PwmOut          motor2MagnitudePin(D6);
00022 QEI             Encoder2(D10,D11,NC,32);
00023 
00024 //Output motor 3 and reading Encoder motor 3
00025 DigitalOut      motor3DirectionPin(D8);
00026 PwmOut          motor3MagnitudePin(D9);
00027 QEI             Encoder3(D2,D3,NC,32);
00028  
00029 //LED output, needed for feedback
00030 DigitalOut      led_R(LED_RED);
00031 DigitalOut      led_G(LED_GREEN);
00032 DigitalOut      led_B(LED_BLUE);
00033  
00034 //Setting Tickers for sampling EMG and determing if the threshold is met
00035 Ticker          sample_timer;
00036 Ticker          threshold_timerR;
00037 Ticker          threshold_timerL;
00038  
00039 Timer           t_thresholdR;
00040 Timer           t_thresholdL;
00041  
00042 double          currentTimeTR;
00043 double          currentTimeTL;
00044  
00045 InterruptIn     button(SW2); // Wordt uiteindelijk vervangen door EMG
00046  
00047 Timer           t;
00048 double          speedfactor; // = 0.01; snelheid in, zonder potmeter gebruik            <- waarom is dit zo?
00049  
00050 // Getting the counts from the Encoder
00051 int             counts1 = Encoder1.getPulses();
00052 int             counts2 = Encoder2.getPulses();
00053 int             counts3 = Encoder3.getPulses();
00054  
00055 // Defining variables delta (the difference between position and desired position)      <- Is dit zo?
00056 int             delta1;
00057 int             delta2; 
00058 int             delta3;
00059 
00060 // Boolean needed to know if new input coordinates have to be given 
00061 bool            Move_done = false;
00062  
00063 /* Defining all the different BiQuad filters, which contain a Notch filter,
00064 High-pass filter and Low-pass filter. The Notch filter cancels all frequencies
00065 between 49 and 51 Hz, the High-pass filter cancels all frequencies below 20 Hz
00066 and the Low-pass filter cancels out all frequencies below 4 Hz. The filters are
00067 declared four times, so that they can be used for sampling of right and left
00068 biceps, during measurements and calibration. */
00069  
00070 /* Defining all the normalized values of b and a in the Notch filter for the
00071 creation of the Notch BiQuad */
00072  
00073 BiQuad          bqNotch1( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
00074 BiQuad          bqNotch2( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
00075  
00076 BiQuad          bqNotchTR( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
00077 BiQuad          bqNotchTL( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
00078  
00079 /* Defining all the normalized values of b and a in the High-pass filter for the
00080 creation of the High-pass BiQuad */
00081  
00082 BiQuad          bqHigh1( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
00083 BiQuad          bqHigh2( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
00084  
00085 BiQuad          bqHighTR( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
00086 BiQuad          bqHighTL( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
00087  
00088 /* Defining all the normalized values of b and a in the Low-pass filter for the
00089 creation of the Low-pass BiQuad */
00090  
00091 BiQuad          bqLow1( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
00092 BiQuad          bqLow2( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
00093  
00094 BiQuad          bqLowTR( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
00095 BiQuad          bqLowTL( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
00096  
00097 // Creating a variable needed for the creation of the BiQuadChain
00098 BiQuadChain     bqChain1;
00099 BiQuadChain     bqChain2;
00100  
00101 BiQuadChain     bqChainTR;
00102 BiQuadChain     bqChainTL;
00103  
00104 //Declaring all doubles needed in the filtering process
00105 double          emgBRfiltered;   //Right biceps Notch+High pass filter
00106 double          emgBRrectified;  //Right biceps rectified
00107 double          emgBRcomplete;   //Right biceps low-pass filter, filtering complete
00108  
00109 double          emgBLfiltered;   //Left biceps Notch+High pass filter
00110 double          emgBLrectified;  //Left biceps rectified
00111 double          emgBLcomplete;   //Left biceps low-pass filter, filtering complete
00112 
00113 // Declaring all variables needed for getting the Threshold value 
00114 double          numsamples = 500;
00115 double          emgBRsum = 0;
00116 double          emgBRmeanMVC;
00117 double          thresholdBR;
00118  
00119 double          emgBLsum = 0;
00120 double          emgBLmeanMVC;
00121 double          thresholdBL;
00122  
00123 /* Function to sample the EMG of the Right Biceps and get a Threshold value 
00124 from it, which can be used throughout the process */
00125  
00126 void Threshold_samplingBR() {
00127     t_thresholdR.start();
00128     currentTimeTR = t_thresholdR.read();
00129     
00130     if (currentTimeTR <= 1) {
00131            
00132         emgBRfiltered = bqChainTR.step( emgBR.read() );   //Notch+High-pass
00133         emgBRrectified = fabs(emgBRfiltered);            //Rectification
00134         emgBRcomplete = bqLowTR.step(emgBRrectified);     //Low-pass
00135     
00136         emgBRsum = emgBRsum + emgBRcomplete;
00137         }
00138     emgBRmeanMVC = emgBRsum/numsamples; 
00139     thresholdBR = emgBRmeanMVC * 0.20;
00140        
00141     //pc.printf("ThresholdBR = %f \n", thresholdBR);
00142 }
00143 /* Function to sample the EMG of the Left Biceps and get a Threshold value 
00144 from it, which can be used throughout the process */
00145 
00146 void Threshold_samplingBL() {
00147     t_thresholdL.start();  
00148     currentTimeTL = t_thresholdL.read();
00149     
00150     if (currentTimeTL <= 1) {
00151             
00152         emgBLfiltered = bqChain2.step( emgBL.read() );    //Notch+High-pass
00153         emgBLrectified = fabs( emgBLfiltered );           //Rectification
00154         emgBLcomplete = bqLow2.step( emgBLrectified );    //Low-pass
00155         
00156         emgBLsum = emgBLsum + emgBLcomplete;
00157         }
00158         
00159     emgBLmeanMVC = emgBLsum/numsamples;
00160     thresholdBL = emgBLmeanMVC * 0.20;
00161  
00162 }
00163  
00164 // EMG sampling and filtering
00165 
00166 void EMG_sample()
00167 {
00168     //Filtering steps for the Right Biceps EMG
00169     emgBRfiltered = bqChain1.step( emgBR.read() );   //Notch+High-pass
00170     emgBRrectified = fabs(emgBRfiltered);            //Rectification
00171     emgBRcomplete = bqLow1.step(emgBRrectified);     //Low-pass
00172       
00173     //Filtering steps for the Left Biceps EMG
00174     emgBLfiltered = bqChain2.step( emgBL.read() );    //Notch+High-pass
00175     emgBLrectified = fabs( emgBLfiltered );           //Rectification
00176     emgBLcomplete = bqLow2.step( emgBLrectified );    //Low-pass
00177  
00178 }
00179 // Function to make the BiQuadChain for the Notch and High pass filter for all three filters    
00180 void getbqChain()
00181 {
00182     bqChain1.add(&bqNotch1).add(&bqHigh1);                 //Making the BiQuadChain
00183     bqChain2.add(&bqNotch2).add(&bqHigh2);
00184     
00185     bqChainTR.add(&bqNotchTR).add(&bqHighTR);
00186     bqChainTL.add(&bqNotchTR).add(&bqHighTL);
00187 }
00188  
00189 // Initial input value for couting the X-values                                                           
00190 int Xin=0;  
00191 int Xin_new;                                                                           
00192 double huidigetijdX;
00193  
00194 // Feedback system for counting values of X
00195 void ledtX(){
00196     t.reset();
00197     Xin++;
00198     pc.printf("Xin is %i\n",Xin);
00199     led_G=0;
00200     led_R=1;
00201     wait(0.2);
00202     led_G=1;
00203     led_R=0;
00204     wait(0.5);
00205 }  
00206  
00207 // Couting system for values of X
00208 int tellerX(){
00209     if (Move_done == true) {
00210         t.reset();
00211         led_G=1; 
00212         led_B=1;
00213         led_R=0;
00214         while(true){
00215         //button.fall(ledtX);    
00216         if (emgBRcomplete > thresholdBR) {
00217         ledtX();
00218         }                       
00219         t.start();
00220         huidigetijdX=t.read();
00221         if (huidigetijdX>2){
00222             led_R=1;                //Go to the next program (counting values for Y)
00223             Xin_new = Xin;
00224             Xin = 0;
00225                       
00226             return Xin_new;
00227             }
00228             
00229             }
00230             
00231     } 
00232     return 0; 
00233 }  
00234  
00235 // Initial values needed for Y (see comments at X function) 
00236 int Yin=0;
00237 int Yin_new;
00238 double huidigetijdY;
00239  
00240 //Feedback system for couting values of Y
00241 void ledtY(){
00242     t.reset();
00243     Yin++;
00244     pc.printf("Yin is %i\n",Yin);
00245     led_G=0;
00246     led_B=1;
00247     wait(0.2);
00248     led_G=1;
00249     led_B=0;
00250     wait(0.5);
00251 }  
00252  
00253 // Couting system for values of Y
00254 int tellerY(){
00255     if (Move_done == true) {
00256     t.reset();
00257     led_G=1;
00258     led_B=0; 
00259     led_R=1;
00260     while(true){
00261     //button.fall(ledtY);  
00262     if (emgBRcomplete > thresholdBR) {
00263         ledtY();
00264         }
00265     t.start();
00266     huidigetijdY=t.read();
00267     if (huidigetijdY>2){
00268         led_B=1; 
00269         Yin_new = Yin;
00270         Yin = 0;
00271                 
00272         Move_done = false;
00273         return Yin_new;
00274         
00275         }
00276     }
00277     }
00278     return 0;      // ga door naar het volgende programma 
00279 }
00280  
00281  
00282  
00283  // Oude shit voor input waardes geven
00284 /*---------------------------------------------------------------------------------
00285 // Feedback system for counting values of X
00286 void ledtX(){
00287     t.reset();
00288     Xin++;
00289     pc.printf("Xin is %i\n",Xin);
00290     led_G=0;
00291     led_R=1;
00292     wait(0.2);
00293     led_G=1;
00294     led_R=0;
00295     wait(0.5);
00296 }  
00297 
00298 
00299 // Couting system for values of X
00300 int tellerX(){
00301     led_G=1; 
00302     led_B=1;
00303     led_R=0;
00304         while(true){
00305     //button.fall(ledtX);          //This has to be replaced by EMG   
00306     if (emgBRcomplete > thresholdBR){
00307         ledtX();                 // dit is wat je uiteindelijk wil dat er staat
00308     }
00309     t.start();
00310     huidigetijdX=t.read();
00311     if (huidigetijdX>2){
00312         led_R=1;                //Go to the next program (couting values for Y)
00313         if (emgBRcomplete > thresholdBR){
00314         0;                 // dit is wat je uiteindelijk wil dat er staat
00315         }
00316         return 0;
00317         }
00318     }  
00319 }  
00320 
00321 // Initial values needed for Y (see comments at X function) 
00322 int Yin=0;
00323 double huidigetijdY;
00324 
00325 //Feedback system for couting values of Y
00326 void ledtY(){
00327     t.reset();
00328     Yin++;
00329     pc.printf("Yin is %i\n",Yin);
00330     led_G=0;
00331     led_B=1;
00332     wait(0.2);
00333     led_G=1;
00334     led_B=0;
00335     wait(0.5);
00336 }  
00337 
00338 // Couting system for values of Y
00339 int tellerY(){
00340     t.reset();
00341     led_G=1;
00342     led_B=0; 
00343     led_R=1;
00344     while(true){
00345     //button.fall(ledtY);         //See comments at X   
00346     if (emgBRcomplete > thresholdBR){
00347         ledtY();                 // dit is wat je uiteindelijk wil dat er staat
00348     }
00349     t.start();
00350     huidigetijdY=t.read();
00351     if (huidigetijdY>2){
00352         led_B=1; 
00353         if (emgBRcomplete > thresholdBR){
00354         0;                 // dit is wat je uiteindelijk wil dat er staat
00355         }
00356         
00357         //button.fall(0);   // Wat is deze?
00358         return 0;      // ga door naar het volgende programma 
00359         }
00360     }
00361 }
00362 
00363 */
00364 
00365  
00366 // Declaring all variables needed for calculating rope lengths, 
00367 double          Pox = 0;
00368 double          Poy = 0;
00369 double          Pbx = 0;
00370 double          Pby = 887;
00371 double          Prx = 768;
00372 double          Pry = 443;
00373 double          Pex=121;
00374 double          Pey=308;
00375 double          diamtrklosje=20;
00376 double          pi=3.14159265359;
00377 double          omtrekklosje=diamtrklosje*pi;
00378 double          Lou;
00379 double          Lbu;
00380 double          Lru;
00381 double          dLod;
00382 double          dLbd;
00383 double          dLrd;
00384  
00385 // Declaring variables needed for calculating motor counts
00386 double          roto;
00387 double          rotb;
00388 double          rotr;
00389 double          rotzo;
00390 double          rotzb;
00391 double          rotzr;
00392 double          counto;
00393 double          countb;
00394 double          countr;
00395 double          countzo;
00396 double          countzb;
00397 double          countzr;
00398  
00399 double          hcounto;
00400 double          dcounto;
00401 double          hcountb;
00402 double          dcountb;
00403 double          hcountr;
00404 double          dcountr;
00405  
00406 // Declaring variables neeeded for calculating motor movements to get to a certain point        <- klopt dit?
00407 double          Psx;
00408 double          Psy;
00409 double          Vex;
00410 double          Vey;
00411 double          Kz=0.7; // nadersnelheid instellen
00412 double          modVe;
00413 double          Vmax=20;
00414 double          Pstx;
00415 double          Psty;
00416 double          T=0.02;//seconds
00417  
00418 double          speedfactor1;
00419 double          speedfactor2;
00420 double          speedfactor3;
00421  
00422  
00423 //Deel om motor(en) aan te sturen--------------------------------------------
00424  
00425    
00426  
00427 void calcdelta1() {    
00428     delta1 = (dcounto - Encoder1.getPulses());
00429     }
00430  
00431 void calcdelta2() {    
00432     delta2 = (dcountb - Encoder2.getPulses());              //  <------- de reden dat de delta negatief is (jitse)
00433     }
00434     
00435 void calcdelta3() {    
00436     delta3 = (dcountr - Encoder3.getPulses());              //  <------- de reden dat de delta negatief is (jitse)
00437     }    
00438  
00439 double          referenceVelocity1; 
00440 double          motorValue1; 
00441  
00442 double          referenceVelocity2; 
00443 double          motorValue2; 
00444 
00445 double          referenceVelocity3; 
00446 double          motorValue3; 
00447  
00448 Ticker          controlmotor1; // één ticker van maken?
00449 Ticker          calculatedelta1; 
00450 Ticker          printdata1;     //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
00451  
00452 Ticker          controlmotor2; // één ticker van maken?
00453 Ticker          calculatedelta2; 
00454 Ticker          printdata2;     //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
00455 
00456 Ticker          controlmotor3; // één ticker van maken?
00457 Ticker          calculatedelta3; 
00458 Ticker          printdata3;     //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
00459  
00460 double GetReferenceVelocity1()
00461 {
00462     // Returns reference velocity in rad/s. Positive value means clockwise rotation.
00463     double maxVelocity1=Vex*25+Vey*25; // max 8.4 in rad/s of course!       
00464     referenceVelocity1 = (-1)*speedfactor1 * maxVelocity1;  
00465                 
00466     if (dcounto < (10))
00467         {   speedfactor1 = 0.01;
00468             if (dcounto > (-10))
00469                 { printf("kleiner111111111");
00470                 speedfactor1=0;
00471                 }
00472         }
00473         else if (dcounto > (-10))
00474         {   speedfactor1 = -0.01;
00475              if (dcounto < (10))
00476                 { printf("groter");
00477                 speedfactor1=0;
00478                 }
00479         }
00480         else 
00481         {   speedfactor1 = 0;
00482         pc.printf("speedfactor nul;");
00483         }
00484         
00485     return referenceVelocity1;
00486 }         
00487  
00488 double GetReferenceVelocity2()
00489 {
00490     // Returns reference velocity in rad/s. Positive value means clockwise rotation.
00491     double maxVelocity2=Vex*25+Vey*25; // max 8.4 in rad/s of course!       
00492     referenceVelocity2 = (-1)*speedfactor2 * maxVelocity2;  
00493                 
00494     if (Encoder2.getPulses() < (dcountb+10))
00495         {   speedfactor2 = -0.01;
00496             if (Encoder2.getPulses() > (dcountb-10))
00497                 { //printf("kleiner22222222222");
00498                 speedfactor2=0;
00499                 }
00500         }
00501         else if (Encoder2.getPulses() > (dcountb-10))
00502         {   speedfactor2 = 0.01;
00503              if (Encoder2.getPulses() < (dcountb+10))
00504                 { //printf("groter");
00505                 speedfactor2=0;
00506                 }
00507         }
00508         else 
00509         {   speedfactor2 = 0;
00510         //pc.printf("speedfactor nul;");
00511         }
00512         
00513     return referenceVelocity2;
00514 } 
00515 
00516 double GetReferenceVelocity3()
00517 {
00518     // Returns reference velocity in rad/s. Positive value means clockwise rotation.
00519     double maxVelocity3=Vex*25+Vey*25; // max 8.4 in rad/s of course!       
00520     referenceVelocity3 = (-1)*speedfactor3 * maxVelocity3;  
00521                 
00522     if (Encoder3.getPulses() < (dcountr+10))
00523         {   speedfactor3 = -0.01;
00524             if (Encoder3.getPulses() > (dcountr-10))
00525                 { //printf("kleiner22222222222");
00526                 speedfactor3=0;
00527                 }
00528         }
00529         else if (Encoder3.getPulses() > (dcountr-10))
00530         {   speedfactor3 = 0.01;
00531              if (Encoder3.getPulses() < (dcountr+10))
00532                 { //printf("groter");
00533                 speedfactor3=0;
00534                 }
00535         }
00536         else 
00537         {   speedfactor3 = 0;
00538         //pc.printf("speedfactor nul;");
00539         }
00540         
00541     return referenceVelocity3;
00542 } 
00543  
00544 void SetMotor1(double motorValue1)
00545 {
00546     // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes 
00547     // motor rotating clockwise. motorValues outside range are truncated to within range
00548     if (motorValue1 >=0) motor1DirectionPin=1;
00549         else motor1DirectionPin=0;
00550     if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
00551         else motor1MagnitudePin = fabs(motorValue1);
00552       
00553 }
00554  
00555 void SetMotor2(double motorValue2)
00556 {
00557     // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes 
00558     // motor rotating clockwise. motorValues outside range are truncated to within range
00559     if (motorValue2 >=0) motor2DirectionPin=1;
00560         else motor2DirectionPin=0;
00561     if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
00562         else motor2MagnitudePin = fabs(motorValue2);
00563       
00564 }
00565 
00566 void SetMotor3(double motorValue3)
00567 {
00568     // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes 
00569     // motor rotating clockwise. motorValues outside range are truncated to within range
00570     if (motorValue3 >=0) motor3DirectionPin=1;
00571         else motor3DirectionPin=0;
00572     if (fabs(motorValue3)>1) motor3MagnitudePin = 1;
00573         else motor3MagnitudePin = fabs(motorValue3);
00574       
00575 }
00576  
00577 double FeedForwardControl1(double referenceVelocity1)
00578 {
00579     // very simple linear feed-forward control
00580     const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
00581     double motorValue1 = referenceVelocity1 / MotorGain;
00582     return motorValue1;  
00583 }
00584  
00585 double FeedForwardControl2(double referenceVelocity2)
00586 {
00587     // very simple linear feed-forward control
00588     const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
00589     double motorValue2 = referenceVelocity2 / MotorGain;
00590     return motorValue2;  
00591 }
00592 
00593 double FeedForwardControl3(double referenceVelocity3)
00594 {
00595     // very simple linear feed-forward control
00596     const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
00597     double motorValue3 = referenceVelocity3 / MotorGain;
00598     return motorValue3;  
00599 }
00600         
00601 void MeasureAndControl1()
00602 {
00603     // This function measures the potmeter position, extracts a reference velocity from it, 
00604     // and controls the motor with a simple FeedForward controller. Call this from a Ticker.
00605     double referenceVelocity1 = GetReferenceVelocity1();
00606     double motorValue1 = FeedForwardControl1(referenceVelocity1);
00607     SetMotor1(motorValue1);
00608 }
00609  
00610 void MeasureAndControl2()
00611 {
00612     // This function measures the potmeter position, extracts a reference velocity from it, 
00613     // and controls the motor with a simple FeedForward controller. Call this from a Ticker.
00614     double referenceVelocity2 = GetReferenceVelocity2();
00615     double motorValue2 = FeedForwardControl2(referenceVelocity2);
00616     SetMotor2(motorValue2);
00617 }
00618 
00619 void MeasureAndControl3()
00620 {
00621     // This function measures the potmeter position, extracts a reference velocity from it, 
00622     // and controls the motor with a simple FeedForward controller. Call this from a Ticker.
00623     double referenceVelocity3 = GetReferenceVelocity3();
00624     double motorValue3 = FeedForwardControl3(referenceVelocity3);
00625     SetMotor3(motorValue3);
00626 }
00627  
00628 void readdata1()
00629     {   //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
00630         //pc.printf("Pulses_M1 = %i \r\n",Encoder1.getPulses());
00631         //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
00632         //pc.printf("Delta_M1 = %i \r\n",delta1);
00633     }
00634     
00635 void readdata2()
00636     {   //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
00637         //pc.printf("Pulses_M2 = %i \r\n",Encoder2.getPulses());
00638         //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
00639         //pc.printf("Delta_M2 = %i \r\n",delta2);
00640     }
00641     
00642 void readdata3()
00643     {   //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
00644         //pc.printf("Pulses_M2 = %i \r\n",Encoder3.getPulses());
00645         //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
00646         //pc.printf("Delta_M2 = %i \r\n",delta3);
00647     }
00648  
00649 // einde deel motor------------------------------------------------------------------------------------
00650  
00651 Ticker loop;
00652  
00653 /*Calculates ropelengths that are needed to get to new positions, based on the 
00654 set coordinates and the position of the poles */
00655 double touwlengtes(){
00656     Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2));     
00657     Lbu=sqrt(pow((Pstx-Pbx),2)+pow((Psty-Pby),2));
00658     Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2));
00659     return 0;
00660 }
00661  
00662 /* Calculates rotations (and associated counts) of the motor to get to the 
00663 desired new position*/
00664 double turns(){
00665     
00666     roto=Lou/omtrekklosje; 
00667     rotb=Lbu/omtrekklosje; 
00668     rotr=Lru/omtrekklosje; 
00669     counto=roto*4200; 
00670     dcounto=counto-hcounto;
00671     pc.printf("dcounto = %f \n\r",dcounto);
00672     countb=rotb*4200;
00673     dcountb=countb-hcountb;
00674     pc.printf("dcountb = %f \n\r",dcountb);
00675     countr=rotr*4200;
00676     dcountr=countr-hcountr;
00677  
00678     return 0;
00679 }
00680  
00681 // Waar komen Pstx en Psty vandaan en waar staan ze voor?   En is dit maar voor een paal?  
00682 double Pst(){
00683     Pstx=Pex+Vex*T;
00684     Psty=Pey+Vey*T;
00685     touwlengtes();
00686     Pex=Pstx;                  
00687     Pey=Psty;
00688     pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty);
00689     //pc.printf("met lengtes:\n\r Lo=%.2f Lb=%.2f Lr=%.2f\n\r",Lou,Lbu,Lru);
00690     turns();
00691     //pc.printf("rotatie per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",roto,rotb,rotr); 
00692     pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr);
00693     /*float R;
00694     R=Vex/Vey;                  // met dit stukje kan je zien dat de verhouding tussen Vex en Vey constant is en de end efector dus een rechte lijn maakt
00695     pc.printf("\n\r R=%f",R);*/
00696     return 0;
00697 } 
00698  
00699 //Calculating desired end position based on the EMG input                       <- Waarom maar voor een paal?
00700 double Ps(){
00701     if (Move_done==true);
00702     Psx=(Xin_new)*30+121;
00703     Psy=(Yin_new)*30+308;     
00704     pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
00705     hcounto=4200*((sqrt(pow((Pex-Pox),2)+pow((Pey-Poy),2)))/omtrekklosje);
00706     hcountb=4200*((sqrt(pow((Pex-Pbx),2)+pow((Pey-Pby),2)))/omtrekklosje);
00707     hcountr=4200*((sqrt(pow((Pex-Prx),2)+pow((Pey-Pry),2)))/omtrekklosje);
00708     return 0;
00709 }
00710  
00711 // Rekent dit de snelheid uit waarmee de motoren bewegen?
00712 void Ve(){
00713     Vex=Kz*(Psx-Pex);
00714     Vey=Kz*(Psy-Pey);
00715     modVe=sqrt(pow(Vex,2)+pow(Vey,2));
00716     if(modVe>Vmax){
00717         Vex=(Vex/modVe)*Vmax;
00718         Vey=(Vey/modVe)*Vmax;
00719     }
00720     Pst();
00721     //pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);
00722     if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){
00723         Move_done=true;
00724         loop.detach();
00725         }
00726 }
00727  
00728 // Calculating the desired position, so that the motors can go here
00729 int calculator(){
00730     Ps();
00731     loop.attach(&Ve,0.02);
00732     return 0;
00733 }
00734  
00735 // Function which makes it possible to lower the end-effector to pick up a piece
00736 void zakker(){
00737     while(1){
00738         wait(1);
00739     if(Move_done==true){    //misschien moet je hier als voorwaarden een delta is 1 zetten                               // hierdoor wacht dit programma totdat de beweging klaar is  
00740     dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou;    //dit is wat je motoren moeten doen om te zakken
00741     dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu;
00742     dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru;
00743     rotzo=dLod/omtrekklosje;
00744     rotzb=dLbd/omtrekklosje;
00745     rotzr=dLrd/omtrekklosje;
00746     countzo=rotzo*4200;
00747     countzb=rotzb*4200;
00748     countzr=rotzr*4200;
00749     
00750     //pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr);       // hier moet komen te staan hoe het zakken gaat
00751     }
00752     }
00753 }
00754  
00755 int main()
00756 {
00757     pc.baud(115200);
00758     getbqChain();
00759     threshold_timerR.attach(&Threshold_samplingBR, 0.002);
00760     threshold_timerL.attach(&Threshold_samplingBL, 0.002);
00761     while(true){
00762         sample_timer.attach(&EMG_sample, 0.002);
00763         wait(2.5f);
00764         
00765         tellerX();
00766         tellerY();
00767         calculator();
00768         controlmotor1.attach(&MeasureAndControl1, 0.01);
00769         calculatedelta1.attach(&calcdelta1, 0.01);
00770         printdata1.attach(&readdata1, 0.5); 
00771         //controlmotor2.attach(&MeasureAndControl2, 0.01);
00772         //calculatedelta2.attach(&calcdelta2, 0.01);
00773         //printdata2.attach(&readdata2, 0.5); 
00774         //controlmotor3.attach(&MeasureAndControl3, 0.01);
00775         //calculatedelta3.attach(&calcdelta3, 0.01);
00776         //printdata3.attach(&readdata3, 0.5);
00777         //zakker();
00778         wait(5.0f);
00779         }
00780 
00781 }
00782