Diff: main.cpp

Revision:

15:3a09783b2406

Parent:

14:df5822cd0198

Child:

16:cd11083c754a

--- a/main.cpp	Thu Nov 02 20:42:54 2017 +0000
+++ b/main.cpp	Mon Nov 06 11:23:38 2017 +0000
@@ -1,22 +1,21 @@
 #include "mbed.h"
 #include "math.h"
-//#include "encoder.h"
 #include "QEI.h"
 #include "BiQuad.h"
 #include "MODSERIAL.h"
- 
+
 MODSERIAL       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);
@@ -26,142 +25,141 @@
 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 needed to determine the threshold for a pre-set time period
 Timer           t_thresholdR;
 Timer           t_thresholdL;
- 
-float          currentTimeTR;
-float          currentTimeTL;
- 
-InterruptIn     button(SW2); // Wordt uiteindelijk vervangen door EMG
+
+//Variables to store the current time in
+float           currentTimeTR;
+float           currentTimeTL;
+
+//This is used when testing without EMG
+InterruptIn     button(SW2); 
 InterruptIn     button2(SW3);
- 
+
+//Timer needed for timing EMG input for the X and Y coördinates
 Timer           t;
-float          speedfactor; // = 0.01; snelheid in, zonder potmeter gebruik            <- waarom is dit zo?
- 
-// 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 
+// Boolean needed to know if new input coordinates have to be given
 bool            Move_done = false;
 bool            Input_done = true;
- 
+
 /* 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 floats needed in the filtering process
 float          emgBRfiltered;   //Right biceps Notch+High pass filter
 float          emgBRrectified;  //Right biceps rectified
 float          emgBRcomplete;   //Right biceps low-pass filter, filtering complete
- 
+
 float          emgBLfiltered;   //Left biceps Notch+High pass filter
 float          emgBLrectified;  //Left biceps rectified
 float          emgBLcomplete;   //Left biceps low-pass filter, filtering complete
 
-// Declaring all variables needed for getting the Threshold value 
+// Declaring all variables needed for getting the Threshold value
 float          numsamples = 500;
 float          emgBRsum = 0;
 float          emgBRmeanMVC;
 float          thresholdBR;
- 
+
 float          emgBLsum = 0;
 float          emgBLmeanMVC;
 float          thresholdBL;
- 
-/* Function to sample the EMG of the Right Biceps and get a Threshold value 
+
+/* 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() {
+
+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; 
+    }
+    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 
+/* 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();  
+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()
@@ -171,30 +169,31 @@
     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    
+// 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;                                                                           
+
+// Initial input value for couting the X-values
+int Xin=0   ;
+int Xin_new;
 float huidigetijdX;
- 
+
 // Feedback system for counting values of X
-void ledtX(){
+void ledtX()
+{
     t.reset();
     Xin++;
     pc.printf("Xin is %i\n",Xin);
@@ -204,43 +203,45 @@
     led_G=1;
     led_R=0;
     wait(0.5);
-}  
- 
+}
+
 // Couting system for values of X
-int tellerX(){
+int tellerX()
+{
     if (Move_done == true) {
         t.reset();
-        led_G=1; 
+        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;
+        while(true) {
+            //button.fall(ledtX);   // this can be used for testing without EMG
+            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) 
+
+        }
+
+    }
+    return 0;
+}
+
+// Initial values needed for Y (see comments at X function)
 int Yin=0;
 int Yin_new;
 float huidigetijdY;
- 
+
 //Feedback system for couting values of Y
-void ledtY(){
+void ledtY()
+{
     t.reset();
     Yin++;
     pc.printf("Yin is %i\n",Yin);
@@ -250,47 +251,50 @@
     led_G=1;
     led_B=0;
     wait(0.5);
-}  
- 
+}
+
 // Couting system for values of Y
-int tellerY(){
+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;
-        Input_done = true;        
-        Move_done = false;
-        return Yin_new;
-        
+        t.reset();
+        led_G=1;
+        led_B=0;
+        led_R=1;
+        while(true) {
+            //button.fall(ledtY);   // this can be used for testing without EMG
+            if (emgBRcomplete > thresholdBR) {
+                ledtY();
+            }
+            t.start();
+            huidigetijdY=t.read();
+            if (huidigetijdY>2) {
+                led_B=1;
+                Yin_new = Yin;
+                Yin = 0;
+                Input_done = true;
+                Move_done = false;
+                return Yin_new;
+
+            }
         }
     }
-    }
-    return 0;      // ga door naar het volgende programma 
+    return 0;      // ga door naar het volgende programma
 }
- 
-// Declaring all variables needed for calculating rope lengths, 
+
+// Declaring all variables needed for calculating rope lengths,
+/* The following six floats were found using our SOLIDWORKS model*/
 float          Pox = 0;
 float          Poy = 0;
 float          Pbx = 0;
 float          Pby = 887;
 float          Prx = 768;
 float          Pry = 443;
