Diff: main.cpp

Revision:

1:23834862b877

Parent:

0:2aa29a0824df

Child:

2:58ec7347245e

--- a/main.cpp	Tue Oct 30 14:17:16 2018 +0000
+++ b/main.cpp	Tue Oct 30 15:57:25 2018 +0000
@@ -6,22 +6,47 @@
 
 DigitalOut gpo(D0);
 DigitalOut led(LED_RED);
-AnalogIn pot1(A0);
-AnalogIn pot2(A1);
-AnalogIn pot3(A2);
+AnalogIn pot1(A0);                  //POORTEN VERANDEREN
+AnalogIn pot2(A1);                  //POORTEN veranderen
+AnalogIn pot3(A2);                  //POORTEN VERANDEREN
 QEI encoder1(D10, D11, NC, 32);
 QEI encoder2(D12, D13, NC, 32);
 FastPWM Motor1PWM(D6);
 DigitalOut Motor1Direction(D7);
 FastPWM Motor2PWM(D5);            //!!!!! Juiste poorten aangeven
 DigitalOut Motor2Direction(D4);   //!!!!! Juiste poort aangeven
+//EMG
+AnalogIn    a0(A0);
+AnalogIn    a1(A1);
+AnalogIn    a2(A2);
+AnalogIn    a3(A3);
 
 MODSERIAL pc(USBTX, USBRX);
 
-//HIDSCOPE
+//HIDSCOPE          //Oppassen waar we HIDSCOPE aanroepen want nu voor meerdere dingen
 HIDScope    scope(4);
 Ticker      scopeTimer;
 
+Ticker      EMGTicker;
+
+// BiQuad filters
+    //BiQuad Chains
+BiQuadChain bqc1;
+BiQuadChain bqc2;
+BiQuadChain bqc3;
+BiQuadChain bqc4;
+
+    //High pass filters 20 Hz
+BiQuad HP_emg1(1,-2,1,-1.142980502539901,0.412801598096189);
+BiQuad HP_emg2(1,-2,1,-1.142980502539901,0.412801598096189); 
+BiQuad HP_emg3(1,-2,1,-1.142980502539901,0.412801598096189); 
+BiQuad HP_emg4(1,-2,1,-1.142980502539901,0.412801598096189);
+
+    //Notch filters 50 Hz
+BiQuad Notch_emg1(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492); 
+BiQuad Notch_emg2(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492); 
+BiQuad Notch_emg3(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492); 
+BiQuad Notch_emg4(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492);
 
 ///Variables
 
@@ -32,8 +57,6 @@
 int n = 100;            // Zelfde waarde als PrevErrorarray
 double q1Ref = 1.0;     //VOOR TESTEN
 double q2Ref;
-//Double xPos;
-//Double yPos;
 double xRef;
 double yRef;
 double q1Pos;
@@ -51,25 +74,202 @@
 double GainI2 = 2.0;
 double GainD2 = 2.0;
 double TickerPeriod = 1.0/400.0;
+// Global variables EMG
+double EMGdata1;
+double EMGdata2;
+double EMGdata3;
+double EMGdata4;
+int   count;
+double EMG_filt1;
+double EMG_filt2;
+double EMG_filt3;
+double EMG_filt4;
+double unit_vx;   
+double unit_vY;
 
 Ticker Kikker;
-int count = 0;
+int counter = 0;
 int countstep = 0;
 
 
