File content as of revision 5:81d3b53087c0:

#include "mbed.h"
#include "math.h"
#include "encoder.h"
#include "QEI.h"
#include "BiQuad.h"
 
Serial          pc(USBTX, USBRX);
 
//Defining all in- and outputs
//EMG input
AnalogIn        emgBR( A0 );    //Right Biceps
AnalogIn        emgBL( A1 );    //Left Biceps
 
//Output motor 1 and reading Encoder motor 1
DigitalOut      motor1DirectionPin(D4);
PwmOut          motor1MagnitudePin(D5);
QEI             Encoder1(D12,D13,NC,32);
  
//Output motor 2 and reading Encoder motor 2
DigitalOut      motor2DirectionPin(D7);
PwmOut          motor2MagnitudePin(D6);
QEI             Encoder2(D10,D11,NC,32);

//Output motor 3 and reading Encoder motor 3
DigitalOut      motor3DirectionPin(D8);
PwmOut          motor3MagnitudePin(D9);
QEI             Encoder3(D2,D3,NC,32);
 
//LED output, needed for feedback
DigitalOut      led_R(LED_RED);
DigitalOut      led_G(LED_GREEN);
DigitalOut      led_B(LED_BLUE);
 
//Setting Tickers for sampling EMG and determing if the threshold is met
Ticker          sample_timer;
Ticker          threshold_timerR;
Ticker          threshold_timerL;
 
Timer           t_thresholdR;
Timer           t_thresholdL;
 
double          currentTimeTR;
double          currentTimeTL;
 
InterruptIn     button(SW2); // Wordt uiteindelijk vervangen door EMG
 
Timer           t;
double          speedfactor; // = 0.01; snelheid in, zonder potmeter gebruik            <- waarom is dit zo?
 
// Getting the counts from the Encoder
int             counts1 = Encoder1.getPulses();
int             counts2 = Encoder2.getPulses();
int             counts3 = Encoder3.getPulses();
 
// Defining variables delta (the difference between position and desired position)      <- Is dit zo?
int             delta1;
int             delta2; 
int             delta3;

// Boolean needed to know if new input coordinates have to be given 
bool            Move_done = false;
 
/* Defining all the different BiQuad filters, which contain a Notch filter,
High-pass filter and Low-pass filter. The Notch filter cancels all frequencies
between 49 and 51 Hz, the High-pass filter cancels all frequencies below 20 Hz
and the Low-pass filter cancels out all frequencies below 4 Hz. The filters are
declared four times, so that they can be used for sampling of right and left
biceps, during measurements and calibration. */
 
/* Defining all the normalized values of b and a in the Notch filter for the
creation of the Notch BiQuad */
 