-float          Pex=91;
-float          Pey=278;
+/* The end-effector is manually placed in this (see beneath) position*/
+float          Pex=91;          //initial value choosen for calibration
+float          Pey=278;         //initial value choosen for calibration
 float          diamtrklosje=20;
-float          pi=3.14159265359;
+float          pi=3.14159265359;    //M_PI didn't work for some reason
 float          omtrekklosje=diamtrklosje*pi;
 float          Lou;
 float          Lbu;
@@ -298,7 +302,7 @@
 float          dLod;
 float          dLbd;
 float          dLrd;
- 
+
 // Declaring variables needed for calculating motor counts
 float          roto;
 float          rotb;
@@ -322,21 +326,14 @@
 int position_r;
 int error_r;
 
+// Declaring variables neeeded for calculating motor movements to get to a certain point        <- klopt dit?
 float          hcounto;
-float          dcounto;
 float          hcountb;
-float          dcountb;
 float          hcountr;
-float          dcountr;
- 
-// Declaring variables neeeded for calculating motor movements to get to a certain point        <- klopt dit?
 float          Psx;
 float          Psy;
 float          Vex;
 float          Vey;
-float          Kz=0.7; // nadersnelheid instellen
-float          modVe;
-float          Vmax=20;
 float          Pstx;
 float          Psty;
 float          T=0.02;//seconds
@@ -344,201 +341,167 @@
 float          kpo = 21;
 float          kpb = 21;
 float          kpr = 21;
- 
-float          speedfactor1;
-float          speedfactor2;
-float          speedfactor3;
- 
- 
-//Deel om motor(en) aan te sturen-------------------------------------------- 
-float          motorValue1; 
+
+float          motorValue1;
 float          motorValue2;
-float          motorValue3; 
+float          motorValue3;
 
 Ticker          controlmotor1; // één ticker van maken?
 Ticker          controlmotor2; // één ticker van maken?
 Ticker          controlmotor3; // één ticker van maken?
 
