RotatableCartesianSolution.cpp

00001 #include "RotatableCartesianSolution.h"
00002 #include <math.h>
00003 
00004 // degrees * (pi / 180) = radians
00005 #define DEG2RAD       0.01745329251994329576923690768489
00006 
00007 RotatableCartesianSolution::RotatableCartesianSolution(Config* config) {
00008     float alpha_angle  = config->value(alpha_angle_checksum)->by_default(0.0f)->as_number() * DEG2RAD;
00009     sin_alpha          = sinf(alpha_angle);
00010     cos_alpha          = cosf(alpha_angle);
00011 }
00012 
00013 void RotatableCartesianSolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] ){
00014     rotate( cartesian_mm, actuator_mm, sin_alpha, cos_alpha );
00015 }
00016 
00017 void RotatableCartesianSolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] ){
00018     rotate( actuator_mm, cartesian_mm, - sin_alpha, cos_alpha );
00019 }
00020 
00021 void RotatableCartesianSolution::rotate(float in[], float out[], float sin, float cos ){
00022     out[ALPHA_STEPPER] = cos * in[X_AXIS] - sin * in[Y_AXIS];
00023     out[BETA_STEPPER ] = sin * in[X_AXIS] + cos * in[Y_AXIS];
00024     out[GAMMA_STEPPER] =       in[Z_AXIS];
00025 }