Diff: main.cpp

Revision:

27:2d9f172c66ad

Parent:

26:bfb1ae203c11

Child:

28:19cccdd68b5b

--- a/main.cpp	Mon Nov 06 09:57:31 2017 +0000
+++ b/main.cpp	Mon Nov 06 14:57:15 2017 +0000
@@ -5,14 +5,12 @@
 #include "encoder.h"
 #include "MODSERIAL.h"
 
-//State Machine en calibratie
-enum States {Cal1, Cal2, CalEMG, SelectDevice, EMG, Rest, Demo};
+//State Machine and calibration
+enum States {CalEMG, SelectDevice, EMG, Rest, Demonstration};
 States State;
 int Counter;
 bool Position_controller_on;
 double looptime = 0.002f;
-double value;
-double home1;
 DigitalIn button (D1);
 
 //Encoder/motor
@@ -21,8 +19,8 @@
 double motorValue1;
 double motorValue2;
 
-//globalvariables Motor
-Ticker Treecko;             //We make a awesome ticker for our control system
+//global variables Motor
+Ticker Treecko;             //We make an awesome ticker for our control system
 Ticker printer;
 PwmOut M1E(D6);             //Biorobotics Motor 1 PWM control of the speed 
 PwmOut M2E(D5);
@@ -31,7 +29,7 @@
 Encoder motor2(D9,D8,true);
 DigitalOut M2D(D4);
 
-//DEMO
+//Demonstration
 AnalogIn potMeter2(A1);
 AnalogIn potMeter1(A2);
 
@@ -70,9 +68,8 @@
 int Timescalibration = 0;
 int TimescalibrationREST = 0;
 
-
-// Biquad filters voor Left Bicep (LB)
-// Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
+// Filters
+// Biquad filters voor Left Biceps (LB): Notch, High-pass and Low-pass filter
 BiQuad N1LB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
 BiQuadChain NFLB;
 BiQuad HP1LB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
@@ -82,8 +79,7 @@
 BiQuad LP2LB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
 BiQuadChain LPFLB;
 
-// Biquad filters voor Right Bicep (RB)
-// Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
+// Biquad filters voor Right Biceps (RB): Notch, High-pass and Low-pass filter
 BiQuad N1RB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
 BiQuadChain NFRB;
 BiQuad HP1RB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
@@ -93,8 +89,7 @@
 BiQuad LP2RB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
 BiQuadChain LPFRB;
 
-// Biquad filters voor Left Tricep (LT)
-// Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
+// Biquad filters voor Left Triceps (LT): Notch, High-pass and Low-pass filter
 BiQuad N1LT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
 BiQuadChain NFLT;
 BiQuad HP1LT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
@@ -104,8 +99,7 @@
 BiQuad LP2LT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
 BiQuadChain LPFLT;
 
-// Biquad filters voor Left Tricep (RT)
-// Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
+// Biquad filters for Right Triceps (RT): Notch, High-pass and Low-pass filter
 BiQuad N1RT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
 BiQuadChain NFRT;
 BiQuad HP1RT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
@@ -115,9 +109,6 @@
 BiQuad LP2RT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
 BiQuadChain LPFRT;
 
-Timer LooptimeEMG; //moetuiteindelijk weg
-
-//filters
 double emgNotchLB;
 double emgHPLB;
 double emgAbsHPLB;
@@ -146,14 +137,6 @@
 AnalogIn emgLT(A2);
 AnalogIn emgRT(A3);
 
-//double MVCLB = 0.3;
-//double MVCRB = 0.3;
-//double MVCLT = 0.3;
-//double MVCRT = 0.3;
-
-// variabelen changePosition
-int goalx, goaly;
-
 // variables RKI
 double pi = 3.14159265359;
 double q1 = (pi/2);    //Reference position hoek 1 in radiance
@@ -173,7 +156,7 @@
 double R1x = 0;                         //The x-component of the joint 1 radius
 double R1y = 0;                         //The y-component of the joint 1 radius
 double R2x = cos(q1)*L1;                //The x-component of the joint 2 radius
