Diff: main.cpp

Revision:

17:358e7e1213cf

Parent:

16:cd11083c754a

Child:

18:7fb73aa6dbc0

--- a/main.cpp	Mon Nov 06 17:32:09 2017 +0000
+++ b/main.cpp	Tue Nov 07 10:51:44 2017 +0000
@@ -1,3 +1,11 @@
+/*   ___________     ___
+    /  |    |   \   /  o\
+   /___|____|____\_/   _ >
+  /   /   |  \     _____/
+  \_ / ___|__ \ __/ 
+   /  /      \  \
+  /__/        \__\      */
+
 #include "mbed.h"
 #include "math.h"
 #include "QEI.h"
@@ -123,20 +131,17 @@
     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
+        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 */
@@ -164,7 +169,6 @@
 
 void EMG_sample()
 {
-
     //Filtering steps for the Right Biceps EMG
     emgBRfiltered = bqChain1.step( emgBR.read() );   //Notch+High-pass
     emgBRrectified = fabs(emgBRfiltered);            //Rectification
@@ -174,9 +178,9 @@
     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
@@ -186,23 +190,27 @@
     bqChainTL.add(&bqNotchTR).add(&bqHighTL);
 }
 
-// Initial input value for couting the X-values
-int Xin=0   ;
+//Initial input value for couting the X-values
+int Xin=0   ;           //set X to zero for the first input sequence
 int Xin_new;
 float huidigetijdX;
 
-// Feedback system for counting values of X
+/*Feedback system for counting values of X:
+The user has 2 secondes to give input before the program jumps to the next 
+section. If input is regesered the timer is reset so the user has 2 secondes 
+again for the next input.*/
 void ledtX()
 {
-    t.reset();
+    t.reset();  //Reset (restart) the timer
     Xin++;
     pc.printf("Xin is %i\n",Xin);
-    led_G=0;
-    led_R=1;
+    led_G=0;    //Feedback for user to ensure his input is regestered
+    led_R=1;    
     wait(0.2);  //time led green on
     led_G=1;
     led_R=0;
-    wait(0.5);  // prevent multiple counts for one aanspanning
+    wait(0.5);  /*prevent multiple counts for one muscle contraction. This way
+only one contraction can be regestered per half second*/
 }
 
 // Couting system for values of X
@@ -218,14 +226,14 @@
             if (emgBRcomplete > thresholdBR) {
                 ledtX();
             }
-            t.start();
+            t.start();                  //Start timer 
             huidigetijdX=t.read();
-            if (huidigetijdX>2) {
-                led_R=1;                //Go to the next program (counting values for Y)
+            if (huidigetijdX>2) {       //After 2 seconds without input
+                led_R=1;                
                 Xin_new = Xin;
-                Xin = 0;
+                Xin = 0;        //Reset X to zero for the next input sequence
 
-                return Xin_new;
+                return Xin_new;         //Go to the next program 
             }
 
         }
@@ -239,18 +247,22 @@
 int Yin_new;
 float huidigetijdY;
 
-//Feedback system for couting values of Y
+/*Feedback system for counting values of Y:
+The user has 2 secondes to give input before the program jumps to the next 
+section. If input is regesered the timer is reset so the user has 2 secondes 
+again for the next input.*/
 void ledtY()
 {
-    t.reset();
+    t.reset();  //Reset (restart) the timer
     Yin++;
     pc.printf("Yin is %i\n",Yin);
-    led_G=0;
+    led_G=0;    //Feedback for user to ensure his input is regestered
     led_B=1;
-    wait(0.2);
+    wait(0.2);  //time led green on
     led_G=1;
     led_B=0;
-    wait(0.5);
+    wait(0.5);  /*prevent multiple counts for one muscle contraction. This way
+only one contraction can be regestered per half second*/
 }
 
 // Couting system for values of Y
@@ -266,20 +278,20 @@
             if (emgBRcomplete > thresholdBR) {
                 ledtY();
             }
-            t.start();
+            t.start();              //Start timer
             huidigetijdY=t.read();
