/* 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
}
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();
}
}
}