-double R2y = sin(q1)*L1;                //The y-component of the joint 1 radius   
+double R2y = sin(q1)*L1;                //The y-component of the joint 2 radius   
 double Fx = 0;
 double Fy = 0;
 double Tor1 = 0;
@@ -181,13 +164,9 @@
 double w1= 0;
 double w2= 0;
 
+
 void Filteren() 
-{
-   // LooptimeEMG.reset();
-   // LooptimeEMG.start();
-    
-    //EMG 1
-    
+{  
     emgNotchLB = NFLB.step(emgLB.read() );  // Notch filter
     emgHPLB = HPFLB.step(emgNotchLB);         // High-pass filter: also normalises around 0.
     emgAbsHPLB = abs(emgHPLB);            // Take absolute value
@@ -220,11 +199,6 @@
     //{
     //    emgFiltered=1.00;
     //}
-//pc.printf("emgreadLB = %f , emgFiltered = %f, maxi = %f, loop = %f \r\n, emgreadRB = %f , emgFiltered = %f, maxi = %f \r\n, emgreadLT = %f , emgFiltered = %f, maxi = %f \r\n, emgreadRT = %f , emgFiltered = %f, maxi = %f \r\n",emgLB.read(), LBF, maxiLB,looptime.read(),emgRB.read(), RBF, maxiRB,emgLT.read(), LTF, maxiLT, emgRT.read(), RTF, maxiRT);
-    //int maxwaarde = 4096;                   // = 64x64
-    //double refP = emgFiltered*maxwaarde;
-    //return refP;                            // value between 0 and 4096 
-   
 }
 
 void CalibrationEMG()
@@ -234,44 +208,35 @@
     
     if(Timescalibration<2000)
     {
-        pc.printf("calibratie rust EMG \r\n");
+        pc.printf("calibration rest EMG \r\n");
         led = 1;
-       // wait(0.2);
-        //led = 0;
         
-    emgNotchLB = NFLB.step(emgLB.read() );
-    emgHPLB = HPFLB.step(emgNotchLB);   
-    emgAbsHPLB = abs(emgHPLB);
-    emgLPLB = LPFLB.step(emgAbsHPLB);
-    emgSUMLB += emgLPLB;                         //SUM all rest values LB
+        emgNotchLB = NFLB.step(emgLB.read() );
+        emgHPLB = HPFLB.step(emgNotchLB);   
+        emgAbsHPLB = abs(emgHPLB);
+        emgLPLB = LPFLB.step(emgAbsHPLB);
+        emgSUMLB += emgLPLB;                         //SUM all rest values LB
     
-    emgNotchRB = NFRB.step(emgRB.read()); 
-    emgHPRB = HPFRB.step(emgNotchRB);
-    emgAbsHPRB = abs(emgHPRB);
-    emgLPRB = LPFRB.step(emgAbsHPRB);
-    emgSUMRB += emgLPRB;                        //SUM all rest values RB
+        emgNotchRB = NFRB.step(emgRB.read()); 
+        emgHPRB = HPFRB.step(emgNotchRB);
+        emgAbsHPRB = abs(emgHPRB);
+        emgLPRB = LPFRB.step(emgAbsHPRB);
+        emgSUMRB += emgLPRB;                        //SUM all rest values RB
     
-    emgNotchLT = NFLT.step(emgLT.read() );
-    emgHPLT = HPFLT.step(emgNotchLT);
-    emgAbsHPLT = abs(emgHPLT);
-    emgLPLT = LPFLT.step(emgAbsHPLT);
-    emgSUMLT += emgLPLT;                         //SUM all rest values LT
+        emgNotchLT = NFLT.step(emgLT.read() );
+        emgHPLT = HPFLT.step(emgNotchLT);
+        emgAbsHPLT = abs(emgHPLT);
+        emgLPLT = LPFLT.step(emgAbsHPLT);
+        emgSUMLT += emgLPLT;                         //SUM all rest values LT
     
-    emgNotchRT = NFRT.step(emgRT.read() );  
-    emgHPRT = HPFRT.step(emgNotchRT);       
-    emgAbsHPRT = abs(emgHPRT);           
-    emgLPRT = LPFRT.step(emgAbsHPRT);
-    emgSUMRT += emgLPRT;                         //SUM all rest values RT
+        emgNotchRT = NFRT.step(emgRT.read() );  
+        emgHPRT = HPFRT.step(emgNotchRT);       
+        emgAbsHPRT = abs(emgHPRT);           
+        emgLPRT = LPFRT.step(emgAbsHPRT);
+        emgSUMRT += emgLPRT;                         //SUM all rest values RT
     }
     if(Timescalibration==1999)
     {
-        /*led = 1;
-        wait(0.2);
-        led = 0; 
-        wait(0.2);
-        led = 1;
-        wait(0.2);
-        */
         led = 0;
         RESTMEANLB = emgSUMLB/Timescalibration; //determine the mean rest value
         RESTMEANRB = emgSUMRB/Timescalibration; //determine the mean rest value
@@ -282,104 +247,59 @@
     {
         pc.printf("maximum linker biceps \r\n");
         led = 1;
-        /*wait(0.2);
-        led = 0;
-        wait(0.2);
-        led = 1;
-        wait(0.2);
-        led = 0;
-        wait(0.2);
-        led = 1;
-        wait(0.2);
-        led = 0;
-        */
-    emgNotchLB = NFLB.step(emgLB.read() ); 
-    emgHPLB = HPFLB.step(emgNotchLB);        
-    emgAbsHPLB = abs(emgHPLB);          
-    emgLPLB = LPFLB.step(emgAbsHPLB);       
-    double emgfinalLB = emgLPLB;
-    if (emgfinalLB > MVCLB)
-    {                                       //determine what the highest reachable emg signal is
-          MVCLB = emgfinalLB;
-    }    
+        emgNotchLB = NFLB.step(emgLB.read() ); 
+        emgHPLB = HPFLB.step(emgNotchLB);        
+        emgAbsHPLB = abs(emgHPLB);          
+        emgLPLB = LPFLB.step(emgAbsHPLB);       
+        double emgfinalLB = emgLPLB;
+        if (emgfinalLB > MVCLB)
+        {                                       //determine what the highest reachable emg signal is
+            MVCLB = emgfinalLB;
+        }    
     }
     
     if(Timescalibration>3000 && Timescalibration<4000)
     {
         pc.printf(" maximum rechter biceps \r\n");
-        /*led = 1;
-        wait(0.2);
         led = 0;
-        wait(0.2);
-        led = 1;
-        wait(0.2);
-        led = 0;
-        wait(0.2);
-        led = 1;
-        wait(0.2);
-        led = 0;
-        wait(0.2);
-        led = 1;
-        wait(0.2);
-        */
-        led = 0;
-    emgNotchRB = NFRB.step(emgRB.read()); 
-    emgHPRB = HPFRB.step(emgNotchRB); 
-    emgAbsHPRB = abs(emgHPRB);           
-    emgLPRB = LPFRB.step(emgAbsHPRB);      
-    double emgfinalRB = emgLPRB;
-    if (emgfinalRB > MVCRB)
-    {                                       //determine what the highest reachable emg signal is
-          MVCRB = emgfinalRB;
-    }    
+        emgNotchRB = NFRB.step(emgRB.read()); 
+        emgHPRB = HPFRB.step(emgNotchRB); 
+        emgAbsHPRB = abs(emgHPRB);           
+        emgLPRB = LPFRB.step(emgAbsHPRB);      
+        double emgfinalRB = emgLPRB;
+        if (emgfinalRB > MVCRB)
+        {                                       //determine what the highest reachable emg signal is
+            MVCRB = emgfinalRB;
+        }    
     }
     
     if(Timescalibration>4000 && Timescalibration<5000)
     {
         pc.printf("maximum linker triceps \r\n");
         led = 1;
-        /*wait(0.2);
-        led = 0;
-        wait(0.2);
-        led = 1;
-        wait(0.2);
-        led = 0;
-        wait(0.2);
-        led = 1;
-        wait(0.2);
-        led = 0;
-        wait(0.2);
-        led = 1;
-        wait(0.2);
-        led = 0;
-        wait(0.2);
-        led = 1;
-        wait(0.2);
-        led = 0;
-        */
-    emgNotchLT = NFLT.step(emgLT.read() );
-    emgHPLT = HPFLT.step(emgNotchLT);
-    emgAbsHPLT = abs(emgHPLT);
-    emgLPLT = LPFLT.step(emgAbsHPLT);
-    double emgfinalLT = emgLPLT;
-    if (emgfinalLT > MVCLT)
-    {                                       //determine what the highest reachable emg signal is
-          MVCLT = emgfinalLT;
-    }    
+        emgNotchLT = NFLT.step(emgLT.read() );
+        emgHPLT = HPFLT.step(emgNotchLT);
+        emgAbsHPLT = abs(emgHPLT);
+        emgLPLT = LPFLT.step(emgAbsHPLT);
+        double emgfinalLT = emgLPLT;
+        if (emgfinalLT > MVCLT)
+        {                                       //determine what the highest reachable emg signal is
+            MVCLT = emgfinalLT;
+        }    
     }
     
     if(Timescalibration>5000 && Timescalibration<6000)
     {
         pc.printf("maximum rechter triceps");
-    emgNotchRT = NFRT.step(emgRT.read() );  
-    emgHPRT = HPFRT.step(emgNotchRT);       
-    emgAbsHPRT = abs(emgHPRT);           
-    emgLPRT = LPFRT.step(emgAbsHPRT);
-    double emgfinalRT = emgLPRT;
-    if (emgfinalRT > MVCRT)
-    {                                       //determine what the highest reachable emg signal is
-          MVCRT = emgfinalRT;
-    }    
+        emgNotchRT = NFRT.step(emgRT.read() );  
+        emgHPRT = HPFRT.step(emgNotchRT);       
+        emgAbsHPRT = abs(emgHPRT);           
+        emgLPRT = LPFRT.step(emgAbsHPRT);
+        double emgfinalRT = emgLPRT;
+        if (emgfinalRT > MVCRT)
+        {                                       //determine what the highest reachable emg signal is
+            MVCRT = emgfinalRT;
+        }    
     }
     
     if(Timescalibration>6000)
@@ -387,10 +307,7 @@
          pc.printf("calibratie afgelopen");
          State = SelectDevice; 
     }
-   // pc.printf("maxi waarde = %f emgfinal = %f \r\n",maxi,emgfinal);
-   //}
-                                         //PAS ALS DEZE TRUE IS, MOET DE MOTOR PAS BEWEGEN!!!
-  //return maxi;
+
 }
 
 void RKI()
