Commit | Line | Data |
---|---|---|
4e04bcd3 | 1 | #include "RostockSolution.h" |
21461a92 | 2 | #include <fastmath.h> |
7af0714f | 3 | #include "checksumm.h" |
8d54c34c | 4 | #include "ConfigValue.h" |
4e04bcd3 | 5 | |
21461a92 | 6 | #define PIOVER180 0.01745329251994329576923690768489F |
deee97d2 | 7 | |
78d0e16a MM |
8 | RostockSolution::RostockSolution(Config* config) |
9 | { | |
10 | float alpha_angle = PIOVER180 * config->value(alpha_angle_checksum)->by_default(30.0f)->as_number(); | |
11 | sin_alpha = sinf(alpha_angle); | |
12 | cos_alpha = cosf(alpha_angle); | |
13 | float beta_angle = PIOVER180 * config->value( beta_relative_angle_checksum)->by_default(120.0f)->as_number(); | |
14 | sin_beta = sinf(beta_angle); | |
15 | cos_beta = cosf(beta_angle); | |
16 | float gamma_angle = PIOVER180 * config->value(gamma_relative_angle_checksum)->by_default(240.0f)->as_number(); | |
17 | sin_gamma = sinf(gamma_angle); | |
18 | cos_gamma = cosf(gamma_angle); | |
deee97d2 | 19 | |
c826a67a | 20 | // arm_length is the length of the arm from hinge to hinge |
78d0e16a | 21 | arm_length = config->value(arm_length_checksum)->by_default(250.0f)->as_number(); |
c826a67a | 22 | // arm_radius is the horizontal distance from hinge to hinge when the effector is centered |
78d0e16a | 23 | arm_radius = config->value(arm_radius_checksum)->by_default(124.0f)->as_number(); |
c826a67a | 24 | |
78d0e16a | 25 | arm_length_squared = powf(arm_length, 2); |
4e04bcd3 L |
26 | } |
27 | ||
01cf96fb | 28 | void RostockSolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] ){ |
78d0e16a | 29 | float alpha_rotated[3], rotated[3]; |
7af0714f | 30 | |
78d0e16a | 31 | if( sin_alpha == 0 && cos_alpha == 1){ |
4c9ccd0b MM |
32 | alpha_rotated[X_AXIS] = cartesian_mm[X_AXIS]; |
33 | alpha_rotated[Y_AXIS] = cartesian_mm[Y_AXIS]; | |
34 | alpha_rotated[Z_AXIS] = cartesian_mm[Z_AXIS]; | |
1d893e5a | 35 | }else{ |
01cf96fb | 36 | rotate( cartesian_mm, alpha_rotated, sin_alpha, cos_alpha ); |
1d893e5a | 37 | } |
01cf96fb | 38 | actuator_mm[ALPHA_STEPPER] = solve_arm( alpha_rotated ); |
1d893e5a L |
39 | |
40 | rotate( alpha_rotated, rotated, sin_beta, cos_beta ); | |
01cf96fb | 41 | actuator_mm[BETA_STEPPER ] = solve_arm( rotated ); |
4e04bcd3 | 42 | |
1d893e5a | 43 | rotate( alpha_rotated, rotated, sin_gamma, cos_gamma ); |
01cf96fb | 44 | actuator_mm[GAMMA_STEPPER] = solve_arm( rotated ); |
4e04bcd3 L |
45 | } |
46 | ||
01cf96fb | 47 | void RostockSolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] ){ |
78d0e16a MM |
48 | // unimplemented |
49 | } | |
4e04bcd3 | 50 | |
01cf96fb MM |
51 | float RostockSolution::solve_arm( float cartesian_mm[]) { |
52 | return sqrtf(arm_length_squared - powf(cartesian_mm[X_AXIS] - arm_radius, 2) - powf(cartesian_mm[Y_AXIS], 2)) + cartesian_mm[Z_AXIS]; | |
4e04bcd3 L |
53 | } |
54 | ||
21461a92 | 55 | void RostockSolution::rotate(float in[], float out[], float sin, float cos ){ |
1d893e5a L |
56 | out[X_AXIS] = cos * in[X_AXIS] - sin * in[Y_AXIS]; |
57 | out[Y_AXIS] = sin * in[X_AXIS] + cos * in[Y_AXIS]; | |
c826a67a | 58 | out[Z_AXIS] = in[Z_AXIS]; |
4e04bcd3 | 59 | } |