I've been trying to make a 6 DOF controller for class this semester but I've hit a wall. The mechanism to tell translational position is akin to a linear delta robot with sliding potentiometers instead of steppers.
The trouble is that I found some people who wrote code for the forward kinematics of similar systems but they only have it in java. One source is the fantastic rundown of the rostock printer by Steve Graves, however the code:
double[] forwardKinematics(double[] dZ){
double[] cartesian = new double[3];
double[][] dColP = new double[3][3];
//Create the three points from the X-Y for each column combined with
//the input Z values
for(int iIdx = 0; iIdx < 3; iIdx++){
dColP[iIdx][0] = dCol[iIdx][0];
dColP[iIdx][1] = dCol[iIdx][1];
dColP[iIdx][2] = dZ[iIdx];
}
//dColP now has the three points on the columns. Calculate the vectors
//representing the sides of the triangle
double[] dv01 = vectorSub(dColP[1], dColP[0]);
double[] dv02 = vectorSub(dColP[2], dColP[0]);
double[] dv12 = vectorSub(dColP[2], dColP[1]);
double dMag01 = vectorMag(dv01);
double dMag02 = vectorMag(dv02);
double dMag12 = vectorMag(dv12);
//This gives us a vector normal to the plane of the triangle
double[] dvZ = vectorCrossProd(dv02, dv01);
double dMagZ = vectorMag(dvZ);
double dDeterminate = 2 * Math.pow(dMagZ, 2);
double alpha = Math.pow(dMag12, 2) * vectorDotProd(dv01, dv02)/dDeterminate;
double beta = -Math.pow(dMag02, 2) * vectorDotProd(dv12, dv01)/dDeterminate;
double gamma = Math.pow(dMag01, 2) * vectorDotProd(dv02, dv12)/dDeterminate;
double[] pCircumcenter =
vectorAdd(vectorMult(dColP[0], alpha), vectorMult(dColP[1], beta));
pCircumcenter = vectorAdd(pCircumcenter, vectorMult(dColP[2], gamma));
double[] dvCircumcenter = vectorSub(pCircumcenter, dColP[0]);
//Find the length from the circumcenter to the carriage point on a column
//(by the definition of circumcenter the distance is the same to any column)
double dMag2Circumcenter = vectorMag(dvCircumcenter);
//Now use the Pythagorem theorum to calculate the distance
double dZLen =
Math.sqrt(Math.pow(dArmLen, 2) - Math.pow(dMag2Circumcenter, 2));
//Create the new vector and add it to the circumcenter point
cartesian = vectorAdd(pCircumcenter, vectorMult(dvZ, dZLen/dMagZ));
return cartesian;
is beyond me as I don't know how to use other coding languages besides C++.
Another bit of code I found is by Erik Kastner
DELTA_DIAGONAL_ROD = 288.5
// Horizontal offset from middle of printer to smooth rod center.
DELTA_SMOOTH_ROD_OFFSET = 206.0 // mm
// Horizontal offset of the universal joints on the end effector.
// DELTA_EFFECTOR_OFFSET = 32.0 // mm
DELTA_EFFECTOR_OFFSET = 32.0 // mm
// Horizontal offset of the universal joints on the carriages.
// DELTA_CARRIAGE_OFFSET = 26.0 // mm
DELTA_CARRIAGE_OFFSET = 25.0 // 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
DELTA_FUDGE = 0.5
// Effective horizontal distance bridged by diagonal push rods.
DELTA_RADIUS = (DELTA_SMOOTH_ROD_OFFSET-DELTA_EFFECTOR_OFFSET-DELTA_CARRIAGE_OFFSET-DELTA_FUDGE)
// DELTA_RADIUS = (DELTA_SMOOTH_ROD_OFFSET-DELTA_EFFECTOR_OFFSET-DELTA_CARRIAGE_OFFSET-DELTA_FUDGE)
SIN_60 = 0.8660254037844386
COS_60 = 0.5
DELTA_TOWER1_X = 0.0 // back middle tower
DELTA_TOWER1_Y = DELTA_RADIUS
DELTA_TOWER2_X = -SIN_60*DELTA_RADIUS // front left tower
DELTA_TOWER2_Y = -COS_60*DELTA_RADIUS
DELTA_TOWER3_X = SIN_60*DELTA_RADIUS // front right tower
DELTA_TOWER3_Y = -COS_60*DELTA_RADIUS
function sqrt(num) { return Math.sqrt(num); }
function sq(num) { return num*num; }
X_AXIS = 0;
Y_AXIS = 1;
Z_AXIS = 2;
function inverse(cartesian) {
var delta = [];
delta[X_AXIS] = sqrt(sq(DELTA_DIAGONAL_ROD)
- sq(DELTA_TOWER1_X-cartesian[X_AXIS])
- sq(DELTA_TOWER1_Y-cartesian[Y_AXIS])
) + cartesian[Z_AXIS];
delta[Y_AXIS] = sqrt(sq(DELTA_DIAGONAL_ROD)
- sq(DELTA_TOWER2_X-cartesian[X_AXIS])
- sq(DELTA_TOWER2_Y-cartesian[Y_AXIS])
) + cartesian[Z_AXIS];
delta[Z_AXIS] = sqrt(sq(DELTA_DIAGONAL_ROD)
- sq(DELTA_TOWER3_X-cartesian[X_AXIS])
- sq(DELTA_TOWER3_Y-cartesian[Y_AXIS])
) + cartesian[Z_AXIS];
return delta;
}
function forward(delta) {
var cartesian = [];
var y1 = DELTA_TOWER1_Y;
var z1 = delta[X_AXIS];
var x2 = DELTA_TOWER2_X;
var y2 = DELTA_TOWER2_Y;
var z2 = delta[Y_AXIS];
var x3 = DELTA_TOWER3_X;
var y3 = DELTA_TOWER3_Y;
var z3 = delta[Z_AXIS];
var re = DELTA_DIAGONAL_ROD;
var dnm = (y2-y1)*x3-(y3-y1)*x2;
var w1 = y1*y1 + z1*z1;
var w2 = x2*x2 + y2*y2 + z2*z2;
var w3 = x3*x3 + y3*y3 + z3*z3;
// x = (a1*z + b1)/dnm
var a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);
var b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;
// y = (a2*z + b2)/dnm;
var a2 = -(z2-z1)*x3+(z3-z1)*x2;
var b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0;
// a*z^2 + b*z + c = 0
var a = a1*a1 + a2*a2 + dnm*dnm;
var b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm);
var c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re);
// discriminant
var d = b*b - 4.0*a*c;
if (d < 0) return -1; // non-existing point
cartesian[Z_AXIS] = -0.5*(b+sqrt(d))/a;
cartesian[X_AXIS] = (a1*cartesian[Z_AXIS] + b1)/dnm;
cartesian[Y_AXIS] = (a2*cartesian[Z_AXIS] + b2)/dnm;
return cartesian;
}
exports.forward = forward;
exports.inverse = inverse;
is really much more straightforward to me, but arduino does not appreciate the java version of declaring functions and operations.
For my purpose I need just the final portion after "function forward(delta)". Best I've come up with so far is to declare the variables at the top and the math at the bottom is all very doable in C. I'm just struggling trying to figure out how to translate this math into an arduino friendly format. I would love to learn java and do this myself, but the semester is rapidly closing and I really don't have the time. Thanks in advance, any advice will help.