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 }