-            if (huidigetijdY>2) {
+            if (huidigetijdY>2) {   //After 2 seconds without input
                 led_B=1;
                 Yin_new = Yin;
                 Yin = 0;
-                Input_done = true;
-                Move_done = false;
-                return Yin_new;
+                Input_done = true;  //Set input done to True
+                Move_done = false;  //Next section is moving so move done is false
+                return Yin_new;     //Go to the next program 
 
             }
         }
     }
-    return 0;      // ga door naar het volgende programma
+    return 0;      //Go to the next program 
 }
 
 // Declaring all variables needed for calculating rope lengths,
@@ -316,20 +328,23 @@
 float          countzo;
 float          countzb;
 float          countzr;
-int reference_o;
-int position_o;
-int error_o;
-int reference_b;
-int position_b;
-int error_b;
-int reference_r;
-int position_r;
-int error_r;
 
-// Declaring variables neeeded for calculating motor movements to get to a certain point        <- klopt dit?
+// Declaring variables needed for calculating motor movements to get to a certain point
 float          hcounto;
 float          hcountb;
 float          hcountr;
+int            reference_o;
+int            reference_b;
+int            reference_r;
+int            position_o;
+int            position_b;
+int            position_r;
+int            error_o;
+int            error_b;
+int            error_r;
+float          motorValue1;
+float          motorValue2;
+float          motorValue3;
 float          Psx;
 float          Psy;
 float          Vex;
@@ -337,33 +352,31 @@
 float          Pstx;
 float          Psty;
 float          T=0.02;//seconds
-
 float          kpo = 21;
 float          kpb = 21;
 float          kpr = 21;
-
-float          motorValue1;
-float          motorValue2;
-float          motorValue3;
-
-Ticker          controlmotor1; // één ticker van maken?
-Ticker          controlmotor2; // één ticker van maken?
-Ticker          controlmotor3; // één ticker van maken?
+Ticker          controlmotor1; 
+Ticker          controlmotor2; 
+Ticker          controlmotor3; 
 
 //Deel om motor(en) aan te sturen--------------------------------------------
 // start Motor 1 ------------------------------------------------------------