@@ -425,7 +342,7 @@
         Rsx -= 0.001;                    //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 staat
     }
     else {                               //de x-waarde van de setpoint verandert niet
-        }
+    }
 
     if (Potmeterwaarde1>0.6) {           //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
         Rsy += 0.001;
@@ -434,7 +351,7 @@
         Rsy -= 0.001;                       
     }
     else {                               //de y-waarde van de setpoint verandert niet
-        }
+    }
 }
 
 double GetReferencePosition() 
@@ -467,7 +384,6 @@
     e_int = e_int+Ts*error;
     double Integrator = ki*e_int;
     
-    
     double motorValue = Proportional + Integrator + Derivative;
     return motorValue;
 }
@@ -486,10 +402,8 @@
     e_int2 = e_int2+Ts*error2;
     double Integrator2 = ki2*e_int2;
     
-    
     double motorValue2 = Proportional2 + Integrator2 + Derivative2;
     return motorValue2;
-    
 }
 
 void SetMotor1(double motorValue)
@@ -500,7 +414,6 @@
     else {
         M1D = 1;
     }
-
     if  (fabs(motorValue) > 1) {
         M1E = 1;                    //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
     }
@@ -517,7 +430,6 @@
     else {
         M2D = 0;
     }
-
     if  (fabs(motorValue2) > 1) {
         M2E = 1;                    //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
     }