-float P1(int erroro, float kpo) {
- return erroro*kpo;
- }
+//Deel om motor(en) aan te sturen--------------------------------------------
+// start Motor 1 ------------------------------------------------------------
+float P1(int error_o, float kpo)
+{
+    return error_o*kpo;
+}
 
 void MotorController1()
 {
-             reference_o = (int) (counto-hcounto);
-             position_o = Encoder1.getPulses();
-    
-             error_o = reference_o - position_o;
-    
-    //pc.printf("Position_o = %i          reference_o=%i              Error_o=%i\n\r" ,position_o,reference_o,error_o);
-    
-    if (-20<error_o && error_o<20){
+    reference_o = (int) (counto-hcounto);
+    position_o = Encoder1.getPulses();
+    error_o = reference_o - position_o;
+
+    if (-20<error_o && error_o<20) {
         motorValue1 = 0;
-        }
-    else {
-    motorValue1 = P1(error_o, kpo)/4200;
-    }    
-    
-    
-    
-    if (motorValue1 >=0) motor1DirectionPin=0;
-        else motor1DirectionPin=1;
+    } else {
+        motorValue1 = P1(error_o, kpo)/4200;
+    }
+/*differs from the other to due to the motor being on the opposite side of the pillar*/
+    if (motorValue1 >=0) motor1DirectionPin=0;       
+    else motor1DirectionPin=1;
     if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
-        else motor1MagnitudePin = fabs(motorValue1);
-    }
-      
-
-
-float P2(int error_b, float kpb) {
- return error_b*kpb;
- }
+    else motor1MagnitudePin = fabs(motorValue1);
+}
+// end Motor 1 --------------------------------------------------------------
+// start Motor 2 ------------------------------------------------------------
+float P2(int error_b, float kpb)
+{
+    return error_b*kpb;
+}
 
 void MotorController2()
 {
+
+    reference_b = (int) (-(countb-hcountb));
+    position_b = Encoder2.getPulses();
+    error_b = reference_b - position_b;
     
-     reference_b = (int) (-(countb-hcountb));
-     position_b = Encoder2.getPulses();
-
-     error_b = reference_b - position_b;
-    
-    //pc.printf("Position_b = %i         reference_b=%i         Error_b=%i      " ,position_b,reference_b,error_b);
-        
-    if (-20<error_b && error_b<20){
+    if (-20<error_b && error_b<20) {
         motorValue2 = 0;
-        }
-    else {
-    motorValue2 = P2(error_b, kpb)/4200;
-    }    
-        
-    
+    } else {
+        motorValue2 = P2(error_b, kpb)/4200;
+    }
+
     if (motorValue2 <=0) motor2DirectionPin=0;
-        else motor2DirectionPin=1;
+    else motor2DirectionPin=1;
     if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
-        else motor2MagnitudePin = fabs(motorValue2);
-    }
-    
+    else motor2MagnitudePin = fabs(motorValue2);
+}
+// end Motor 2 --------------------------------------------------------------
+// start Motor 3 ------------------------------------------------------------
 
 
-float P3(int error_r, float kpr) {
- return error_r*kpr;
- }
+float P3(int error_r, float kpr)
+{
+    return error_r*kpr;
+}
 
 void MotorController3()
 {
-     reference_r = (int) (-(countr-hcountr));
-     position_r = Encoder3.getPulses();
-        
-     error_r = reference_r - position_r;
-    
+    reference_r = (int) (-(countr-hcountr));
+    position_r = Encoder3.getPulses();
+    error_r = reference_r - position_r;
     pc.printf("%i; %i; %i\n\r" ,position_b,reference_b,error_b);
-    
-    
-    if (-20<error_r && error_r<20){
-        motorValue3 = 0;
-        
-        }
-    else {
-    motorValue3 = P3(error_r, kpr)/4200;
-    }    
-            
-    if (motorValue3 <=0) motor3DirectionPin=0;
-        else motor3DirectionPin=1;
-    if (fabs(motorValue3)>1) motor3MagnitudePin = 1;
-        else motor3MagnitudePin = fabs(motorValue3);
-        }
-        
 
 
-// einde deel motor------------------------------------------------------------------------------------
- 
+    if (-20<error_r && error_r<20) {
+        motorValue3 = 0;
+    } else {
+        motorValue3 = P3(error_r, kpr)/4200;
+    }
+
+    if (motorValue3 <=0) motor3DirectionPin=0;
+    else motor3DirectionPin=1;
+    if (fabs(motorValue3)>1) motor3MagnitudePin = 1;
+    else motor3MagnitudePin = fabs(motorValue3);
+}
+// end Motor 3 --------------------------------------------------------------
+// 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 */
-float touwlengtes(){
-    Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2));     
+
+/*Calculates ropelengths that are needed to get to new positions for each time 
+step, based on the set coordinates and the position of the poles */
+float 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*/
-float turns(){
-    
-    roto=Lou/omtrekklosje; 
-    rotb=Lbu/omtrekklosje; 
-    rotr=Lru/omtrekklosje; 
-    counto=roto*4200; 
-    dcounto=counto-hcounto;
-    //pc.printf("counto = %f \n\r", counto);
-    //pc.printf("hcounto = %f \n\r", hcounto);
-    //pc.printf("dcounto = %f \n\r",dcounto);
+
+/* Calculates rotations (and associated counts) of the motor to get to the
+desired new position for each time step*/
+float turns()
+{
+    roto=Lou/omtrekklosje;
+    rotb=Lbu/omtrekklosje;
+    rotr=Lru/omtrekklosje;
+    counto=roto*4200;
     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?  
-float Pst(){
-    Pstx=Pex+Vex*T;                                // ASDJFASDKGJKASGJASDGJLSDAGHJGDJKJHFGHJUIK<KTBYNUMI<OIUNYBTVRTBYNUMI<UMNYBTVfadhsgfjhghagfryestrjsghdbf
+
+//calculate the setpoint for each time step
+float Pst()
+{
+    Pstx=Pex+Vex*T;                               
     Psty=Pey+Vey*T;
     touwlengtes();
-    Pex=Pstx;                  
+    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?
-float Ps(){
+float Ps()
+{
     Psx=(Xin_new)*30+91;
-    Psy=(Yin_new)*30+278;     
- //   pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
+    Psy=(Yin_new)*30+278;
     return 0;
 }
- 
+
 // Rekent dit de snelheid uit waarmee de motoren bewegen?
-void Ve(){
+void Ve()
+{
     Vex=(Psx-Pex);
     Vey=(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((fabs(Vex)<0.01f)&&(fabs(Vey)<0.01f)){
+    if((fabs(Vex)<0.01f)&&(fabs(Vey)<0.01f)) {
         Move_done=true;
         loop.detach();
-        }
+    }
 }
- 
+
 // Calculating the desired position, so that the motors can go here
-int calculator(){
+int calculator()
+{
     Ps();
     if (Move_done == false) {
-    loop.attach(&Ve,0.02);
+        loop.attach(&Ve,0.02);
     }
     return 0;
 }
- 
+
 // Function which makes it possible to lower the end-effector to pick up a piece
-void zakker(){                                  
-    dLod=sqrt(pow(Lou,2)+pow((277.85),2))-Lou;    //dit is wat je motoren moeten doen om te zakken
-    dLbd=sqrt(pow(Lbu,2)+pow((277.85),2))-Lbu;    // dat laatste getal moet nog aangepast worden
+void zakker()
+{
+    dLod=sqrt(pow(Lou,2)+pow((277.85),2))-Lou;
+    dLbd=sqrt(pow(Lbu,2)+pow((277.85),2))-Lbu;    
     dLrd=sqrt(pow(Lru,2)+pow((277.85),2))-Lru;
     rotzo=dLod/omtrekklosje;
     rotzb=dLbd/omtrekklosje;
@@ -546,71 +509,47 @@
     countzo=rotzo*4200;
     countzb=rotzb*4200;
     countzr=rotzr*4200;
-    counto=countzo+hcounto+reference_o; //+ gemaakt van een min
-    countb=-(reference_b-countzb-hcountb);//(countzb)+hcountb+reference_b;
-    countr=-(reference_r-countzr-hcountr);//(countzr)+hcountr+reference_r;
-    
-    pc.printf("dLod=%.2f         dLbd=%.2f       dLrd=%.2f\n\r",dLod,dLbd,dLrd);
-    //pc.printf("hcounto=%.2f         hcountb=%.2f       hcountr=%.2f\n\r",hcounto,hcountb,hcountr);
-    pc.printf("o=%.2f  b=%.2f  countzr=%.2f",countzo,countzb,countzr);       // hier moet komen te staan hoe het zakken gaat
-    pc.printf("Position_r = %i;reference_r=%i              Error_r=%i\n\r" ,position_o,reference_o,error_o);
-    }
+    counto=countzo+hcounto+reference_o;             //differs from the other to due to the motor being on the opposite side of the pillar
+    countb=-(reference_b-countzb-hcountb);
+    countr=-(reference_r-countzr-hcountr);
+
+    pc.printf("dLod=%.2f; dLbd=%.2f; dLrd=%.2f\n\r",dLod,dLbd,dLrd);
+    pc.printf("o=%.2f  b=%.2f  countzr=%.2f",countzo,countzb,countzr);
+    pc.printf("Position_r = %i; reference_r=%i; Error_r=%i\n\r" ,position_o,reference_o,error_o);
+}
 
 
-void tiller(){
-    int stijg_o = -12487;       //eerst hcounto minus 12487?
-    int stijg_b = -8148;
-    int stijg_r = -7386;
-    double lou_t = sqrt(91.0*91.0+278.0*278.0); //sqrt(pow(91,2)+pow(278,2));         // (Pex - Pox)^2 + (Pey - Poy)^2
-    double lou_b = sqrt(91.0*91.0+(278.0-887.0)*(278.0-887.0)); //sqrt(pow((91),2)+pow((278-887),2));
-    double lou_r = sqrt((91.0-768.0)*(91.0-768.0)+(278.0-443.0)*(278.0-443.0)); //sqrt(pow((91-768),2)+pow((278-443),2));
-    int roto_t = lou_t/omtrekklosje;
-    int roto_b = lou_b/omtrekklosje;
-    int roto_r = lou_r/omtrekklosje;  
-    
-    counto = stijg_o + roto_t*4200;
-    countb = stijg_b + roto_b*4200;
-    countr = stijg_r + roto_r*4200;
-    pc.printf("Tiller");
-    controlmotor1.attach(&MotorController1, 0.01);
-    controlmotor2.attach(&MotorController2, 0.01);
-    controlmotor3.attach(&MotorController3, 0.01);
-    pc.printf("Tiller done");
+void zakken_threshold()
+{
+    if (Move_done == true) {
+        if (emgBLcomplete > thresholdBL) {
+            zakker();
+        }
     }
-   
-void zakken_threshold() {
-    if (Move_done == true) {
-    if (emgBLcomplete > thresholdBL) {
-        zakker();
-        }
-        }
-        }    
+}
 
-void setcurrentposition(){ 
-    if(Input_done==true){
+void setcurrentposition()
+{
+    if(Input_done==true) {
         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);
-        pc.printf("ik reset hcounts");
         Input_done=false;
-        }
     }
-    
+}
+
 int main()
 {
     pc.baud(115200);
     wait(1.0f);
-    tiller();
     getbqChain();
     threshold_timerR.attach(&Threshold_samplingBR, 0.002);
     threshold_timerL.attach(&Threshold_samplingBL, 0.002);
     setcurrentposition();
-    while(true){
+    while(true) {
         sample_timer.attach(&EMG_sample, 0.002);
-        //button2.fall(zakker);
         zakken_threshold();
         wait(2.5f); // om te zorgen dat je niet direct na de calibratie input moet geven en om te zorgen dat de zakker de tijd heeft voor nieuwe input vragen
-        
         tellerX();
         tellerY();
         calculator();
@@ -618,6 +557,6 @@
         controlmotor2.attach(&MotorController2, 0.01);
         controlmotor3.attach(&MotorController3, 0.01);
         wait(4.0f); // om te zorgen dat je niet meteen tijdens t zakken nieuwe input moet geven
-        }
+    }
 
 }
\ No newline at end of file