Diff: NR_method_1.cpp

Revision:

0:0af507ea0d83

Child:

1:fafea1d00d0c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/NR_method_1.cpp	Mon Oct 22 11:57:03 2018 +0000
@@ -0,0 +1,221 @@
+#include "mbed.h"
+#include "BiQuad.h"
+#include <math.h>
+#include <stdio.h>
+#include <iostream>
+#include <stdlib.h>
+#include <ctime>
+#include <QEI.h>
+
+//Define in/outputs
+
+float 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 Ts = 0.001;
+
+float counts_a;
+float counts_b;
+
+
+//Define Variables
+
+double pi = 3.14159265359;
+double bb;
+double bc;
+float z;
+
+double angle_a = 0.5* pi; //in rad
+double angle_b = 0 * pi;  //in rad
+
+double X0[2][1] = {{angle_a},{angle_b}};
+double X[2][1];
+double Xold[2][1];
+double fval[2][1];
+double J[2][2];
+double err[2][1];
+
+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
+
+Ticker position_controll;
+
+void NR() //Newton Rapshon Calculation
+{
+    //Variables
+    double Hob = X[0][0];
+    double Hoa = X[1][0];
+    
+    //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);
+    //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));
+}
+
+
+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]);
+
+        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)
+            {
+                break;
+            }
+        }      
+    }    
+}
+
+void position_define()
+{
+    if (Cx >= Cxx)
+    {
+        if (Cy >= Cyy)
+        {
+        }
+        else
+        {
+            if (Cy > Cyy)
+            {
+                Cy = Cy - 0.005;
+            }
+            if (Cy < Cyy)
+            {
+                Cy = Cy + 0.005;
+            }
+        }
+    }
+    else
+    {
+        if (Cx > Cxx)
+        {
+            Cx = Cx - 0.005;
+        }
+        if (Cx < Cxx)
+        {
+            Cx = Cx + 0.005;
+        }
+    } 
+}
+
+double PID_controller(double error)
+{
+    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);
+}
+
+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)
+    {
+
+        z = PID_controller((counts_a - bb));
+        PMW1.write(abs(z));
+        M1 = 1;
+    }
+    if (bc >= counts_b)
+    {
+        M2 = 0;
+        z = PID_controller((counts_b) - bc);
+        PMW2.write(abs(z));
+    }  
+    if (bc <= counts_b)
+    {
+        M2 = 1;
+        z = PID_controller((counts_b) - bc);
+        PMW2.write(abs(z));
+    } 
+}
+    
+void position_controll_void()
+{
+    Led = 1;
+    
+    position_define();     
+    angle_define();
+    motor_controler();
+    Led = 0;
+    
+
+}
+
+
+int main()
+{
+    PMW1.period_us(60);
+    X[0][0] = X0[0][0];
+    X[1][0] = X0[1][0];
+    Xold[0][0] = X0[0][0];
+    Xold[1][0] = X0[1][0];
+    position_controll.attach(position_controll_void,0.001);
+    while(true)
+    {
+
+        
+    }
+}