// inverse kinematics: (x0, y0, z0) -> (theta1, theta2, theta3)
// returned status: 0=OK, -1=non-existing position
int delta_calcInverse(float x0, float y0, float z0, float &theta1, float &theta2, float &theta3) {
theta1 = theta2 = theta3 = 0;
int status = delta_calcAngleYZ(x0, y0, z0, theta1);
if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0, theta2); // rotate coords to +120 deg
if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z0, theta3); // rotate coords to -120 deg
return status;
}
void RotaryDeltaSolution::cartesian_to_actuator(const float cartesian_mm[], ActuatorCoordinates &actuator_mm )
{
//We need to translate the Cartesian coordinates in mm to the actuator position required in mm so the stepper motor functions
float alpha_theta = 0.0F;
float beta_theta = 0.0F;
float gamma_theta = 0.0F;
//Code from Trossen Robotics tutorial, has X in front Y to the right and Z to the left
// firepick is X at the back and negates X0 X0
// selected by a config option
float x0 = cartesian_mm[X_AXIS];
float y0 = cartesian_mm[Y_AXIS];
if(mirror_xy) {
x0= -x0;
y0= -y0;
}
float z_with_offset = cartesian_mm[Z_AXIS] + z_calc_offset; //The delta calculation below places zero at the top. Subtract the Z offset to make zero at the bottom.
int status = delta_calcAngleYZ(x0, y0, z_with_offset, alpha_theta);
if (status == 0) status = delta_calcAngleYZ(x0 * cos120 + y0 * sin120, y0 * cos120 - x0 * sin120, z_with_offset, beta_theta); // rotate co-ordinates to +120 deg
if (status == 0) status = delta_calcAngleYZ(x0 * cos120 - y0 * sin120, y0 * cos120 + x0 * sin120, z_with_offset, gamma_theta); // rotate co-ordinates to -120 deg
if (status == -1) { //something went wrong,
//force to actuator FPD home position as we know this is a valid position
actuator_mm[ALPHA_STEPPER] = 0;
actuator_mm[BETA_STEPPER ] = 0;
actuator_mm[GAMMA_STEPPER] = 0;
//DEBUG CODE, uncomment the following to help determine what may be happening if you are trying to adapt this to your own different rotary delta.
if(debug_flag) {
THEKERNEL->streams->printf("//ERROR: Delta calculation fail! Unable to move to:\n");
THEKERNEL->streams->printf("// x= %f\n", cartesian_mm[X_AXIS]);
THEKERNEL->streams->printf("// y= %f\n", cartesian_mm[Y_AXIS]);
THEKERNEL->streams->printf("// z= %f\n", cartesian_mm[Z_AXIS]);
THEKERNEL->streams->printf("// CalcZ= %f\n", z_calc_offset);
THEKERNEL->streams->printf("// Offz= %f\n", z_with_offset);
}
} else {
actuator_mm[ALPHA_STEPPER] = alpha_theta;
actuator_mm[BETA_STEPPER ] = beta_theta;
actuator_mm[GAMMA_STEPPER] = gamma_theta;
if(debug_flag) {
THEKERNEL->streams->printf("//cartesian x= %f\n\r", cartesian_mm[X_AXIS]);
THEKERNEL->streams->printf("// y= %f\n\r", cartesian_mm[Y_AXIS]);
THEKERNEL->streams->printf("// z= %f\n\r", cartesian_mm[Z_AXIS]);
THEKERNEL->streams->printf("// Offz= %f\n\r", z_with_offset);
THEKERNEL->streams->printf("// actuator x= %f\n\r", actuator_mm[X_AXIS]);
THEKERNEL->streams->printf("// y= %f\n\r", actuator_mm[Y_AXIS]);
THEKERNEL->streams->printf("// z= %f\n\r", actuator_mm[Z_AXIS]);
}
}
}
void RotatableDeltaSolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] )
{
//We need to translate the Cartesian coordinates in mm to the actuator position required in mm so the stepper motor functions
float alpha_theta = 0.0F;
float beta_theta = 0.0F;
float gamma_theta = 0.0F;
//Code from Trossen Robotics tutorial, note we put the X axis at the back and not the front of the robot.
float x0 = cartesian_mm[X_AXIS];
float y0 = cartesian_mm[Y_AXIS];
float z_with_offset = cartesian_mm[Z_AXIS] + z_calc_offset; //The delta calculation below places zero at the top. Subtract the Z offset to make zero at the bottom.
int status = delta_calcAngleYZ(x0, y0, z_with_offset, alpha_theta);
if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z_with_offset, beta_theta); // rotate co-ordinates to +120 deg
if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z_with_offset, gamma_theta); // rotate co-ordinates to -120 deg
if (status == -1) //something went wrong,
{
//force to actuator FPD home position as we know this is a valid position
actuator_mm[ALPHA_STEPPER] = 0;
actuator_mm[BETA_STEPPER ] = 0;
actuator_mm[GAMMA_STEPPER] = 0;
//DEBUG CODE, uncomment the following to help determine what may be happening if you are trying to adapt this to your own different roational delta.
// THEKERNEL->streams->printf("ERROR: Delta calculation fail! Unable to move to:\n");
// THEKERNEL->streams->printf(" x= %f\n",cartesian_mm[X_AXIS]);
// THEKERNEL->streams->printf(" y= %f\n",cartesian_mm[Y_AXIS]);
// THEKERNEL->streams->printf(" z= %f\n",cartesian_mm[Z_AXIS]);
// THEKERNEL->streams->printf(" CalcZ= %f\n",z_calc_offset);
// THEKERNEL->streams->printf(" Offz= %f\n",z_with_offset);
}
else
{
actuator_mm[ALPHA_STEPPER] = alpha_theta;
actuator_mm[BETA_STEPPER ] = beta_theta;
actuator_mm[GAMMA_STEPPER] = gamma_theta;
// THEKERNEL->streams->printf("cartesian x= %f\n\r",cartesian_mm[X_AXIS]);
// THEKERNEL->streams->printf(" y= %f\n\r",cartesian_mm[Y_AXIS]);
// THEKERNEL->streams->printf(" z= %f\n\r",cartesian_mm[Z_AXIS]);
// THEKERNEL->streams->printf(" Offz= %f\n\r",z_with_offset);
// THEKERNEL->streams->printf(" delta x= %f\n\r",delta[X_AXIS]);
// THEKERNEL->streams->printf(" y= %f\n\r",delta[Y_AXIS]);
// THEKERNEL->streams->printf(" z= %f\n\r",delta[Z_AXIS]);
}
}
