File content as of revision 0:0af507ea0d83:

#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)
    {

        
    }
}