-float P1(int error_o, float kpo)
+float P1(int error_o, float kpo)    //Virtual spring with springconstant kpo
 {
     return error_o*kpo;
 }
 
 void MotorController1()
 {
+/*The reference is the place you want to go to but you have to subtract the 
+initial set position (hcounts) since the encoders 'think' they are at 0 when 
+starting*/
     reference_o = (int) (counto-hcounto);
     position_o = Encoder1.getPulses();
     error_o = reference_o - position_o;
 
-    if (-20<error_o && error_o<20) {
+    if (-20<error_o && error_o<20) {    /*within this bandwith we are satisfied
+    with the error thus the motor should not move anymore*/
         motorValue1 = 0;
     } else {
         motorValue1 = P1(error_o, kpo)/4200;
@@ -376,19 +389,22 @@
 }
 // end Motor 1 --------------------------------------------------------------
 // start Motor 2 ------------------------------------------------------------
-float P2(int error_b, float kpb)
+float P2(int error_b, float kpb)    //Virtual spring with springconstant kpb
 {
     return error_b*kpb;
 }
 
 void MotorController2()
 {
-
+/*The reference is the place you want to go to but you have to subtract the 
+initial set position (hcounts) since the encoders 'think' they are at 0 when 
+starting*/
     reference_b = (int) (-(countb-hcountb));
     position_b = Encoder2.getPulses();
     error_b = reference_b - position_b;
     
-    if (-20<error_b && error_b<20) {
+    if (-20<error_b && error_b<20) {    /*within this bandwith we are satisfied
+    with the error thus the motor should not move anymore*/
         motorValue2 = 0;
     } else {
         motorValue2 = P2(error_b, kpb)/4200;
@@ -401,22 +417,22 @@
 }
 // end Motor 2 --------------------------------------------------------------
 // start Motor 3 ------------------------------------------------------------
-
-
-float P3(int error_r, float kpr)
+float P3(int error_r, float kpr)    //Virtual spring with springconstant kpr
 {
     return error_r*kpr;
 }
 
 void MotorController3()
 {
+/*The reference is the place you want to go to but you have to subtract the 
+initial set position (hcounts) since the encoders 'think' they are at 0 when 
+starting*/
     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) {
+    if (-20<error_r && error_r<20) {    /*within this bandwith we are satisfied
+    with the error thus the motor should not move anymore*/
         motorValue3 = 0;
     } else {
         motorValue3 = P3(error_r, kpr)/4200;
@@ -434,6 +450,7 @@
 
 /*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 */
+/*We think a lot of float with return zero could have been voids*/
 float touwlengtes()
 {
     Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2));
@@ -455,10 +472,10 @@
     return 0;
 }
 
-//calculate the setpoint for each time step
+//calculate the setpoint for each time step in coördinates, ropelenghts and counts
 float Pst()
 {
-    Pstx=Pex+Vex*T;                     //void maken (ook bij de rest)                      
+    Pstx=Pex+Vex*T;                     
     Psty=Pey+Vey*T;
     touwlengtes();
     Pex=Pstx;
@@ -467,7 +484,7 @@
     return 0;
 }
 
-//Calculating desired end position based on the EMG input                       <- Waarom maar voor een paal?
+//Calculating desired end position based on the EMG input                 
 float Ps()
 {
     Psx=(Xin_new)*30+91;
@@ -475,23 +492,25 @@
     return 0;
 }
 
-// Rekent dit de snelheid uit waarmee de motoren bewegen?
+//Calculates the vector pointing from position to setpoint
 void Ve()
 {
     Vex=(Psx-Pex);
     Vey=(Psy-Pey);
-    Pst();
-    if((fabs(Vex)<0.01f)&&(fabs(Vey)<0.01f)) {
+    Pst();      //calculates the new position for the next time step
+    if((fabs(Vex)<0.01f)&&(fabs(Vey)<0.01f)) {  /*If the velocities are lower
+    than 0.01 m/s the move is done and the loop can be detached*/
         Move_done=true;
         loop.detach();
     }
 }
 
-// Calculating the desired position, so that the motors can go here
+// Calculating the desired position
 int calculator()
 {
     Ps();
-    if (Move_done == false) {
+    if (Move_done == false) {   /*While the move is being executed the new 
+    vector and new position (Pst) have to be calculated continiously*/
         loop.attach(&Ve,0.02);
     }
     return 0;
@@ -500,6 +519,7 @@
 // Function which makes it possible to lower the end-effector to pick up a piece
 void zakker()
 {
+    /*277.85 is the distance between the board and the bottom of the magnet */
     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;
@@ -509,25 +529,23 @@
     countzo=rotzo*4200;
     countzb=rotzb*4200;
     countzr=rotzr*4200;
-    counto=countzo+hcounto+reference_o;             //differs from the other to due to the motor being on the opposite side of the pillar
+    /*first one (counto)differs from the other to due to the motor being on the 
+    opposite side of the pillar*/
+    counto=countzo+hcounto+reference_o;        
     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);
 }
 
-
+//Checks if the threshold for the left biceps is reached 
 void zakken_threshold()
 {
-    if (Move_done == true) {
+    if (Move_done == true) {    //should only be executed when the move is done
         if (emgBLcomplete > thresholdBL) {
             zakker();
         }
     }
 }
-
+/*Calculates the counts corrosponding with the set position (which is (0,0))*/
 void setcurrentposition()
 {
     if(Input_done==true) {
@@ -541,7 +559,7 @@
 int main()
 {
     pc.baud(115200);
-    wait(1.0f);
+    wait(1.0f);//Gives you one second between starting te program and calibrating
     getbqChain();
     threshold_timerR.attach(&Threshold_samplingBR, 0.002);
     threshold_timerL.attach(&Threshold_samplingBL, 0.002);
@@ -549,8 +567,10 @@
     while(true) {
         sample_timer.attach(&EMG_sample, 0.002);
         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();
+        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();
         controlmotor1.attach(&MotorController1, 0.01);