Revision 36:22d1bcb82061, committed 2019-11-01

Comitter:

viviien

Date:

Fri Nov 01 09:57:21 2019 +0000

Parent:

35:4cb2ed6dd2d2

Commit message:

werkt niet;

Changed in this revision

diff -r 4cb2ed6dd2d2 -r 22d1bcb82061 Motor_tryout.cpp
--- a/Motor_tryout.cpp	Fri Nov 01 08:24:42 2019 +0000
+++ b/Motor_tryout.cpp	Fri Nov 01 09:57:21 2019 +0000
@@ -4,6 +4,7 @@
 #include "Math.h"
 #include "ttmath.h"
 #include "FastPWM.h"
+#include "BiQuad.h"
 
 MODSERIAL pc(USBTX, USBRX);
 //Serial term (USBTX, USBRX);
@@ -16,6 +17,203 @@
 
 AnalogIn potmeter1(A1);
 
+//Define objects
+AnalogIn    emg0( A0 ); // 1e
+AnalogIn    emg2( A2 ); // 2e
+AnalogIn    emg4( A4 ); // 3e
+
+float A;
+float B;
+Ticker      sample_timer;
+DigitalOut  led(LED1);
+
+const int leng_filt = 60;
+const int box_length = 50;
+const int box_lengthC = 150;
+const int box_checkC = 50;
+const float grenswaardeA0 = 0.016;
+const float grenswaardeB0 = 0.012;
+const float grenswaardeC = 0.012;
+float Ay1;
+float Ay2;
+float A_array[leng_filt] = {0};
+float A_ar[leng_filt] = {0};
+float A_ar2[leng_filt] = {0};
+float A_ar3[box_length] = {0};
+int boxcheckC = 0;
+int boxcheckA = 0;
+int boxcheckB = 0;
+
+int boxcheckCC = 0;
+int boxcheckAA = 0;
+int boxcheckBB = 0;
+
+float By1;
+float By2;
+float B_array[leng_filt] = {0};
+float B_ar[leng_filt] = {0};
+float B_ar2[leng_filt] = {0};
+float B_ar3[box_length] = {0};
+
+float Cy1;
+float Cy2;
+float C_array[leng_filt] = {0};
+float C_ar[leng_filt] = {0};
+float C_ar2[leng_filt] = {0};
+float C_ar3[box_lengthC] = {0};
+
+float result = 0;
+float Asum = 0;
+const int Fs = 2000; //Sample Frequency
+const double b0 = 0.292893;
+const double b1 = 0.585786;
+const double b2 = 0.292893;
+const double a0 = 1.000000;
+const double a1 = -0.00000;
+const double a2 = 0.171573;
+   
+void sample()
+{    
+    BiQuad lowpassA(b0,b1, b2, a0, a1, a2);
+    // Signaal 1 (A)
+    float A = emg0.read();
+    float Amean = 0;
+    float Ay2 = 0;
+    
+    for (int j=leng_filt-1; j>=1; j--)
+    {    A_ar[j] = A_ar[j-1];           }
+
+    A_ar[0] = A;
+    
+    for(int i=0; i<=leng_filt-1; i++)
+    {    Amean += A_ar[i]*1/leng_filt;  }
+    
+    Ay1 = A - Amean;
+    Ay1 = fabs(Ay1);
+    Ay1 = lowpassA.step(Ay1);              // First signal, after Butterworth  
+        
+    for (int j=leng_filt-1; j>=1; j--)
+    {   A_ar2[j] = A_ar2[j-1];          }
+    
+    A_ar2[0] = Ay1;
+    
+    for(int i=0; i<=leng_filt-1; i++)
+    {   Ay2 += A_ar2[i]*1/leng_filt;     }
+    
+    float Ay3;
+    if(Ay2>grenswaardeA0)
+    {   Ay3 = 1; }
+    
+    //if(Ay2<=grenswaardeA0)
+    //{   if(Ay2>grenswaardeA1)
+    //    {   Ay3 = 0.5; }   }
+    
+    if(Ay2<=grenswaardeA0)
+    {   Ay3 = 0; }
+    
+    for (int j=box_length-1; j>=1; j--)
+    {   A_ar3[j] = A_ar3[j-1];         }
+    
+    A_ar3[0] = Ay3;
+    boxcheckA = 0;
+    
+    for (int j=0; j<=box_length-1; j++)
+    {   if(A_ar3[j] == 1)
+            { boxcheckA = 1;
+            boxcheckAA = 1;}           }
+
+    // Signaal 2 (B)    
+    BiQuad lowpassB(b0,b1, b2, a0, a1, a2);
+    float B = emg2.read();
+    float Bmean = 0;
+    float By2 = 0;
+    
+    for (int j=leng_filt-1; j>=1; j--)
+    {    B_ar[j] = B_ar[j-1];           }
+
+    B_ar[0] = B;
+    
+    for(int i=0; i<=leng_filt-1; i++)
+    {    Bmean += B_ar[i]*1/leng_filt;  }
+    
+    By1 = B - Bmean;
+    By1 = fabs(By1);
+    By1 = lowpassB.step(By1);              // First signal, after Butterworth  
+        
+    for (int j=leng_filt-1; j>=1; j--)
+    {   B_ar2[j] = B_ar2[j-1];          }
+    
+    B_ar2[0] = By1;
+    
+    for(int i=0; i<=leng_filt-1; i++)
+    {   By2 += B_ar2[i]*1/leng_filt;     }
+    
+    float By3;
+    if(By2>grenswaardeB0)
+    {   By3 = 1; }
+    
+    if(By2<=grenswaardeB0)
+    {   By3 = 0; }
+    
+    for (int j=box_length-1; j>=1; j--)
+    {   B_ar3[j] = B_ar3[j-1];         }
+    
+    B_ar3[0] = By3;
+    boxcheckB = 0;
+    
+    for (int j=0; j<=box_length-1; j++)
+    {   if(B_ar3[j] == 1)
+            { boxcheckB = 1;
+            boxcheckBB = 1;}           }  
+    
+    // Signaal 3 (C)
+    BiQuad lowpassC(b0,b1, b2, a0, a1, a2);
+    float C = emg4.read();
+    float Cmean = 0;
+    float Cy2 = 0;
+    
+    for (int j=leng_filt-1; j>=1; j--)
+    {    C_ar[j] = C_ar[j-1];           }
+
+    C_ar[0] = C;
+    
+    for(int i=0; i<=leng_filt-1; i++)
+    {    Cmean += C_ar[i]*1/leng_filt;  }
+    
+    Cy1 = C - Cmean;
+    Cy1 = fabs(Cy1);
+    Cy1 = lowpassC.step(Cy1);              // First signal, after Butterworth  
+    
+    for (int j=leng_filt-1; j>=1; j--)
+    {   C_ar2[j] = C_ar2[j-1];          }
+    
+    C_ar2[0] = Cy1;
+    
+    for(int i=0; i<=leng_filt-1; i++)
+    {   Cy2 += C_ar2[i]*1/leng_filt;    }
+    
+    float Cy3;
+    if(Cy2>grenswaardeC)
+    {   Cy3 = 1; }
+    
+    if(Cy2<=grenswaardeC)
+    {   Cy3 = 0; }
+    
+    for (int j=box_lengthC-1; j>=1; j--)
+    {   C_ar3[j] = C_ar3[j-1];         }
+    
+    C_ar3[0] = Cy3;
+    boxcheckC = 0;
+    int C_sum = 0;
+    for (int j=0; j<=box_length-1; j++)
+    {   C_sum += C_ar3[j];
+        if(C_sum >= box_checkC)
+        {   boxcheckC = 1;
+        boxcheckCC=1;} 
+        }
+    
+    led = !led;
+}
 
 
 QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
