File content as of revision 1:26bd86929178:

#include "mbed.h"
#include "QEI.h"


QEI encoder1(D10, D11, NC, 32);
QEI encoder2(D12, D13, NC, 32);

double L1 = 40.0; //cm
double L2 = 40.0; //cm

/*void forward(double M1,double M2,double & X1,double &Y1) //Forward kinematics
{
    THETA1 = M1/9.0; //Angle of motor 1 after gears
    THETA2 = M2/4.0; //Angle of motor 2 after gears
    X1 = L1 * cos(THETA1) + L2 * cos(THETA1 + THETA2); // gives x coordinates
    Y1 = L1 * sin(THETA1) + L2 * sin(THETA1 + THETA2); // gives y coordinates
    }*/
/*
void inverse(double X1, double Y1, double & THETA1, double & THETA2) //inverse kinematics
{
THETA1 =  (acos((pow(-L1,2.0)-pow(X1,2.0)-pow(Y1,2.0)+pow(L2,2.0))/(-2.0*L1*sqrt(pow(X1,2.0)+pow(Y1,2.0))))+atan(Y1/X1))*9.0; //gives theta M1 in radians
//THETA2 =  (acos((pow(X1,2.0)+ pow(Y1,2.0)-pow(L1,2.0)-pow(L2,2.0))/(-2.0*L1*L2))-3.1416)*4.0; //gives theta M2 in radians
THETA2=  (acos(X1-L1*cos(THETA1))/(L2))*4.0; //gives theta M2 in radians
}
*/

void inverse(double X1, double Y1, double & thetaM1, double & thetaM2)
{
    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;
    }


double meas_pos1 () //Encoder functie
{
    double encoderpulses = encoder1.getPulses(); //number of pulses
    double motorangle = encoderpulses/32.0/131.25*2.0*3.1416; //motor angle in radians
    return motorangle;
}

double meas_pos2 () //Encoder functie
{
    double encoderpulses = encoder2.getPulses(); //number of pulses
    double motorangle = encoderpulses/32.0/131.25*2.0*3.1416; //motor angle in radians
    return motorangle;
}

void kinematics (double unit_vX, double unit_vY, double & THETA1, double & THETA2)// Kinematics function, takes imput between 1 and -1
{
    double Ts = 0.0001; //timestep(see whatever ticker we attach to this)
    double V_max = 30.0; //Maximum velocity in either direction
    
    double deltaX = Ts*V_max*unit_vX; // imput to delta
    double deltaY = Ts*V_max*unit_vY;
    
    static double X1;
    static double Y1;
    X1 += deltaX;
    Y1 += deltaY;
    inverse(X1, Y1, THETA1, THETA2);
}




int main()
{
    while (true) {
    }
}