Commit | Line | Data |
---|---|---|
1217e470 QH |
1 | #include "MorganSCARASolution.h" |
2 | #include <fastmath.h> | |
3 | #include "checksumm.h" | |
4 | #include "ConfigValue.h" | |
5 | #include "libs/Kernel.h" | |
6 | //#include "StreamOutputPool.h" | |
7 | //#include "Gcode.h" | |
8 | //#include "SerialMessage.h" | |
9 | //#include "Conveyor.h" | |
10 | //#include "Robot.h" | |
11 | //#include "StepperMotor.h" | |
12 | ||
13 | #include "libs/nuts_bolts.h" | |
14 | ||
15 | #include "libs/Config.h" | |
16 | ||
17 | #define arm1_length_checksum CHECKSUM("arm1_length") | |
18 | #define arm2_length_checksum CHECKSUM("arm2_length") | |
19 | #define morgan_offset_x_checksum CHECKSUM("morgan_offset_x") | |
20 | #define morgan_offset_y_checksum CHECKSUM("morgan_offset_y") | |
21 | #define axis_scaling_x_checksum CHECKSUM("axis_scaling_x") | |
22 | #define axis_scaling_y_checksum CHECKSUM("axis_scaling_y") | |
23 | ||
24 | #define SQ(x) powf(x, 2) | |
25 | #define ROUND(x, y) (roundf(x * 1e ## y) / 1e ## y) | |
26 | ||
27 | MorganSCARASolution::MorganSCARASolution(Config* config) | |
28 | { | |
29 | // arm1_length is the length of the inner main arm from hinge to hinge | |
30 | arm1_length = config->value(arm1_length_checksum)->by_default(150.0f)->as_number(); | |
31 | // arm2_length is the length of the inner main arm from hinge to hinge | |
32 | arm2_length = config->value(arm2_length_checksum)->by_default(150.0f)->as_number(); | |
33 | // morgan_offset_x is the x offset of bed zero position towards the SCARA tower center | |
34 | morgan_offset_x = config->value(morgan_offset_x_checksum)->by_default(100.0f)->as_number(); | |
35 | // morgan_offset_y is the y offset of bed zero position towards the SCARA tower center | |
36 | morgan_offset_y = config->value(morgan_offset_y_checksum)->by_default(-65.0f)->as_number(); | |
37 | ||
38 | init(); | |
39 | } | |
40 | ||
41 | void MorganSCARASolution::init() { | |
42 | ||
43 | } | |
44 | ||
45 | float MorganSCARASolution::to_degrees(float radians) { | |
46 | return radians*(180.0F/3.14159265359f); | |
47 | } | |
48 | ||
49 | void MorganSCARASolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] ) | |
50 | { | |
51 | ||
52 | float SCARA_pos[2], | |
53 | SCARA_C2, | |
54 | SCARA_S2, | |
55 | SCARA_K1, | |
56 | SCARA_K2, | |
57 | SCARA_theta, | |
58 | SCARA_psi; | |
59 | ||
60 | SCARA_pos[X_AXIS] = cartesian_mm[X_AXIS] - this->morgan_offset_x; //Translate cartesian to tower centric SCARA X Y | |
61 | SCARA_pos[Y_AXIS] = cartesian_mm[Y_AXIS] - this->morgan_offset_y; // morgan_offset not to be confused with home offset. Makes the SCARA math work. | |
62 | ||
63 | if (this->arm1_length == this->arm2_length) | |
64 | SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-2.0f*SQ(this->arm1_length)) / (2.0f * SQ(this->arm1_length)); | |
65 | else | |
66 | SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-SQ(this->arm1_length)-SQ(this->arm2_length)) / (2.0f * SQ(this->arm1_length)); | |
67 | ||
68 | SCARA_S2 = sqrtf(1.0f-SQ(SCARA_C2)); | |
69 | ||
70 | SCARA_K1 = this->arm1_length+this->arm2_length*SCARA_C2; | |
71 | SCARA_K2 = this->arm2_length*SCARA_S2; | |
72 | ||
73 | SCARA_theta = (atan2f(SCARA_pos[X_AXIS],SCARA_pos[Y_AXIS])-atan2f(SCARA_K1, SCARA_K2))*-1.0f; // Morgan Thomas turns Theta in oposite direction | |
74 | SCARA_psi = atan2f(SCARA_S2,SCARA_C2); | |
75 | ||
76 | ||
77 | actuator_mm[ALPHA_STEPPER] = to_degrees(SCARA_theta); // Multiply by 180/Pi - theta is support arm angle | |
78 | actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_theta + SCARA_psi); // Morgan kinematics (dual arm) | |
79 | //actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_psi); // real scara | |
80 | actuator_mm[GAMMA_STEPPER] = cartesian_mm[Z_AXIS]; // No inverse kinematics on Z - Position to add bed offset? | |
81 | ||
82 | } | |
83 | ||
84 | void MorganSCARASolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] ) { | |
85 | // Perform forward kinematics, and place results in cartesian_mm[] | |
86 | ||
87 | float y1, y2, | |
88 | actuator_rad[2]; | |
89 | ||
90 | actuator_rad[X_AXIS] = actuator_mm[X_AXIS]/(180.0F/3.14159265359f); | |
91 | actuator_rad[Y_AXIS] = actuator_mm[Y_AXIS]/(180.0F/3.14159265359f); | |
92 | ||
93 | y1 = sinf(actuator_rad[X_AXIS])*this->arm1_length; | |
94 | y2 = sinf(actuator_rad[Y_AXIS])*this->arm2_length + y1; | |
95 | ||
96 | cartesian_mm[X_AXIS] = cosf(actuator_rad[X_AXIS])*this->arm1_length + cosf(actuator_rad[Y_AXIS])*this->arm2_length + this->morgan_offset_x; | |
97 | cartesian_mm[Y_AXIS] = y2 + this->morgan_offset_y; | |
98 | cartesian_mm[Z_AXIS] = actuator_mm[Z_AXIS]; | |
99 | ||
100 | cartesian_mm[0] = ROUND(cartesian_mm[0], 4); | |
101 | cartesian_mm[1] = ROUND(cartesian_mm[1], 4); | |
102 | cartesian_mm[2] = ROUND(cartesian_mm[2], 4); | |
103 | } | |
104 | ||
105 | bool MorganSCARASolution::set_optional(const arm_options_t& options) { | |
106 | ||
107 | arm_options_t::const_iterator i; | |
108 | ||
109 | i= options.find('T'); // Theta arm1 length | |
110 | if(i != options.end()) { | |
111 | arm1_length= i->second; | |
112 | ||
113 | } | |
114 | i= options.find('P'); // Psi arm2 length | |
115 | if(i != options.end()) { | |
116 | arm2_length= i->second; | |
117 | } | |
118 | i= options.find('X'); // Home initial position X | |
119 | if(i != options.end()) { | |
120 | morgan_offset_x= i->second; | |
121 | } | |
122 | i= options.find('Y'); // Home initial position Y | |
123 | if(i != options.end()) { | |
124 | morgan_offset_y= i->second; | |
125 | } | |
126 | ||
127 | init(); | |
128 | return true; | |
129 | } | |
130 | ||
131 | bool MorganSCARASolution::get_optional(arm_options_t& options) { | |
132 | options['T']= this->arm1_length; | |
133 | options['P']= this->arm2_length; | |
134 | options['X']= this->morgan_offset_x; | |
135 | options['Y']= this->morgan_offset_y; | |
136 | return true; | |
137 | }; |