+//EMGDINGEN
+
+void ReadEMG()
+{
+    EMGdata1 = a0.read(); // store values in the scope
+    EMGdata2 = a1.read();
+    EMGdata3 = a2.read();
+    EMGdata4 = a3.read();
+}
+
+// EMG High pass filters
+double EMG_HP1(double EMGdata) //data 1
+{
+    double HP_abs_EMGdata = bqc1.step(EMGdata);
+
+    return fabs(HP_abs_EMGdata);
+}
+
+double EMG_HP2(double EMGdata) //data 2
+{
+    double HP_abs_EMGdata = bqc2.step(EMGdata);
+
+    return fabs(HP_abs_EMGdata);
+}
+
+double EMG_HP3(double EMGdata) //data 3
+{
+    double HP_abs_EMGdata = bqc3.step(EMGdata);
+
+    return fabs(HP_abs_EMGdata);
+}
+
+double EMG_HP4(double EMGdata) // data 4
+{
+    double HP_abs_EMGdata = bqc4.step(EMGdata);
+
+    return fabs(HP_abs_EMGdata);
+}
+
+// EMG moving filter
+double EMG_mean (double EMGarray[100])
+{
+    double sum = 0.0;
+
+    for(int j=0; j<100; j++) 
+        {
+            sum += EMGarray[j];
+        }
+
+    double EMG_filt = sum / 100.0;
+    
+    return EMG_filt;
+}
+
+// Filtered signal to output between -1 and 1
+double filt2kin (double EMG_filt1, double EMG_filt2, double max1, double max2)
+{
+    double EMG_scaled1 = EMG_filt1 / max1;
+    double EMG_scaled2 = EMG_filt2 / max2;
+
+    double kin_input = EMG_scaled2 - EMG_scaled1;
+
+    if (kin_input > 1.0) {
+        kin_input = 1.0;
+    }
+    if (kin_input < -1.0) {
+        kin_input = -1.0;
+    }
+
+    return kin_input;
+}
+
+void EMG ()
+{     
+    ReadEMG();
+
+    double HP_abs_EMGdata1 =  EMG_HP1(EMGdata1);
+    double HP_abs_EMGdata2 =  EMG_HP2(EMGdata2);
+    double HP_abs_EMGdata3 =  EMG_HP3(EMGdata3);
+    double HP_abs_EMGdata4 =  EMG_HP4(EMGdata4);
+    
+    static double EMG_array1[100];
+    static double EMG_array2[100];
+    static double EMG_array3[100];
+    static double EMG_array4[100];
+    
+    // Fill array 1
+     for(int i = 100-1; i >= 1; i--) 
+        {
+            EMG_array1[i] = EMG_array1[i-1];
+        }
+    EMG_array1[0] = HP_abs_EMGdata1;
+    
+    // Fill array 2
+     for(int i = 100-1; i >= 1; i--) 
+        {
+            EMG_array2[i] = EMG_array2[i-1];
+        }
+    EMG_array2[0] = HP_abs_EMGdata2;
+    
+    // Fill array 3
+     for(int i = 100-1; i >= 1; i--) 
+        {
+            EMG_array3[i] = EMG_array3[i-1];
+        }
+    EMG_array3[0] = HP_abs_EMGdata3;
+    
+    // Fill array 4
+     for(int i = 100-1; i >= 1; i--) 
+        {
+            EMG_array4[i] = EMG_array4[i-1];
+        }
+    EMG_array4[0] = HP_abs_EMGdata4;
+
+    
+    EMG_filt1 = EMG_mean(EMG_array1);         //global maken
+    EMG_filt2 = EMG_mean(EMG_array2);
+    EMG_filt3 = EMG_mean(EMG_array3);
+    EMG_filt4 = EMG_mean(EMG_array4);
+
+    
+
+    unit_vX = filt2kin (EMG_filt1, EMG_filt2, EMG_max1, EMG_max2);
+    unit_vY   = filt2kin (EMG_filt3, EMG_filt4, EMG_max3, EMG_max4);
+
+    scope.set(0, unit_Vx);
+    scope.set(1, unit_Vy);
+    //scope.set(2, EMG_filt3);
+    //scope.set(3, EMG_filt4);
+    
+
+    count++;
+
+    if (count == 100) 
+    {
+        pc.printf("EMG filt 1 = %lf \t EMG filt 2 = %lf \t EMG filt 3 = %lf \t EMG filt 4 = %lf \n\r", EMG_filt1, EMG_filt2, EMG_filt3, EMG_filt4);
+        count = 0;
+    }
+}
+
+
 
