void RostockSolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] ){
float alpha_rotated[3], rotated[3];
if( sin_alpha == 0 && cos_alpha == 1){
alpha_rotated[X_AXIS] = cartesian_mm[X_AXIS];
alpha_rotated[Y_AXIS] = cartesian_mm[Y_AXIS];
alpha_rotated[Z_AXIS] = cartesian_mm[Z_AXIS];
}else{
rotate( cartesian_mm, alpha_rotated, sin_alpha, cos_alpha );
}
actuator_mm[ALPHA_STEPPER] = solve_arm( alpha_rotated );
rotate( alpha_rotated, rotated, sin_beta, cos_beta );
actuator_mm[BETA_STEPPER ] = solve_arm( rotated );
rotate( alpha_rotated, rotated, sin_gamma, cos_gamma );
actuator_mm[GAMMA_STEPPER] = solve_arm( rotated );
}
void RostockSolution::millimeters_to_steps( double millimeters[], int steps[] ){
float mm[3], alpha_rotated[3], rotated[3];
// convert input to float
mm[0]= millimeters[0];
mm[1]= millimeters[1];
mm[2]= millimeters[2];
if( this->sin_alpha == 0 && this->cos_alpha == 1){
alpha_rotated[0] = mm[0];
alpha_rotated[1] = mm[1];
alpha_rotated[2] = mm[2];
}else{
rotate( mm, alpha_rotated, sin_alpha, cos_alpha );
}
steps[ALPHA_STEPPER] = lround( solve_arm( alpha_rotated ) * this->alpha_steps_per_mm );
rotate( alpha_rotated, rotated, sin_beta, cos_beta );
steps[BETA_STEPPER ] = lround( solve_arm( rotated ) * this->beta_steps_per_mm );
rotate( alpha_rotated, rotated, sin_gamma, cos_gamma );
steps[GAMMA_STEPPER] = lround( solve_arm( rotated ) * this->gamma_steps_per_mm );
}
