/* Function: Inverse Velocity
* Inputs: current task position (X, Y, Z) and desired task velocities (Xd, Yd, Zd)
* Outputs: resulting joint velocities (Ad, Bd, Cd)
*/
int servostock_velInverse(float X, float Y, float Z, float Xd, float Yd, float Zd,
float * Ad, float * Bd, float * Cd){
// Calculate Current Joint Positions from given Tool Positions
float A = 0;
float B = 0;
float C = 0;
if (servostock_calcInverse(X, Y, Z, &A, &B, &C))
return 1;
// Form Inverse Jacobian
float J[3][3] = {{0}};
if (formJacobian(A, B, C, J, getBaseRadius(), getEndEffectorRadius(), getRodLength()))
return 2;
float Jinv[3][3] = {{0}};
if (formInvJacobian(A, B, C, J, Jinv))
return 3;
// Inverse calculation: jointVel = Jinv * taskVel
float jointVel[3] = {0};
float taskVel[3] = {Xd, Yd, Zd};
if (mtxMlt_ThreeByOne(Jinv, taskVel, jointVel))
return 4;
// Return Values
Ad[0] = jointVel[0];
Bd[0] = jointVel[1];
Cd[0] = jointVel[2];
return 0; //success
}
/* Function: Forward Velocity
* Inputs: current joint position (A, B, C) and desired joint velocities (Ad, Bd, Cd)
* Outputs: resulting task velocities (Xd, Yd, Zd)
*/
int servostock_velForward(float A, float B, float C, float Ad, float Bd, float Cd,
float * Xd, float * Yd, float * Zd)
{
// Form Jacobian
float J[3][3] = {{0}};
if (formJacobian(A, B, C, J, getBaseRadius(), getEndEffectorRadius(), getRodLength()))
return 1;
// Forward Calculation: taskVel = J * joinVel
float taskVel[3] = {0};
float jointVel[3] = {Ad, Bd, Cd};
mtxMlt_ThreeByOne(J, jointVel, taskVel);
// Return Values
Xd[0] = taskVel[0];
Yd[0] = taskVel[1];
Zd[0] = taskVel[2];
return 0;
}
void HomeLinks(){
if(homingAllLinks){
if ( GetPIDCalibrateionState(linkToHWIndex(0))==CALIBRARTION_DONE&&
GetPIDCalibrateionState(linkToHWIndex(1))==CALIBRARTION_DONE&&
GetPIDCalibrateionState(linkToHWIndex(2))==CALIBRARTION_DONE
){
homingAllLinks = FALSE;
configured = TRUE;
println_W("All linkes reported in");
pidReset(hwMap.Extruder0.index,0);
int i;
float Alpha,Beta,Gama;
servostock_calcInverse(0, 0, getmaxZ(), &Alpha, &Beta, &Gama);
for(i=0;i<3;i++){
pidReset(linkToHWIndex(i), (Alpha+getRodLength()/3)/getLinkScale(i));
}
initializeCartesianController();
cancelPrint();
}
}
}
int servostock_calcInverse(float X, float Y, float Z, float *Alpha, float *Beta, float *Gamma){
float L = getRodLength();
float R = getBaseRadius()-getEndEffectorRadius();
float Lsqr=L*L;
float maxRad=sqrt((X*X)+(Y*Y));
//#warning "Z is not used yet"
if((maxRad>(L-R))|| (Z<getminZ())||(Z>(getmaxZ()+L))){
println_E("Outside of workspace x=");p_fl_E(X);print_E(" y=");p_fl_E(Y);print_E(" z=");p_fl_E(Z);print_E(" Bound radius=");p_fl_E((maxRad));
//printf("\r\nOutside of workspace x= %g y=%g z=%g Bound = %g",X,Y,Z,maxRad);
return 1;//This is ourside the reachable work area
}
float SIN_60 = 0.8660254037844386;
float COS_60 = 0.5;
// Values are in mm, Alpha, Beta, Gamma starts at 0 at the base platform.
Alpha[0] = sqrt(Lsqr - (0 - X)*(0 - X) - (R - Y)*(R - Y))+Z;
Beta[0] = sqrt(Lsqr - (-SIN_60*R - X)*(-SIN_60*R - X) - (-COS_60*R - Y)*(-COS_60*R - Y))+Z;
Gamma[0] = sqrt(Lsqr - (SIN_60*R - X)*(SIN_60*R - X) - (-COS_60*R - Y)*(-COS_60*R - Y))+Z;
if( abs(Alpha[0]-Beta[0])>L||
abs(Alpha[0]-Gamma[0])>L||
abs(Beta[0]-Alpha[0])>L||
abs(Beta[0]-Gamma[0])>L||
abs(Gamma[0]-Alpha[0])>L||
abs(Gamma[0]-Beta[0])>L){
println_E("Outside of workspace x=");p_fl_E(X);print_E(" y=");p_fl_E(Y);print_E(" z=");p_fl_E(Z);print_E(" Bound radius=");p_fl_E((maxRad));
println_E("Alpha=");p_fl_E(Alpha[0]);
print_E(" Beta=");p_fl_E(Beta[0]);
print_E(" Gama=");p_fl_E(Gamma[0]);
return 1;//This is ourside the reachable work area
}
return 0;//SUCCESS
}
int servostock_calcForward(float Alpha, float Beta, float Gamma, float * X, float *Y, float * Z){
// modified from https://gist.github.com/kastner/5279172
//http://www.cutting.lv/fileadmin/user_upload/lindeltakins.c
//http://blog.machinekit.io/2013/07/linear-delta-kinematics.html
float DELTA_DIAGONAL_ROD = getRodLength();
// Horizontal offset from middle of printer to smooth rod center.
float DELTA_SMOOTH_ROD_OFFSET = getBaseRadius(); // mm
// Horizontal offset of the universal joints on the end effector.
// DELTA_EFFECTOR_OFFSET = 32.0 // mm
//float DELTA_EFFECTOR_OFFSET = 32.0 // mm
// Horizontal offset of the universal joints on the carriages.
// DELTA_CARRIAGE_OFFSET = 26.0 // mm
float DELTA_CARRIAGE_OFFSET = getEndEffectorRadius(); // mm
// In order to correct low-center, DELTA_RADIUS must be increased.
// In order to correct high-center, DELTA_RADIUS must be decreased.
// For convex/concave -- -20->-30 makes the center go DOWN
// DELTA_FUDGE -27.4 // 152.4 total radius
float DELTA_FUDGE =0;
// Effective horizontal distance bridged by diagonal push rods.
float DELTA_RADIUS = (DELTA_SMOOTH_ROD_OFFSET-DELTA_CARRIAGE_OFFSET-DELTA_FUDGE);
float SIN_60 = 0.8660254037844386;
float COS_60 = 0.5;
//float DELTA_TOWER1_X = 0.0; // back middle tower
float DELTA_TOWER1_Y = DELTA_RADIUS;
float DELTA_TOWER2_X = -SIN_60*DELTA_RADIUS; // front left tower
float DELTA_TOWER2_Y = -COS_60*DELTA_RADIUS;
float DELTA_TOWER3_X = SIN_60*DELTA_RADIUS; // front right tower
float DELTA_TOWER3_Y = -COS_60*DELTA_RADIUS;
float y1 = DELTA_TOWER1_Y;
float z1 = Alpha;
float x2 = DELTA_TOWER2_X;
float y2 = DELTA_TOWER2_Y;
float z2 = Beta;
float x3 = DELTA_TOWER3_X;
float y3 = DELTA_TOWER3_Y;
float z3 = Gamma;
float re = DELTA_DIAGONAL_ROD;
float dnm = (y2-y1)*x3-(y3-y1)*x2;
float w1 = y1*y1 + z1*z1;
float w2 = x2*x2 + y2*y2 + z2*z2;
float w3 = x3*x3 + y3*y3 + z3*z3;
// x = (a1*z + b1)/dnm
float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);
float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;
// y = (a2*z + b2)/dnm;
float a2 = -(z2-z1)*x3+(z3-z1)*x2;
float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0;
// a*z^2 + b*z + c = 0
float a = a1*a1 + a2*a2 + dnm*dnm;
float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm);
float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re);
// discriminant
float d = b*b - 4.0*a*c;
if (d < 0)
return -1; // non-existing point
Z[0] = -0.5*(b+sqrt(d))/a;
X[0] = (a1*Z[0] + b1)/dnm;
Y[0] = (a2*Z[0] + b2)/dnm;
return 0;//success
}