@@ -531,13 +443,13 @@
     //SetpointRobot(); 
     // RKI aanroepen
     RKI();
-    // hier the control of the 1st control system
+    // control of 1st motor
     //double refP = GetReferencePosition();                    //KOMT UIT RKI
     double Huidigepositie = motor1.getPosition(); 
     double error = (refP - Huidigepositie);// make an error
     double motorValue = FeedBackControl(error, e_prev, e_int);
     SetMotor1(motorValue);
-    // hier the control of the 2nd control system
+    // control of 2nd motor
     //double refP2 = GetReferencePosition2(); 
     double Huidigepositie2 = motor2.getPosition(); 
     double error2 = (refP2 - Huidigepositie2);// make an error
@@ -570,75 +482,31 @@
 void Loop_funtion()
 {   
     pc.printf("state machine begint \r\n");
-    switch(State){
-        case Cal1: //Calibration motor 1
-        pc.printf("cal1 \r\n");
-        State=Cal2;
-        // naar achteren bewegen( als voorbeeld Arvid), daarna deze waarde opslaan als offset. Dan bewegen naar home middels PID en verschil encodervalue uiterste stand en home1.
-           /* motorValue1 = 0.1f; motorValue2=0;
-            M2E = fabs(motorValue2);
-            M1E = fabs(motorValue1);
-                    
-               if (Huidigepositie1== 0)
-               {
-                                        SetMotor1(value); //value is waarde encoder voor loodrechte hoeken,.
-                    //if (fabs(Huidigepositie1<0.01) {
-                        State=Cal2;
-                    //}
-                }
-                else {
-                    SetMotor1(0);
-                    Loop_funtion();
-                }*/
-            break;
-            
-        case Cal2: //Calibration motor 2
-        State = CalEMG;
-               /*  if (Huidigepositie2== 0)
-                 {
-                       if (Huidigepositie2<0.01){
-                       State=CalEMG;
-                 }
-                 else {
-                     SetMotor2(0);
-                     Loop_funtion();
-                 } */
-            break;
+    switch(State)
+    {    
         case CalEMG: // Calibration EMG
             CalibrationEMG();   //calculates average EMGFiltered at rest and measures max signal EMGFiltered.
             break;    
         case SelectDevice: //Looks at the difference between current position and home. Select aansturen EMG or buttons
             State = EMG;
-            /*if (button==1) {
+            if (button==1) {
                 State=EMG;
             }
-            if (button==0) {
+            else {           // if (button==0) {
                 State=Demonstration;
-            }*/
+            }
             break;
-        case EMG: //Aansturen met EMG
-        pc.printf("EMG begint met aansturen \r\n");
+        case EMG: //Control by EMG
             Filteren();
             changePosition();
-            //RKI --> output refP van motor
             MeasureAndControl();
             break;
-         case Demonstration: // Aansturen met toetsenbord
+        case Rest: // When it is not your turn, the robot shouldn't react on muscle contractions.
+        case Demonstration: // COntrol with potmeters
             break;
         }
 } 
 
-/*void Tickerfunctie()
-{
-    //if(caldone == false)
-    //{
-    pc.printf("emgreadRB = %f , emgFiltered = %f, maxi = %f meanrest = %f\r\n",emgRB.read(), RBF, MVCRB, RESTMEANLB);
-    pc.printf("emgreadLB = %f , emgFiltered = %f, maxi = %f, meanrest = %f emgSUMLB %f ,Timescalibration %i\r\n",emgLB.read(), LBF, MVCLB,RESTMEANRB,emgSUMLB, Timescalibration);
-    pc.printf("emgreadRT = %f , emgFilteredRT = %f, maxiRT = %f meanrest = %f \r\n",emgRT.read(), RTF, MVCRT,RESTMEANRT);
-    pc.printf("emgreadLT = %f , emgFilteredLT = %f, maxiLT = %f meanrest = %f \r\n",emgLT.read(), LTF, MVCLT,RESTMEANLT);
-    //}
-}
-*/
 int main()//deze moet ooit nog weg --> pas op voor errors
 {
     //voor EMG filteren