void cancelPrint(){
Print_Level l = getPrintLevel();
println_I("Cancel Print");
setPrintLevel(l);
while(FifoGetPacketCount(&packetFifo)>0){
FifoGetPacket(&packetFifo,&linTmpPack);
}
setInterpolateXYZ(0, 0, getmaxZ(), 0);
ZeroPID(hwMap.Extruder0.index);
SetPIDTimed(hwMap.Heater0.index,0,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
}
