New Kinematics 3RRR Scara

[Kruix]

Hello, I would like to implement the firmware for a 3RRR Scara,
which uses 3 Arms/Motors to set the XY position.
is there an easy way to test the kinematic from a function that outputs the coresponding angles from the backward transformation? ie.
The function outputs the angles for the motors from (0,0) after Homing.

vector<double> CalculateAngles(double x, double y, double rot, double l1, double l2,
                   double R, double r)
{
    double pi=3.14159265358979;
    vector<double> phi= {7*pi/6, 11*pi/6, pi/2};
    vector<double> my= {pi/6, 5*pi/6, 3*pi/2};
    vector<double> E= {0, 0, 0};
    vector<double> F= {0, 0, 0};
    vector<double> G= {0, 0, 0};
    vector<double> K= {0, 0, 0};
    vector<double> theta= {0, 0, 0};
    for(int i=0; i<3;i++){
        E[i] = -2*l1*(y+r*sin(rot+phi[i])+R*sin(my[i]));
        F[i] = -2*l1*(x+r*cos(rot+phi[i])+R*cos(my[i])); 
        G[i] = pow(x+r*cos(rot+phi[i])+R*cos(my[i]),2.0)+ pow(y+r*sin(rot+phi[i])+R*sin(my[i]),2.0)+pow(l1,2.0)-pow(l2,2.0);
        K[i] = sqrt(pow(E[i],2.0)+pow(F[i],2.0)-pow(G[i],2.0));
        theta[i] = 2*atan2((-E[i]+K[i]),(G[i]-F[i]));
    }
    vector<double> theta_temp =cart2threeRRR(0,0,rot,l1,l2,R,r);
    theta[0]=theta[0]-theta_temp[0];
    theta[1]=theta[1]-theta_temp[1];
    if(theta[2]>3){theta[2]=theta[2]-2*Pi;}
    theta[2]=theta[2]-theta_temp[2];
    return theta;

[dc42]

@Kruix see https://github.com/Duet3D/RepRapFirmware/wiki/Adding-New-Kinematics.

[Kruix]

@dc42
The MotorStepsToCartisian is a forward kinematic, is that function necessary to implement for testing?

[dc42]

@Kruix yes, it's needed.

[Kruix]

This post is deleted!