Revision 19:aa1ed300be11, committed 2017-11-07

Comitter:

Jitse_Giesen

Date:

Tue Nov 07 13:38:27 2017 +0000

Parent:

18:7fb73aa6dbc0

Commit message:

Final version for verslag

Changed in this revision

--- a/main.cpp	Tue Nov 07 11:05:47 2017 +0000
+++ b/main.cpp	Tue Nov 07 13:38:27 2017 +0000
@@ -1,8 +1,8 @@
 /*           ___________     ___
-            /  |    |   \   /  o\
-           /___|____|____\_/   _ >
+            /  | PG |   \   /  o\
+           /___|_15_|____\_/   _ >
           /   /   |  \     _____/
-          \_ / ___|__ \ __/ 
+          \_ / ___|__ \ __/
            /  /      \  \
           /__/        \__\                */
 
@@ -53,7 +53,7 @@
 float           currentTimeTL;
 
 //This is used when testing without EMG
-InterruptIn     button(SW2); 
+InterruptIn     button(SW2);
 InterruptIn     button2(SW3);
 
 //Timer needed for timing EMG input for the X and Y coördinates
@@ -162,7 +162,7 @@
 
     emgBLmeanMVC = emgBLsum/numsamples;
     thresholdBL = emgBLmeanMVC * 0.20;
-    
+
 }
 
 // EMG sampling and filtering
@@ -180,10 +180,11 @@
     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
+    bqChain1.add(&bqNotch1).add(&bqHigh1);            //Making the BiQuadChain
     bqChain2.add(&bqNotch2).add(&bqHigh2);
 
     bqChainTR.add(&bqNotchTR).add(&bqHighTR);
@@ -196,8 +197,8 @@
 float huidigetijdX;
 
 /*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 
+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()
 {
@@ -205,7 +206,7 @@
     Xin++;
     pc.printf("Xin is %i\n",Xin);
     led_G=0;    //Feedback for user to ensure his input is regestered
-    led_R=1;    
+    led_R=1;
     wait(0.2);  //time led green on
     led_G=1;
     led_R=0;
@@ -226,14 +227,14 @@
             if (emgBRcomplete > thresholdBR) {
                 ledtX();
             }
-            t.start();                  //Start timer 
+            t.start();                  //Start timer
             huidigetijdX=t.read();
             if (huidigetijdX>2) {       //After 2 seconds without input
-                led_R=1;                
+                led_R=1;
                 Xin_new = Xin;
                 Xin = 0;        //Reset X to zero for the next input sequence
 
-                return Xin_new;         //Go to the next program 
+                return Xin_new;         //Go to the next program
             }
 
         }
@@ -248,8 +249,8 @@
 float huidigetijdY;
 
 /*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 
+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()
 {
@@ -286,12 +287,12 @@
                 Yin = 0;
                 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 Yin_new;     //Go to the next program
 
             }
         }
     }
-    return 0;      //Go to the next program 
+    return 0;      //Go to the next program
 }
 
 // Declaring all variables needed for calculating rope lengths,
@@ -329,7 +330,8 @@
 float          countzb;
 float          countzr;
 
-// Declaring variables needed for calculating motor movements to get to a certain point
+/*Declaring variables needed for calculating motor movements to get to a 
+certain point*/
 float          hcounto;
 float          hcountb;
 float          hcountr;
@@ -355,9 +357,9 @@
 float          kpo = 21;
 float          kpb = 21;
 float          kpr = 21;
-Ticker          controlmotor1; 
-Ticker          controlmotor2; 
-Ticker          controlmotor3; 
+Ticker          controlmotor1;
+Ticker          controlmotor2;
+Ticker          controlmotor3;
 
 //Deel om motor(en) aan te sturen--------------------------------------------
 // start Motor 1 ------------------------------------------------------------
@@ -368,21 +370,23 @@
 
 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*/
+    /*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) {    /*within this bandwith we are satisfied
-    with the error thus the motor should not move anymore*/
+    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;
     }
-/*differs from the other to due to the motor being on the opposite side of the pillar*/
-    if (motorValue1 >=0) motor1DirectionPin=0;       
+    /*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);
@@ -396,15 +400,16 @@
 
 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*/
+    /*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) {    /*within this bandwith we are satisfied
-    with the error thus the motor should not move anymore*/
+
+    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;
@@ -424,15 +429,16 @@
 
 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*/
+    /*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;
 
-    if (-20<error_r && error_r<20) {    /*within this bandwith we are satisfied
-    with the error thus the motor should not move anymore*/
+    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;
@@ -448,7 +454,7 @@
 
 Ticker loop;
 
-/*Calculates ropelengths that are needed to get to new positions for each time 
+/*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()
@@ -475,7 +481,7 @@
 //calculate the setpoint for each time step in coördinates, ropelenghts and counts
 float Pst()
 {
-    Pstx=Pex+Vex*T;                     
+    Pstx=Pex+Vex*T;
     Psty=Pey+Vey*T;
     touwlengtes();
     Pex=Pstx;
@@ -484,7 +490,7 @@
     return 0;
 }
 
-//Calculating desired end position based on the EMG input                 
+//Calculating desired end position based on the EMG input
 float Ps()
 {
     Psx=(Xin_new)*30+91;
@@ -498,8 +504,9 @@
     Vex=(Psx-Pex);
     Vey=(Psy-Pey);
     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*/
+    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();
     }
@@ -509,8 +516,9 @@
 int calculator()
 {
     Ps();
-    if (Move_done == false) {   /*While the move is being executed the new 
-    vector and new position (Pst) have to be calculated continiously*/
+    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;
@@ -521,7 +529,7 @@
 {
     /*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;    
+    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;
@@ -529,14 +537,14 @@
     countzo=rotzo*4200;
     countzb=rotzb*4200;
     countzr=rotzr*4200;
-    /*first one (counto)differs from the other to due to the motor being on the 
+    /*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;        
+    counto=countzo+hcounto+reference_o;
     countb=-(reference_b-countzb-hcountb);
     countr=-(reference_r-countzr-hcountr);
 }
 
-//Checks if the threshold for the left biceps is reached 
+//Checks if the threshold for the left biceps is reached
 void zakken_threshold()
 {
     if (Move_done == true) {    //should only be executed when the move is done
@@ -567,15 +575,15 @@
     while(true) {
         sample_timer.attach(&EMG_sample, 0.002);
         zakken_threshold();
-        wait(2.5f); /*To give the user time between calibration and input, and 
-        allow the lowering to take place before new input is asked*/ 
+        wait(2.5f); /*To give the user time between calibration and input, and
+        allow the lowering to take place before new input is asked*/
         tellerX();
         tellerY();
         calculator();
         controlmotor1.attach(&MotorController1, 0.01);
         controlmotor2.attach(&MotorController2, 0.01);
         controlmotor3.attach(&MotorController3, 0.01);
-        wait(4.0f); /*To allow the move in the XY-plane to finish before 
+        wait(4.0f); /*To allow the move in the XY-plane to finish before
         lowering can start*/
     }
 }
\ No newline at end of file