/* 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 }