BiQuad          bqNotch1( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
BiQuad          bqNotch2( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
 
BiQuad          bqNotchTR( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
BiQuad          bqNotchTL( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
 
/* Defining all the normalized values of b and a in the High-pass filter for the
creation of the High-pass BiQuad */
 
BiQuad          bqHigh1( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
BiQuad          bqHigh2( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
 
BiQuad          bqHighTR( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
BiQuad          bqHighTL( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
 
/* Defining all the normalized values of b and a in the Low-pass filter for the
creation of the Low-pass BiQuad */
 
BiQuad          bqLow1( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
BiQuad          bqLow2( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
 
BiQuad          bqLowTR( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
BiQuad          bqLowTL( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
 
// Creating a variable needed for the creation of the BiQuadChain
BiQuadChain     bqChain1;
BiQuadChain     bqChain2;
 
BiQuadChain     bqChainTR;
BiQuadChain     bqChainTL;
 
//Declaring all doubles needed in the filtering process
double          emgBRfiltered;   //Right biceps Notch+High pass filter
double          emgBRrectified;  //Right biceps rectified
double          emgBRcomplete;   //Right biceps low-pass filter, filtering complete
 
double          emgBLfiltered;   //Left biceps Notch+High pass filter
double          emgBLrectified;  //Left biceps rectified
double          emgBLcomplete;   //Left biceps low-pass filter, filtering complete

// Declaring all variables needed for getting the Threshold value 
double          numsamples = 500;
double          emgBRsum = 0;
double          emgBRmeanMVC;
double          thresholdBR;
 
double          emgBLsum = 0;
double          emgBLmeanMVC;
double          thresholdBL;
 
/* Function to sample the EMG of the Right Biceps and get a Threshold value 
from it, which can be used throughout the process */
 
void Threshold_samplingBR() {
    t_thresholdR.start();
    currentTimeTR = t_thresholdR.read();
    
    if (currentTimeTR <= 1) {
           
        emgBRfiltered = bqChainTR.step( emgBR.read() );   //Notch+High-pass
        emgBRrectified = fabs(emgBRfiltered);            //Rectification
        emgBRcomplete = bqLowTR.step(emgBRrectified);     //Low-pass
    
        emgBRsum = emgBRsum + emgBRcomplete;
        }
    emgBRmeanMVC = emgBRsum/numsamples; 
    thresholdBR = emgBRmeanMVC * 0.20;
       
    //pc.printf("ThresholdBR = %f \n", thresholdBR);
}
/* Function to sample the EMG of the Left Biceps and get a Threshold value 
from it, which can be used throughout the process */

void Threshold_samplingBL() {
    t_thresholdL.start();  
    currentTimeTL = t_thresholdL.read();
    
    if (currentTimeTL <= 1) {
            
        emgBLfiltered = bqChain2.step( emgBL.read() );    //Notch+High-pass
        emgBLrectified = fabs( emgBLfiltered );           //Rectification
        emgBLcomplete = bqLow2.step( emgBLrectified );    //Low-pass
        
        emgBLsum = emgBLsum + emgBLcomplete;
        }
        
    emgBLmeanMVC = emgBLsum/numsamples;
    thresholdBL = emgBLmeanMVC * 0.20;
 
}
 
// EMG sampling and filtering

void EMG_sample()
{
    //Filtering steps for the Right Biceps EMG
    emgBRfiltered = bqChain1.step( emgBR.read() );   //Notch+High-pass
    emgBRrectified = fabs(emgBRfiltered);            //Rectification
    emgBRcomplete = bqLow1.step(emgBRrectified);     //Low-pass
      
    //Filtering steps for the Left Biceps EMG
    emgBLfiltered = bqChain2.step( emgBL.read() );    //Notch+High-pass
    emgBLrectified = fabs( emgBLfiltered );           //Rectification
    emgBLcomplete = bqLow2.step( emgBLrectified );    //Low-pass
 
}
// Function to make the BiQuadChain for the Notch and High pass filter for all three filters    
void getbqChain()
{
    bqChain1.add(&bqNotch1).add(&bqHigh1);                 //Making the BiQuadChain
    bqChain2.add(&bqNotch2).add(&bqHigh2);
    
    bqChainTR.add(&bqNotchTR).add(&bqHighTR);
    bqChainTL.add(&bqNotchTR).add(&bqHighTL);
}
 
// Initial input value for couting the X-values                                                           
int Xin=0;  
int Xin_new;                                                                           
double huidigetijdX;
 
// Feedback system for counting values of X
void ledtX(){
    t.reset();
    Xin++;
    pc.printf("Xin is %i\n",Xin);
    led_G=0;
    led_R=1;
    wait(0.2);
    led_G=1;
    led_R=0;
    wait(0.5);
}  
 
// Couting system for values of X
int tellerX(){
    if (Move_done == true) {
        t.reset();
        led_G=1; 
        led_B=1;
        led_R=0;
        while(true){
        //button.fall(ledtX);    
        if (emgBRcomplete > thresholdBR) {
        ledtX();
        }                       
        t.start();
        huidigetijdX=t.read();
        if (huidigetijdX>2){
            led_R=1;                //Go to the next program (counting values for Y)
            Xin_new = Xin;
            Xin = 0;
                      
            return Xin_new;
            }
            
            }
            
    } 
    return 0; 
}  
 
// Initial values needed for Y (see comments at X function) 
int Yin=0;
int Yin_new;
double huidigetijdY;
 
//Feedback system for couting values of Y
void ledtY(){
    t.reset();
    Yin++;
    pc.printf("Yin is %i\n",Yin);
    led_G=0;
    led_B=1;
    wait(0.2);
    led_G=1;
    led_B=0;
    wait(0.5);
}  
 
// Couting system for values of Y
int tellerY(){
    if (Move_done == true) {
    t.reset();
    led_G=1;
    led_B=0; 
    led_R=1;
    while(true){
    //button.fall(ledtY);  
    if (emgBRcomplete > thresholdBR) {
        ledtY();
        }
    t.start();
    huidigetijdY=t.read();
    if (huidigetijdY>2){
        led_B=1; 
        Yin_new = Yin;
        Yin = 0;
                
        Move_done = false;
        return Yin_new;
        
        }
    }
    }
    return 0;      // ga door naar het volgende programma 
}
 
 
 
 // Oude shit voor input waardes geven
/*---------------------------------------------------------------------------------
// Feedback system for counting values of X
void ledtX(){
    t.reset();
    Xin++;
    pc.printf("Xin is %i\n",Xin);
    led_G=0;
    led_R=1;
    wait(0.2);
    led_G=1;
    led_R=0;
    wait(0.5);
}  


// Couting system for values of X
int tellerX(){
    led_G=1; 
    led_B=1;
    led_R=0;
        while(true){
    //button.fall(ledtX);          //This has to be replaced by EMG   
    if (emgBRcomplete > thresholdBR){
        ledtX();                 // dit is wat je uiteindelijk wil dat er staat
    }
    t.start();
    huidigetijdX=t.read();
    if (huidigetijdX>2){
        led_R=1;                //Go to the next program (couting values for Y)
        if (emgBRcomplete > thresholdBR){
        0;                 // dit is wat je uiteindelijk wil dat er staat
        }
        return 0;
        }
    }  
}  

// Initial values needed for Y (see comments at X function) 
int Yin=0;
double huidigetijdY;

//Feedback system for couting values of Y
void ledtY(){
    t.reset();
    Yin++;
    pc.printf("Yin is %i\n",Yin);
    led_G=0;
    led_B=1;
    wait(0.2);
    led_G=1;
    led_B=0;
    wait(0.5);
}  

// Couting system for values of Y
int tellerY(){
    t.reset();
    led_G=1;
    led_B=0; 
    led_R=1;
    while(true){
    //button.fall(ledtY);         //See comments at X   
    if (emgBRcomplete > thresholdBR){
        ledtY();                 // dit is wat je uiteindelijk wil dat er staat
    }
    t.start();
    huidigetijdY=t.read();
    if (huidigetijdY>2){
        led_B=1; 
        if (emgBRcomplete > thresholdBR){
        0;                 // dit is wat je uiteindelijk wil dat er staat
        }
        
        //button.fall(0);   // Wat is deze?
        return 0;      // ga door naar het volgende programma 
        }
    }
}

*/

 
// Declaring all variables needed for calculating rope lengths, 
double          Pox = 0;
double          Poy = 0;
double          Pbx = 0;
double          Pby = 887;
double          Prx = 768;
double          Pry = 443;
double          Pex=121;
double          Pey=308;
double          diamtrklosje=20;
double          pi=3.14159265359;
double          omtrekklosje=diamtrklosje*pi;
double          Lou;
double          Lbu;
double          Lru;
double          dLod;
double          dLbd;
double          dLrd;
 
// Declaring variables needed for calculating motor counts
double          roto;
double          rotb;
double          rotr;
double          rotzo;
double          rotzb;
double          rotzr;
double          counto;
double          countb;
double          countr;
double          countzo;
double          countzb;
double          countzr;
 
double          hcounto;
double          dcounto;
double          hcountb;
double          dcountb;
double          hcountr;
double          dcountr;
 
// Declaring variables neeeded for calculating motor movements to get to a certain point        <- klopt dit?
double          Psx;
double          Psy;
double          Vex;
double          Vey;
double          Kz=0.7; // nadersnelheid instellen
double          modVe;
double          Vmax=20;
double          Pstx;
double          Psty;
double          T=0.02;//seconds
 
double          speedfactor1;
double          speedfactor2;
double          speedfactor3;
 
 
//Deel om motor(en) aan te sturen--------------------------------------------
 
   
 
void calcdelta1() {    
    delta1 = (dcounto - Encoder1.getPulses());
    }
 
void calcdelta2() {    
    delta2 = (dcountb - Encoder2.getPulses());              //  <------- de reden dat de delta negatief is (jitse)
    }
    
void calcdelta3() {    
    delta3 = (dcountr - Encoder3.getPulses());              //  <------- de reden dat de delta negatief is (jitse)
    }    
 
double          referenceVelocity1; 
double          motorValue1; 
 
double          referenceVelocity2; 
double          motorValue2; 

double          referenceVelocity3; 
double          motorValue3; 
 
Ticker          controlmotor1; // één ticker van maken?
Ticker          calculatedelta1; 
Ticker          printdata1;     //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
 
Ticker          controlmotor2; // één ticker van maken?
Ticker          calculatedelta2; 
Ticker          printdata2;     //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes

Ticker          controlmotor3; // één ticker van maken?
Ticker          calculatedelta3; 
Ticker          printdata3;     //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
 
double GetReferenceVelocity1()
{
    // Returns reference velocity in rad/s. Positive value means clockwise rotation.
    double maxVelocity1=Vex*25+Vey*25; // max 8.4 in rad/s of course!       
    referenceVelocity1 = (-1)*speedfactor1 * maxVelocity1;  
                
    if (dcounto < (10))
        {   speedfactor1 = 0.01;
            if (dcounto > (-10))
                { printf("kleiner111111111");
                speedfactor1=0;
                }
        }
        else if (dcounto > (-10))
        {   speedfactor1 = -0.01;
             if (dcounto < (10))
                { printf("groter");
                speedfactor1=0;
                }
        }
        else 
        {   speedfactor1 = 0;
        pc.printf("speedfactor nul;");
        }
        
    return referenceVelocity1;
}         
 
double GetReferenceVelocity2()
{
    // Returns reference velocity in rad/s. Positive value means clockwise rotation.
    double maxVelocity2=Vex*25+Vey*25; // max 8.4 in rad/s of course!       
    referenceVelocity2 = (-1)*speedfactor2 * maxVelocity2;  
                
    if (Encoder2.getPulses() < (dcountb+10))
        {   speedfactor2 = -0.01;
            if (Encoder2.getPulses() > (dcountb-10))
                { //printf("kleiner22222222222");
                speedfactor2=0;
                }
        }
        else if (Encoder2.getPulses() > (dcountb-10))
        {   speedfactor2 = 0.01;
             if (Encoder2.getPulses() < (dcountb+10))
                { //printf("groter");
                speedfactor2=0;
                }
        }
        else 
        {   speedfactor2 = 0;
        //pc.printf("speedfactor nul;");
        }
        
    return referenceVelocity2;
} 

double GetReferenceVelocity3()
{
    // Returns reference velocity in rad/s. Positive value means clockwise rotation.
    double maxVelocity3=Vex*25+Vey*25; // max 8.4 in rad/s of course!       
    referenceVelocity3 = (-1)*speedfactor3 * maxVelocity3;  
                
    if (Encoder3.getPulses() < (dcountr+10))
        {   speedfactor3 = -0.01;
            if (Encoder3.getPulses() > (dcountr-10))
                { //printf("kleiner22222222222");
                speedfactor3=0;
                }
        }
        else if (Encoder3.getPulses() > (dcountr-10))
        {   speedfactor3 = 0.01;
             if (Encoder3.getPulses() < (dcountr+10))
                { //printf("groter");
                speedfactor3=0;
                }
        }
        else 
        {   speedfactor3 = 0;
        //pc.printf("speedfactor nul;");
        }
        
    return referenceVelocity3;
} 
 
void SetMotor1(double motorValue1)
{
    // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes 
    // motor rotating clockwise. motorValues outside range are truncated to within range
    if (motorValue1 >=0) motor1DirectionPin=1;
        else motor1DirectionPin=0;
    if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
        else motor1MagnitudePin = fabs(motorValue1);
      
}
 
void SetMotor2(double motorValue2)
{
    // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes 
    // motor rotating clockwise. motorValues outside range are truncated to within range
    if (motorValue2 >=0) motor2DirectionPin=1;
        else motor2DirectionPin=0;
    if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
        else motor2MagnitudePin = fabs(motorValue2);
      
}

void SetMotor3(double motorValue3)
{
    // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes 
    // motor rotating clockwise. motorValues outside range are truncated to within range
    if (motorValue3 >=0) motor3DirectionPin=1;
        else motor3DirectionPin=0;
    if (fabs(motorValue3)>1) motor3MagnitudePin = 1;
        else motor3MagnitudePin = fabs(motorValue3);
      
}
 
double FeedForwardControl1(double referenceVelocity1)
{
    // very simple linear feed-forward control
    const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
    double motorValue1 = referenceVelocity1 / MotorGain;
    return motorValue1;  
}
 
double FeedForwardControl2(double referenceVelocity2)
{
    // very simple linear feed-forward control
    const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
    double motorValue2 = referenceVelocity2 / MotorGain;
    return motorValue2;  
}

double FeedForwardControl3(double referenceVelocity3)
{
    // very simple linear feed-forward control
    const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
    double motorValue3 = referenceVelocity3 / MotorGain;
    return motorValue3;  
}
        
void MeasureAndControl1()
{
    // This function measures the potmeter position, extracts a reference velocity from it, 
    // and controls the motor with a simple FeedForward controller. Call this from a Ticker.
    double referenceVelocity1 = GetReferenceVelocity1();
    double motorValue1 = FeedForwardControl1(referenceVelocity1);
    SetMotor1(motorValue1);
}
 
void MeasureAndControl2()
{
    // This function measures the potmeter position, extracts a reference velocity from it, 
    // and controls the motor with a simple FeedForward controller. Call this from a Ticker.
    double referenceVelocity2 = GetReferenceVelocity2();
    double motorValue2 = FeedForwardControl2(referenceVelocity2);
    SetMotor2(motorValue2);
}

void MeasureAndControl3()
{
    // This function measures the potmeter position, extracts a reference velocity from it, 
    // and controls the motor with a simple FeedForward controller. Call this from a Ticker.
    double referenceVelocity3 = GetReferenceVelocity3();
    double motorValue3 = FeedForwardControl3(referenceVelocity3);
    SetMotor3(motorValue3);
}
 
void readdata1()
    {   //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
        //pc.printf("Pulses_M1 = %i \r\n",Encoder1.getPulses());
        //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
        //pc.printf("Delta_M1 = %i \r\n",delta1);
    }
    
void readdata2()
    {   //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
        //pc.printf("Pulses_M2 = %i \r\n",Encoder2.getPulses());
        //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
        //pc.printf("Delta_M2 = %i \r\n",delta2);
    }
    
void readdata3()
    {   //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
        //pc.printf("Pulses_M2 = %i \r\n",Encoder3.getPulses());
        //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
        //pc.printf("Delta_M2 = %i \r\n",delta3);
    }
 
// einde deel motor------------------------------------------------------------------------------------
 
Ticker loop;
 
/*Calculates ropelengths that are needed to get to new positions, based on the 
set coordinates and the position of the poles */
double touwlengtes(){
    Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2));     
    Lbu=sqrt(pow((Pstx-Pbx),2)+pow((Psty-Pby),2));
    Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2));
    return 0;
}
 
/* Calculates rotations (and associated counts) of the motor to get to the 
desired new position*/
double turns(){
    
    roto=Lou/omtrekklosje; 
    rotb=Lbu/omtrekklosje; 
    rotr=Lru/omtrekklosje; 
    counto=roto*4200; 
    dcounto=counto-hcounto;
    pc.printf("dcounto = %f \n\r",dcounto);
    countb=rotb*4200;
    dcountb=countb-hcountb;
    pc.printf("dcountb = %f \n\r",dcountb);
    countr=rotr*4200;
    dcountr=countr-hcountr;
 
    return 0;
}
 
// Waar komen Pstx en Psty vandaan en waar staan ze voor?   En is dit maar voor een paal?  
double Pst(){
    Pstx=Pex+Vex*T;
    Psty=Pey+Vey*T;
    touwlengtes();
    Pex=Pstx;                  
    Pey=Psty;
    pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty);
    //pc.printf("met lengtes:\n\r Lo=%.2f Lb=%.2f Lr=%.2f\n\r",Lou,Lbu,Lru);
    turns();
    //pc.printf("rotatie per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",roto,rotb,rotr); 
    pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr);
    /*float R;
    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
    pc.printf("\n\r R=%f",R);*/
    return 0;
} 
 
//Calculating desired end position based on the EMG input                       <- Waarom maar voor een paal?
double Ps(){
    if (Move_done==true);
    Psx=(Xin_new)*30+121;
    Psy=(Yin_new)*30+308;     
    pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
    hcounto=4200*((sqrt(pow((Pex-Pox),2)+pow((Pey-Poy),2)))/omtrekklosje);
    hcountb=4200*((sqrt(pow((Pex-Pbx),2)+pow((Pey-Pby),2)))/omtrekklosje);
    hcountr=4200*((sqrt(pow((Pex-Prx),2)+pow((Pey-Pry),2)))/omtrekklosje);
    return 0;
}
 
// Rekent dit de snelheid uit waarmee de motoren bewegen?
void Ve(){
    Vex=Kz*(Psx-Pex);
    Vey=Kz*(Psy-Pey);
    modVe=sqrt(pow(Vex,2)+pow(Vey,2));
    if(modVe>Vmax){
        Vex=(Vex/modVe)*Vmax;
        Vey=(Vey/modVe)*Vmax;
    }
    Pst();
    //pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);
    if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){
        Move_done=true;
        loop.detach();
        }
}
 
// Calculating the desired position, so that the motors can go here
int calculator(){
    Ps();
    loop.attach(&Ve,0.02);
    return 0;
}
 
// Function which makes it possible to lower the end-effector to pick up a piece
void zakker(){
    while(1){
        wait(1);
    if(Move_done==true){    //misschien moet je hier als voorwaarden een delta is 1 zetten                               // hierdoor wacht dit programma totdat de beweging klaar is  
    dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou;    //dit is wat je motoren moeten doen om te zakken
    dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu;
    dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru;
    rotzo=dLod/omtrekklosje;
    rotzb=dLbd/omtrekklosje;
    rotzr=dLrd/omtrekklosje;
    countzo=rotzo*4200;
    countzb=rotzb*4200;
    countzr=rotzr*4200;
    
    //pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr);       // hier moet komen te staan hoe het zakken gaat
    }
    }
}
 
int main()
{
    pc.baud(115200);
    getbqChain();
    threshold_timerR.attach(&Threshold_samplingBR, 0.002);
    threshold_timerL.attach(&Threshold_samplingBL, 0.002);
    while(true){
        sample_timer.attach(&EMG_sample, 0.002);
        wait(2.5f);
        
        tellerX();
        tellerY();
        calculator();
        controlmotor1.attach(&MeasureAndControl1, 0.01);
        calculatedelta1.attach(&calcdelta1, 0.01);
        printdata1.attach(&readdata1, 0.5); 
        //controlmotor2.attach(&MeasureAndControl2, 0.01);
        //calculatedelta2.attach(&calcdelta2, 0.01);
        //printdata2.attach(&readdata2, 0.5); 
        //controlmotor3.attach(&MeasureAndControl3, 0.01);
        //calculatedelta3.attach(&calcdelta3, 0.01);
        //printdata3.attach(&readdata3, 0.5);
        //zakker();
        wait(5.0f);
        }

}