Fork of Smoothie to port to mbed non-LPC targets.
Fork of Smoothie by
RostockSolution.cpp
00001 #include "RostockSolution.h" 00002 #include <fastmath.h> 00003 00004 #define PIOVER180 0.01745329251994329576923690768489F 00005 00006 RostockSolution::RostockSolution(Config* config) 00007 { 00008 float alpha_angle = PIOVER180 * config->value(alpha_angle_checksum)->by_default(30.0f)->as_number(); 00009 sin_alpha = sinf(alpha_angle); 00010 cos_alpha = cosf(alpha_angle); 00011 float beta_angle = PIOVER180 * config->value( beta_relative_angle_checksum)->by_default(120.0f)->as_number(); 00012 sin_beta = sinf(beta_angle); 00013 cos_beta = cosf(beta_angle); 00014 float gamma_angle = PIOVER180 * config->value(gamma_relative_angle_checksum)->by_default(240.0f)->as_number(); 00015 sin_gamma = sinf(gamma_angle); 00016 cos_gamma = cosf(gamma_angle); 00017 00018 // arm_length is the length of the arm from hinge to hinge 00019 arm_length = config->value(arm_length_checksum)->by_default(250.0f)->as_number(); 00020 // arm_radius is the horizontal distance from hinge to hinge when the effector is centered 00021 arm_radius = config->value(arm_radius_checksum)->by_default(124.0f)->as_number(); 00022 00023 arm_length_squared = powf(arm_length, 2); 00024 } 00025 00026 void RostockSolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] ){ 00027 float alpha_rotated[3], rotated[3]; 00028 00029 if( sin_alpha == 0 && cos_alpha == 1){ 00030 alpha_rotated[X_AXIS] = cartesian_mm[X_AXIS]; 00031 alpha_rotated[Y_AXIS] = cartesian_mm[Y_AXIS]; 00032 alpha_rotated[Z_AXIS] = cartesian_mm[Z_AXIS]; 00033 }else{ 00034 rotate( cartesian_mm, alpha_rotated, sin_alpha, cos_alpha ); 00035 } 00036 actuator_mm[ALPHA_STEPPER] = solve_arm( alpha_rotated ); 00037 00038 rotate( alpha_rotated, rotated, sin_beta, cos_beta ); 00039 actuator_mm[BETA_STEPPER ] = solve_arm( rotated ); 00040 00041 rotate( alpha_rotated, rotated, sin_gamma, cos_gamma ); 00042 actuator_mm[GAMMA_STEPPER] = solve_arm( rotated ); 00043 } 00044 00045 void RostockSolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] ){ 00046 // unimplemented 00047 } 00048 00049 float RostockSolution::solve_arm( float cartesian_mm[]) { 00050 return sqrtf(arm_length_squared - powf(cartesian_mm[X_AXIS] - arm_radius, 2) - powf(cartesian_mm[Y_AXIS], 2)) + cartesian_mm[Z_AXIS]; 00051 } 00052 00053 void RostockSolution::rotate(float in[], float out[], float sin, float cos ){ 00054 out[X_AXIS] = cos * in[X_AXIS] - sin * in[Y_AXIS]; 00055 out[Y_AXIS] = sin * in[X_AXIS] + cos * in[Y_AXIS]; 00056 out[Z_AXIS] = in[Z_AXIS]; 00057 }
Generated on Tue Jul 12 2022 20:09:02 by 1.7.2