@@ -218,11 +416,10 @@
     
 //z0 = 158 - 60* sin(3*time_sin); 
 
-if  (sign == 1) {
-    float r = 20*(1-1/(1+(time_sin)));
-    x0 = savedX0 - r*sin(3*time_sin);
-    y0 = savedY0 - r*cos(3*time_sin);
-}
+//    float r = 20*(1-1/(1+(time_sin)));
+//    x0 = r*sin(3*time_sin);
+//    y0 = r*cos(3*time_sin);
+
      
 
     delta_calctheta1 ();  
@@ -293,11 +490,10 @@
     delta_calctheta2 ();
     delta_calctheta3 ();
     
-    pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
+        motor_timer.attach(&motor, 0.002);
+//            sample_timer.attach(&sample, 0.005);
 
-    char cc = pc.getc();
-    
-        motor_timer.attach(&motor, 0.002);
+       char cc = pc.getc();
 
 
     int frequency_pwm = 10000; //10 kHz PWM
@@ -305,93 +501,51 @@
     motor2_pwm.period_us(6);
     motor3_pwm.period_us(6);
     
-            pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
-            pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
-            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
 
-    
-    
     pc.baud(115200);  
 
     while(true){
-        
-        while (boxcheckC == 0)  {    
-        if (boxcheckA == 1 && x0>=-76 && x0<=75) {
-            x0=x0+1.0f ;
-            wait(1.0/20);
-            }
-        if (boxcheckB== 1 && x0>=-75 && x0<=76) {            
-            x0=x0-1.0f;
-            wait(1.0/20);
-            }
-        }
-        
-        wait(1.5);
-        savedX = x0;
-        
-        while (boxcheckC == 0)  {
-        if (boxcheckA==1 && y0>=-76 && y0<=75){
-            y0=y0+1.0f;
-            wait(1.0/20);
-            }
-        if (boxcheckB==1 && y0>=-75 && y0<=76){
-            y0=y0-1.0f;
-            wait(1.0/20);
-            }
-        }
-        
-        wait(1.5);
-        savedY = y0;
-        
-        while (boxcheckC == 0)  {
-        if (boxcheckA == 1 && z0>=-179 && z0<=-158){
-            z0=z0+1.0f;
-            wait(1.0/20);
-            }
-        if (boxcheckB == 1 && z0>=-178 && z0<=-157){
-            z0=z0-1.0f;
-            wait(1.0/20);
-            }
-        }
-        
-        wait(1.5);
-        savedZ = z0;
-        
-        if (boxcheckC == 1)     {
-            sign = 1;
-            }
-        
-        wait(5.0);
-        
-        sign = 0;
-        
-        float diffZ = -158 - z0;
-        float diffX = 0 - x0;
-        float diffY = 0 - y0;
-        
-        for (int i=0; i<=diffZ; i++)    {
-         z0 = z0+1;
-         }
-         
-        for (int i=0; i<=diffY; i++)    {
-         if (diffY>0)   {
-            y0 = y0+1;
-            }
-         if (diffY<0)   {
-             y0 = y0-1;
-             }
-         }
-         
-        for (int i=0; i<=diffX; i++)    {
-         if (diffX>0)   {
-            x0 = x0+1;
-            }
-         if (diffX<0)   {
-             x0 = x0-1;
-             }
-         }
-
-break;        
+//        
+//        
+//        while (boxcheckCC == 0)  {
+//        if (boxcheckAA == 1 && z0>=-179 && z0<=-158){
+//            z0=z0+1.0f;
+//            wait(1.0/2000);
+//            boxcheckAA = 0;
+//            }
+//        if (boxcheckBB == 1 && z0>=-178 && z0<=-157){
+//            z0=z0-1.0f;
+//            wait(1.0/2000);
+//            boxcheckBB = 0;
+//            }
+//        }
+//        
+//        wait(1.5);
+//        boxcheckCC = 0;
+//        savedZ = z0;
+//        
+//        sign = 1;
+//        
+//        wait(5.0);
+//        
+//        sign = 0;
+//        boxcheckCC = 0;
+//        
+//        
+//        float diffZ = -158 - z0;
+//
+//        
+//        for (int i=0; i<=diffZ; i++)    {
+//         if (diffZ>0)   {
+//         z0 = z0+1;
+//         }
+//         if (diffZ<0)   {
+//         z0 = z0-1;     
+//         }
+//         wait(1.0/20);
+//         }
+//         
+//break;        
 
             
         
@@ -400,44 +554,51 @@
 //        wait(0.1);
 
        
-//       char cc = pc.getc();
-//        if (cc=='d' && x0>=-76 && x0<=75) {
-//            x0=x0+1.0f ;
-//            }
-//
-//        if (cc=='a' && x0>=-75 && x0<=76) {
-//            
-//            x0=x0-1.0f;
-//            }
-//
-//        if (cc=='w' && y0>=-76 && y0<=75){
-//            y0=y0+1.0f;
-//            pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
-//            pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
-//            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
-//
-//            }
-//
-//        if (cc=='s' && y0>=-75 && y0<=76){
-//            y0=y0-1.0f;
-//            pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
-//            pc  .printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
-//            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
-//            }
-//
-//        if (cc=='u' && z0>=-211 && z0<=-130){
-//            pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
-//            pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
-//            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
-//            z0=z0+1.0f;
-//            }
-//
-//        if (cc=='j' && z0>=-210 && z0<=-129){
-//                        pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
-//            pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
-//            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
-//            z0=z0-1.0f;
-//            }
+       char cc = pc.getc();
+        if (cc=='d' && x0>=-76 && x0<=75) {
+            x0=x0+1.0f ;
+            savedX=x0;
+            }
+
+        if (cc=='a' && x0>=-75 && x0<=76) {
+            
+            x0=x0-1.0f;
+            savedX=x0;
+            }
+
+        if (cc=='w' && y0>=-76 && y0<=75){
+            y0=y0+1.0f;
+            pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
+            pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
+            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
+            savedY=y0;
+
+            }
+
+        if (cc=='s' && y0>=-75 && y0<=76){
+            y0=y0-1.0f;
+            pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
+            pc  .printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
+            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
+            savedY=y0;
+            }
+
+        if (cc=='u' && z0>=-211 && z0<=-130){
+            pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
+            pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
+            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
+            z0=z0+1.0f;
+            savedZ=z0;
+            }
+
+        if (cc=='j' && z0>=-210 && z0<=-129){
+                        pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
+            pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
+            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
+            z0=z0-1.0f;
+            savedZ=z0;
+            }
+ 
         }
 //END MAIN
 }  
\ No newline at end of file

diff -r 4cb2ed6dd2d2 -r 22d1bcb82061 biquadFilter.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/biquadFilter.lib	Fri Nov 01 09:57:21 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/tomlankhorst/code/biquadFilter/#26861979d305