Diff: NR_method_1.cpp

Revision:

1:fafea1d00d0c

Parent:

0:0af507ea0d83

Child:

2:f68fd7b1c655

diff -r 0af507ea0d83 -r fafea1d00d0c NR_method_1.cpp
--- a/NR_method_1.cpp	Mon Oct 22 11:57:03 2018 +0000
+++ b/NR_method_1.cpp	Thu Nov 01 09:42:53 2018 +0000
@@ -6,26 +6,80 @@
 #include <stdlib.h>
 #include <ctime>
 #include <QEI.h>
+#include "PID_controler.h"
+//#include  <MODSERIAL.h>
+
+bool bas;
+bool printen;
+
+Serial pc(USBTX, USBRX);
+// emg signals input
+AnalogIn   emg1(A0);
+AnalogIn   emg2(A1);
+InterruptIn sw2(SW2);
+// tickers
+Ticker sample_timer;
+
+volatile int x1;
+volatile int y1;
+double emgFiltered3;
+double emgFiltered23;
+bool dir = true;
+// filtering
+//filter coeffiecents
+// highpass
+double b01h = 0.978030479206560;
+double b02h = -1.95606095841312;
+double b03h =  0.978030479206560;
+double a01h = -1.95557824031504;
+double a02h =   0.956543676511203;
+// notchfilter
+double b01n = 0.995532032687234;
+double b02n =   -1.89361445373551;
+double b03n = 0.995532032687234;
+double a01n =   -1.89361445373551;
+double a02n =  0.991064065374468 ;
+//lowpass 1
+double b01l =  8.76555487540065e-05;
+double b02l =  0.000175311097508013;
+double b03l =  8.76555487540065e-05;
+double a01l =  -1.97334424978130;
+double a02l =  0.973694871976315;
+
+BiQuadChain bqc;
+BiQuad bq1( b01h, b02h, b03h, a01h, a02h ); //highpass
+BiQuad bq2( b01n, b02n, b03n, a01n, a02n ); //notch
+// than we need to rectifie
+// and lowpass afterwards
+BiQuadChain bqc2;
+BiQuad bq3( b01l, b02l, b03l, a01l, a02l);  //lowpass
+// optional is doing a movingaverage after
+
+BiQuadChain bqc3;
+BiQuad bq4( b01h, b02h, b03h, a01h, a02h ); //highpass
+BiQuad bq5( b01n, b02n, b03n, a01n, a02n ); //notch
+// than we need to rectifie
+// and lowpass afterwards
+BiQuadChain bqc4;
+BiQuad bq6( b01l, b02l, b03l, a01l, a02l);  //lowpass
+
 
 //Define in/outputs
 
-float counts = 8400;
-
+int counts = 8400;
 DigitalOut Led(LED1);
 PwmOut PMW1(D5); // Motor 1
 DigitalOut M1(D4); // direction of motor 1
 PwmOut PMW2(D6); // Motor 2
 DigitalOut M2(D7); // direction of motor 2
 
-
-
 //initializing Encoders
 QEI Enc1(D13,D12, NC , counts, QEI::X4_ENCODING); //Motor 1 encoder
 QEI Enc2(D11,D10, NC , counts, QEI::X4_ENCODING); // Motor 3 encoder this checks whetehter the motor has rotated
 
 double Kp = 1;
-double Ki = 1;
-double Kd = 1;
+double Ki = 0.6;
+double Kd = 0.3;
 double Ts = 0.001;
 
 float counts_a;
@@ -35,12 +89,12 @@
 //Define Variables
 
 double pi = 3.14159265359;
-double bb;
-double bc;
+int bb;
+int bc;
 float z;
 
-double angle_a = 0.5* pi; //in rad
-double angle_b = 0 * pi;  //in rad
+double angle_a = 0; //in rad
+double angle_b = 0.5 * pi;  //in rad
 
 double X0[2][1] = {{angle_a},{angle_b}};
 double X[2][1];
@@ -51,159 +105,202 @@
 
 double MaxIter = 20;
 double tolX = 1e-4;
-double Loa = 10;
-double Lob = 10;
-double b = 10;
-
-double c = 10;
-double Cx = 10.0; // current position
-double Cy = 10.0; // current position
-double Cxx = 12; // Goal position
-double Cyy = 12; // Goal position
+double A = 20;
+double B = 30;
+double C = 20;
+double D = 27;
+double E = 35;
+double ex = -35; // current position
+double ey = 27; // current position
+double Cxx = -35; // Goal position
+double Cyy = 27; // Goal position
 
 Ticker position_controll;
 
