1 #include "LinearDeltaSolution.h"
3 #include "ConfigValue.h"
4 #include "libs/Kernel.h"
5 #include "libs/nuts_bolts.h"
6 #include "libs/Config.h"
12 #define arm_length_checksum CHECKSUM("arm_length")
13 #define arm_radius_checksum CHECKSUM("arm_radius")
15 #define tower1_offset_checksum CHECKSUM("delta_tower1_offset")
16 #define tower2_offset_checksum CHECKSUM("delta_tower2_offset")
17 #define tower3_offset_checksum CHECKSUM("delta_tower3_offset")
18 #define tower1_angle_checksum CHECKSUM("delta_tower1_angle")
19 #define tower2_angle_checksum CHECKSUM("delta_tower2_angle")
20 #define tower3_angle_checksum CHECKSUM("delta_tower3_angle")
22 #define SQ(x) powf(x, 2)
23 #define ROUND(x, y) (roundf(x * (float)(1e ## y)) / (float)(1e ## y))
24 #define PIOVER180 0.01745329251994329576923690768489F
26 LinearDeltaSolution::LinearDeltaSolution(Config
* config
)
28 // arm_length is the length of the arm from hinge to hinge
29 arm_length
= config
->value(arm_length_checksum
)->by_default(250.0f
)->as_number();
30 // arm_radius is the horizontal distance from hinge to hinge when the effector is centered
31 arm_radius
= config
->value(arm_radius_checksum
)->by_default(124.0f
)->as_number();
33 tower1_angle
= config
->value(tower1_angle_checksum
)->by_default(0.0f
)->as_number();
34 tower2_angle
= config
->value(tower2_angle_checksum
)->by_default(0.0f
)->as_number();
35 tower3_angle
= config
->value(tower3_angle_checksum
)->by_default(0.0f
)->as_number();
36 tower1_offset
= config
->value(tower1_offset_checksum
)->by_default(0.0f
)->as_number();
37 tower2_offset
= config
->value(tower2_offset_checksum
)->by_default(0.0f
)->as_number();
38 tower3_offset
= config
->value(tower3_offset_checksum
)->by_default(0.0f
)->as_number();
43 void LinearDeltaSolution::init() {
44 arm_length_squared
= SQ(arm_length
);
46 // Effective X/Y positions of the three vertical towers.
47 float delta_radius
= arm_radius
;
49 delta_tower1_x
= (delta_radius
+ tower1_offset
) * cosf((210.0F
+ tower1_angle
) * PIOVER180
); // front left tower
50 delta_tower1_y
= (delta_radius
+ tower1_offset
) * sinf((210.0F
+ tower1_angle
) * PIOVER180
);
51 delta_tower2_x
= (delta_radius
+ tower2_offset
) * cosf((330.0F
+ tower2_angle
) * PIOVER180
); // front right tower
52 delta_tower2_y
= (delta_radius
+ tower2_offset
) * sinf((330.0F
+ tower2_angle
) * PIOVER180
);
53 delta_tower3_x
= (delta_radius
+ tower3_offset
) * cosf((90.0F
+ tower3_angle
) * PIOVER180
); // back middle tower
54 delta_tower3_y
= (delta_radius
+ tower3_offset
) * sinf((90.0F
+ tower3_angle
) * PIOVER180
);
57 void LinearDeltaSolution::cartesian_to_actuator( float cartesian_mm
[], float actuator_mm
[] )
60 actuator_mm
[ALPHA_STEPPER
] = sqrtf(this->arm_length_squared
61 - SQ(delta_tower1_x
- cartesian_mm
[X_AXIS
])
62 - SQ(delta_tower1_y
- cartesian_mm
[Y_AXIS
])
63 ) + cartesian_mm
[Z_AXIS
];
64 actuator_mm
[BETA_STEPPER
] = sqrtf(this->arm_length_squared
65 - SQ(delta_tower2_x
- cartesian_mm
[X_AXIS
])
66 - SQ(delta_tower2_y
- cartesian_mm
[Y_AXIS
])
67 ) + cartesian_mm
[Z_AXIS
];
68 actuator_mm
[GAMMA_STEPPER
] = sqrtf(this->arm_length_squared
69 - SQ(delta_tower3_x
- cartesian_mm
[X_AXIS
])
70 - SQ(delta_tower3_y
- cartesian_mm
[Y_AXIS
])
71 ) + cartesian_mm
[Z_AXIS
];
74 void LinearDeltaSolution::actuator_to_cartesian( float actuator_mm
[], float cartesian_mm
[] )
76 // from http://en.wikipedia.org/wiki/Circumscribed_circle#Barycentric_coordinates_from_cross-_and_dot-products
77 // based on https://github.com/ambrop72/aprinter/blob/2de69a/aprinter/printer/DeltaTransform.h#L81
78 Vector3
tower1( delta_tower1_x
, delta_tower1_y
, actuator_mm
[0] );
79 Vector3
tower2( delta_tower2_x
, delta_tower2_y
, actuator_mm
[1] );
80 Vector3
tower3( delta_tower3_x
, delta_tower3_y
, actuator_mm
[2] );
82 Vector3 s12
= tower1
.sub(tower2
);
83 Vector3 s23
= tower2
.sub(tower3
);
84 Vector3 s13
= tower1
.sub(tower3
);
86 Vector3 normal
= s12
.cross(s23
);
88 float magsq_s12
= s12
.magsq();
89 float magsq_s23
= s23
.magsq();
90 float magsq_s13
= s13
.magsq();
92 float inv_nmag_sq
= 1.0F
/ normal
.magsq();
93 float q
= 0.5F
* inv_nmag_sq
;
95 float a
= q
* magsq_s23
* s12
.dot(s13
);
96 float b
= q
* magsq_s13
* s12
.dot(s23
) * -1.0F
; // negate because we use s12 instead of s21
97 float c
= q
* magsq_s12
* s13
.dot(s23
);
99 Vector3
circumcenter( delta_tower1_x
* a
+ delta_tower2_x
* b
+ delta_tower3_x
* c
,
100 delta_tower1_y
* a
+ delta_tower2_y
* b
+ delta_tower3_y
* c
,
101 actuator_mm
[0] * a
+ actuator_mm
[1] * b
+ actuator_mm
[2] * c
);
103 float r_sq
= 0.5F
* q
* magsq_s12
* magsq_s23
* magsq_s13
;
104 float dist
= sqrtf(inv_nmag_sq
* (arm_length_squared
- r_sq
));
106 Vector3 cartesian
= circumcenter
.sub(normal
.mul(dist
));
108 cartesian_mm
[0] = ROUND(cartesian
[0], 4);
109 cartesian_mm
[1] = ROUND(cartesian
[1], 4);
110 cartesian_mm
[2] = ROUND(cartesian
[2], 4);
113 bool LinearDeltaSolution::set_optional(const arm_options_t
& options
) {
115 for(auto &i
: options
) {
117 case 'L': arm_length
= i
.second
; break;
118 case 'R': arm_radius
= i
.second
; break;
119 case 'A': tower1_offset
= i
.second
; break;
120 case 'B': tower2_offset
= i
.second
; break;
121 case 'C': tower3_offset
= i
.second
; break;
122 case 'D': tower1_angle
= i
.second
; break;
123 case 'E': tower2_angle
= i
.second
; break;
124 case 'F': tower3_angle
= i
.second
; break;
131 bool LinearDeltaSolution::get_optional(arm_options_t
& options
) {
132 options
['L']= this->arm_length
;
133 options
['R']= this->arm_radius
;
135 // don't report these if none of them are set
136 if(this->tower1_offset
!= 0.0F
|| this->tower2_offset
!= 0.0F
|| this->tower3_offset
!= 0.0F
||
137 this->tower1_angle
!= 0.0F
|| this->tower2_angle
!= 0.0F
|| this->tower3_angle
!= 0.0F
) {
139 options
['A'] = this->tower1_offset
;
140 options
['B'] = this->tower2_offset
;
141 options
['C'] = this->tower3_offset
;
142 options
['D'] = this->tower1_angle
;
143 options
['E'] = this->tower2_angle
;
144 options
['F'] = this->tower3_angle
;