JohannKosselSolution.cpp

00001 #include "JohannKosselSolution.h"
00002 #include <fastmath.h>
00003 
00004 #include "Vector3.h"
00005 
00006 #define SQ(x) powf(x, 2)
00007 #define ROUND(x, y) (roundf(x * 1e ## y) / 1e ## y)
00008 
00009 JohannKosselSolution::JohannKosselSolution(Config* config)
00010 {
00011     // arm_length is the length of the arm from hinge to hinge
00012     arm_length         = config->value(arm_length_checksum)->by_default(250.0f)->as_number();
00013     // arm_radius is the horizontal distance from hinge to hinge when the effector is centered
00014     arm_radius         = config->value(arm_radius_checksum)->by_default(124.0f)->as_number();
00015 
00016     init();
00017 }
00018 
00019 void JohannKosselSolution::init() {
00020     arm_length_squared = SQ(arm_length);
00021 
00022     // Effective X/Y positions of the three vertical towers.
00023     float DELTA_RADIUS = arm_radius;
00024 
00025     float SIN_60   = 0.8660254037844386F;
00026     float COS_60   = 0.5F;
00027 
00028     DELTA_TOWER1_X = -SIN_60 * DELTA_RADIUS; // front left tower
00029     DELTA_TOWER1_Y = -COS_60 * DELTA_RADIUS;
00030 
00031     DELTA_TOWER2_X =  SIN_60 * DELTA_RADIUS; // front right tower
00032     DELTA_TOWER2_Y = -COS_60 * DELTA_RADIUS;
00033 
00034     DELTA_TOWER3_X = 0.0F; // back middle tower
00035     DELTA_TOWER3_Y = DELTA_RADIUS;
00036 }
00037 
00038 void JohannKosselSolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] )
00039 {
00040     actuator_mm[ALPHA_STEPPER] = sqrtf(this->arm_length_squared
00041                                 - SQ(DELTA_TOWER1_X - cartesian_mm[X_AXIS])
00042                                 - SQ(DELTA_TOWER1_Y - cartesian_mm[Y_AXIS])
00043                                 ) + cartesian_mm[Z_AXIS];
00044     actuator_mm[BETA_STEPPER ] = sqrtf(this->arm_length_squared
00045                                 - SQ(DELTA_TOWER2_X - cartesian_mm[X_AXIS])
00046                                 - SQ(DELTA_TOWER2_Y - cartesian_mm[Y_AXIS])
00047                                 ) + cartesian_mm[Z_AXIS];
00048     actuator_mm[GAMMA_STEPPER] = sqrtf(this->arm_length_squared
00049                                 - SQ(DELTA_TOWER3_X - cartesian_mm[X_AXIS])
00050                                 - SQ(DELTA_TOWER3_Y - cartesian_mm[Y_AXIS])
00051                                 ) + cartesian_mm[Z_AXIS];
00052 }
00053 
00054 void JohannKosselSolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] )
00055 {
00056     // from http://en.wikipedia.org/wiki/Circumscribed_circle#Barycentric_coordinates_from_cross-_and_dot-products
00057     // based on https://github.com/ambrop72/aprinter/blob/2de69a/aprinter/printer/DeltaTransform.h#L81
00058     Vector3 tower1( DELTA_TOWER1_X, DELTA_TOWER1_Y, actuator_mm[0] );
00059     Vector3 tower2( DELTA_TOWER2_X, DELTA_TOWER2_Y, actuator_mm[1] );
00060     Vector3 tower3( DELTA_TOWER3_X, DELTA_TOWER3_Y, actuator_mm[2] );
00061 
00062     Vector3 s12 = tower1.sub(tower2);
00063     Vector3 s23 = tower2.sub(tower3);
00064     Vector3 s13 = tower1.sub(tower3);
00065 
00066     Vector3 normal = s12.cross(s23);
00067 
00068     float magsq_s12 = s12.magsq();
00069     float magsq_s23 = s23.magsq();
00070     float magsq_s13 = s13.magsq();
00071 
00072     float inv_nmag_sq = 1.0F / normal.magsq();
00073     float q = 0.5F * inv_nmag_sq;
00074 
00075     float a = q * magsq_s23 * s12.dot(s13);
00076     float b = q * magsq_s13 * s12.dot(s23) * -1.0F; // negate because we use s12 instead of s21
00077     float c = q * magsq_s12 * s13.dot(s23);
00078 
00079     Vector3 circumcenter( DELTA_TOWER1_X * a + DELTA_TOWER2_X * b + DELTA_TOWER3_X * c,
00080                           DELTA_TOWER1_Y * a + DELTA_TOWER2_Y * b + DELTA_TOWER3_Y * c,
00081                           actuator_mm[0] * a + actuator_mm[1] * b + actuator_mm[2] * c );
00082 
00083     float r_sq = 0.5F * q * magsq_s12 * magsq_s23 * magsq_s13;
00084     float dist = sqrtf(inv_nmag_sq * (arm_length_squared - r_sq));
00085 
00086     Vector3 cartesian = circumcenter.sub(normal.mul(dist));
00087 
00088     cartesian_mm[0] = ROUND(cartesian[0], 4);
00089     cartesian_mm[1] = ROUND(cartesian[1], 4);
00090     cartesian_mm[2] = ROUND(cartesian[2], 4);
00091 }
00092 
00093 bool JohannKosselSolution::set_optional(char parameter, float value) {
00094 
00095     switch(parameter) {
00096         case 'L': // sets arm_length
00097             arm_length= value;
00098             break;
00099         case 'R': // sets arm_radius
00100             arm_radius= value;
00101             break;
00102         default:
00103             return false;
00104     }
00105     init();
00106     return true;
00107 }
00108 
00109 bool JohannKosselSolution::get_optional(char parameter, float *value) {
00110     if(value == NULL) return false;
00111 
00112     switch(parameter) {
00113         case 'L': // get arm_length
00114             *value= this->arm_length;
00115             break;
00116         case 'R': // get arm_radius
00117             *value= this->arm_radius;
00118             break;
00119         default:
00120             return false;
00121     }
00122 
00123     return true;
00124 };