+void filteren ()
+{
+    double emgSignal1 = emg1.read();
+    double emgSignal2 = emg2.read();
+
+    double emgFiltered1 = bqc.step(emgSignal1);
+    double emgFiltered2 = fabs(emgFiltered1);
+    emgFiltered3 = bqc2.step(emgFiltered2);
+
+    double emgFiltered21 =  bqc3.step(emgSignal2);
+    double emgFiltered22 = fabs(emgFiltered21);
+    emgFiltered23 = bqc4.step(emgFiltered22);
+
+}
+
+void Position1x(double b)
+{
+    if (b > 0.20) {
+        Cxx =x1;
+        if (dir == true) {
+            if(x1 > -46) {
+                x1 = x1-4;
+                pc.printf(" posx is %f\n\r",Cxx);
+                //return x1;
+
+            } else if ( x1 <= -46) {
+                x1 =-14;
+                //return x1;
+            } else {
+            }
+        } else {
+            if(x1 < -14) {
+                x1 = x1+4;
+                //return x1;
+
+            } else if ( x1 >= -14) {
+                x1 = -46;
+                //return x1;
+            } else {
+            }
+        }
+
+        wait(0.5);
+    }
+}
+
+void Position1y(double b)
+{
+    if (b > 0.20) {
+        Cyy=y1;
+        if(dir == true) {
+            if(y1 < 43) {
+                y1 = y1+4;
+                //return y1;
+            } else if ( y1 >= 43) {
+                y1 = 11;
+                //return y1;
+            } else {
+            }
+        } else {
+            if(y1 > 11) {
+                y1 = y1-4;
+                //return y1;
+            } else if ( y1 <= 11) {
+                y1 = 43;
+                //return y1;
+            } else {
+            }
+        }
+
+        wait(0.5);
+    }
+}
+void change()
+{
+    dir = !dir;
+}
 void NR() //Newton Rapshon Calculation
 {
     //Variables
-    double Hob = X[0][0];
-    double Hoa = X[1][0];
-    
+    double Hoa = X[0][0];
+    double Hob = X[1][0];
+
+    double meuk1 = cos(Hoa) * A - ((E + C)/E) * (cos(Hob)*D - ex) - ex;
+    double meuk2 = sin(Hoa) * A - ((E + C)/E) * (sin(Hob)*D - ey) - ey;
+
     //Define f(x)
-    fval[0][0] = pow((Cx - Lob * sin(Hob)),2) + pow((Cy - Lob * cos(Hob)),2) - pow(c,2);
-    fval[1][0] = pow((Cx - Loa * sin(Hoa)),2) + pow((Cy - Lob * cos(Hoa)),2) - pow(b,2);
+    fval[0][0] = pow((ex - D * cos(Hob)),2) + pow((ey - D * sin(Hob)),2) - pow((E),2);
+    fval[1][0] = pow((meuk1),2) + pow((meuk2),2) - pow((B),2);
     //Jacobian
-   
-   
-    J[0][0]= -2 * Lob * cos(Hob) * (Cx - Lob * sin(Hob)) + 2 * Lob *sin(Hob) * (Cy - Lob * cos(Hob));
-    J[1][1]= -2 * Loa * cos(Hoa) * (Cx - Loa * sin(Hoa)) + 2 * Loa* sin(Hoa) * (Cy - Loa * cos(Hoa));
+
+
+
+    J[0][0]= 0;
+    J[0][1]= 2 * D * sin(Hob) * (ex - D * cos(Hob)) - 2 * D * cos(Hob) * (ey - D * sin(Hob));
+    J[1][0]=  - 2 * A * sin(Hoa) * meuk1 + 2 * A * cos(Hoa)* meuk2;
+    J[1][1]= 2 * ((E + C)/E) * D * sin(Hob) * meuk1 - 2 * ((E + C)/E) * D * cos(Hob) * meuk2;
 }
 
