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