-void UpdateRefPosition()
+//PIDCONTROLLLLLLLLLL + INVERSE KINEMATIKS
+
+
+//InverseKinematics
+
+void inverse(double X1, double Y1, double & thetaM1, double & thetaM2)
 {
-  //  Update, (based on EMG or pots) the reference position xRef, yRef
-    //q1Ref = pot1.read()*2*3.1416;
-    //q2Ref = pot2.read()*2*3.1416;
-}
+    double L1 = 40.0; // %define length of arm 1 attached to gear
+    double L3 = sqrt(pow(X1,2.0)+pow(Y1,2.0));
+    double q3 = atan(Y1/X1);
+    double q4 = 2.0*asin(0.5*L3/L1);
+    thetaM1 = (0.5*3.1416-0.5*q4+q3)*9.0;
+    thetaM2 = (3.1416-thetaM1/9.0-q4)*4.0;
+    }
 
 
-void InverseKinematics()
+void InverseKinematics ()// Kinematics function, takes imput between 1 and -1
 {
-  //  Convert, using the inverse kinematics relations, xRef and yRef to q1Ref and q2Ref
-// (So, update the values of q1Ref and q2Ref)
+    
+    double V_max = 1.0; //Maximum velocity in either direction
+    //     CM/s
+    
+    double deltaX = TickerPeriod*V_max*unit_vX; // imput to delta
+    double deltaY = TickerPeriod*V_max*unit_vY;
+    
+    static double X1;
+    static double Y1;
+    X1 += deltaX;
+    Y1 += deltaY;
+    
+    double THETA1;
+    double THETA2;
+    
+    inverse(X1, Y1, THETA1, THETA2);
+    q1Ref = THETA1;
+    q2Ref = THETA2;
 }
 
 
@@ -188,7 +388,7 @@
 
 void NormalOperatingModus()
 {
-     UpdateRefPosition();
+     EMG()
      InverseKinematics();
      ReadCurrentPosition();
      UpdateError();
@@ -203,11 +403,11 @@
      GainD1 = pot1.read() ;
           
      countstep++;
-     count++;
-     if (count == 400) // print 1x per seconde waardes.
+     counter++;
+     if (counter == 400) // print 1x per seconde waardes.
     {
         pc.printf("GainP1 = %1f, GainI1 = %1f, GainD1 = %1f, q1Pos = %lf, q1Ref = %1f \n\r", GainP1, GainI1, GainD1, q1Pos, q1Ref);
-        count = 0;   
+        counter = 0;   
     }
      if (countstep == 4000)
      {
@@ -222,6 +422,13 @@
 int main()
 {
         pc.baud(115200);
+        
+        //BiQuad chains
+        bqc1.add( &HP_emg1 ).add( &Notch_emg1 );
+        bqc2.add( &HP_emg2 ).add( &Notch_emg2 );
+        bqc3.add( &HP_emg3 ).add( &Notch_emg3 );
+        bqc4.add( &HP_emg4 ).add( &Notch_emg4 );
+        
         //Initialize array errors to 0
         for (int i = 0; i <= 9; i++){
              PrevErrorq2[i] = 0;
@@ -235,6 +442,12 @@
         Kikker.attach(NormalOperatingModus, TickerPeriod);
         scopeTimer.attach_us(&scope, &HIDScope::send, 2e4);
         
+        //Onderstaande stond in EMG deel, kijken hoe en wat met tickers!!
+        // Attach the HIDScope::send method from the scope object to the timer at 50Hz
+        scopeTimer.attach_us(&scope, &HIDScope::send, 5e3);
+        //EMGTicker.attach_us(EMG_filtering, 5e3);
+        //.
+        
         while(true);
         {}        
 }