+void angle_define() //define the angle needed.
+{
+    for(int i=1 ; i <= MaxIter; i++) {
+        NR();
 
-void angle_define() //define the angle needed.
-{  
-    for(int i=1 ; i <= MaxIter; i++)
-    {
-        NR();
-        
-        X[0][0] = X[0][0] - ((1/J[0][0]) * fval[0][0]);
-        X[1][0] = X[1][0] - ((1/J[1][1]) * fval[1][0]);
+        X[0][0] = X[0][0] - ((-J[1][1]/(J[0][1] *J[1][0]))*fval[0][0] + (1/J[1][0])* fval[0][1]);
+        X[1][0] = X[1][0] - ((1/J[0][1])*fval[0][0]);
 
         err[0][0] = abs(X[0][0] - Xold[0][0]);
         err[1][0] = abs(X[1][0] - Xold[1][0]);
 
         Xold[0][0] = X[0][0];
         Xold[1][0] = X[1][0];
-        
+
         counts_a = ((X[0][0]) / (2* pi)) * 8400;
-        counts_b = -1 *(((X[1][0]) / (2* pi)) * 8400);
-    
-        if(err[0][0] <= tolX)
-        {   
-            if(err[1][0] <= tolX)
-            {
+        counts_b = ((X[1][0]) / (2* pi)) * 8400;
+
+        if(err[0][0] <= tolX) {
+            if(err[1][0] <= tolX) {
                 break;
             }
-        }      
-    }    
+        }
+    }
 }
 
 void position_define()
 {
-    if (Cx >= Cxx)
-    {
-        if (Cy >= Cyy)
-        {
+    if (ex >= Cxx - 0.01 && ex <= Cxx + 0.01) {
+        if (ey >= Cyy - 0.01 && ey <= Cyy + 0.01) {
+        } else {
+            if (ey > Cyy) {
+                ey = ey - 0.004;
+            }
+            if (ey < Cyy) {
+                ey = ey + 0.004;
+            }
         }
-        else
-        {
-            if (Cy > Cyy)
-            {
-                Cy = Cy - 0.005;
-            }
-            if (Cy < Cyy)
-            {
-                Cy = Cy + 0.005;
-            }
+    } else {
+        if (ex > Cxx) {
+            ex = ex - 0.004;
+        }
+        if (ex < Cxx) {
+            ex = ex + 0.004;
         }
     }
-    else
-    {
-        if (Cx > Cxx)
-        {
-            Cx = Cx - 0.005;
-        }
-        if (Cx < Cxx)
-        {
-            Cx = Cx + 0.005;
-        }
-    } 
 }
 
-double PID_controller(double error)
+
+
+
+
+
+
+void position_controll_void()
 {
-    static double error_integral = 0;
-    static double error_prev = error;
-    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-    
-    //proportional part
-    double u_k = Kp * error;
-    
-    //Integral part
-    error_integral = error_integral + error * Ts;
-    double u_i = Ki * error_integral;
-    
-    // Derivative part
-    double error_derivative = (error - error_prev)/Ts;
-    double filtered_error_derivative = LowPassFilter.step(error_derivative);
-    double u_d = Kd * filtered_error_derivative;
-    error_prev = error;
-    return ((u_k + u_i + u_d)/100);
+    bas = true;
 }
 
+
 void motor_controler()
 {
-    bb = Enc1.getPulses() + 2100;
-    bc = Enc2.getPulses();
-    if (bb >= counts_a)
-    {
-        z = PID_controller((counts_a - bb));
-        PMW1.write(abs(z));
-        M1 = 0;
-    }  
-    if (bb <= counts_a)
-    {
+    bb = -(Enc1.getPulses()) - 201;
+    bc = Enc2.getPulses() + 2100;
 
-        z = PID_controller((counts_a - bb));
+    if (bb >= counts_a) {
+        z = PID_controller((counts_a - bb),Kp, Ki, Kd, Ts);
         PMW1.write(abs(z));
         M1 = 1;
     }
-    if (bc >= counts_b)
-    {
+    if (bb <= counts_a) {
+        z = PID_controller((counts_a - bb),Kp, Ki, Kd, Ts);
+        PMW1.write(abs(z));
+        M1 = 0;
+    }
+    if (bc >= counts_b) {
         M2 = 0;
-        z = PID_controller((counts_b) - bc);
+        z = PID_controller((counts_b - bc),Kp, Ki, Kd, Ts);
         PMW2.write(abs(z));
-    }  
-    if (bc <= counts_b)
-    {
+    }
+    if (bc <= counts_b) {
         M2 = 1;
-        z = PID_controller((counts_b) - bc);
+        z = PID_controller((counts_b - bc),Kp, Ki, Kd, Ts);
         PMW2.write(abs(z));
-    } 
+    }
 }
-    
-void position_controll_void()
-{
-    Led = 1;
-    
-    position_define();     
-    angle_define();
-    motor_controler();
-    Led = 0;
-    
-
-}
-
 
 int main()
 {
@@ -212,10 +309,29 @@
     X[1][0] = X0[1][0];
     Xold[0][0] = X0[0][0];
     Xold[1][0] = X0[1][0];
+    pc.baud(115200);
+    x1 = Cxx;
+    y1= Cyy;
+    bqc.add( &bq1 ).add( &bq2 );
+    bqc2.add( &bq3 );
+    bqc3.add( &bq4 ).add( &bq5 );
+    bqc4.add( &bq6 );
     position_controll.attach(position_controll_void,0.001);
-    while(true)
-    {
+    while(true) {
 
-        
+        if(bas == true) {
+            Led = 1;
+            filteren();
+            position_define();
+            angle_define();
+            motor_controler();
+            
+            sw2.fall(change);
+            Position1x(emgFiltered3);
+            Position1y(emgFiltered23);
+            Led = 0;
+            bas= false;
+        }
     }
+
 }