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