diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h index b1e656b67..7ed7d37cb 100644 --- a/Marlin/Marlin.h +++ b/Marlin/Marlin.h @@ -315,7 +315,7 @@ float code_value_temp_diff(); extern float delta_diagonal_rod_trim_tower_1; extern float delta_diagonal_rod_trim_tower_2; extern float delta_diagonal_rod_trim_tower_3; - void calculate_delta(float cartesian[3]); + void inverse_kinematics(float cartesian[3]); void recalc_delta_settings(float radius, float diagonal_rod); #if ENABLED(AUTO_BED_LEVELING_FEATURE) extern int delta_grid_spacing[2]; @@ -323,8 +323,8 @@ float code_value_temp_diff(); #endif #elif ENABLED(SCARA) extern float axis_scaling[3]; // Build size scaling - void calculate_delta(float cartesian[3]); - void calculate_SCARA_forward_Transform(float f_scara[3]); + void inverse_kinematics(float cartesian[3]); + void forward_kinematics_SCARA(float f_scara[3]); #endif #if ENABLED(Z_DUAL_ENDSTOPS) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index e2e62a9ed..faef1820f 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -613,7 +613,7 @@ static void report_current_position(); #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_delta", current_position); #endif - calculate_delta(current_position); + inverse_kinematics(current_position); planner.set_position_mm(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]); } #define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position_delta() @@ -1528,7 +1528,7 @@ static void set_axis_is_at_home(AxisEnum axis) { * Works out real Homeposition angles using inverse kinematics, * and calculates homing offset using forward kinematics */ - calculate_delta(homeposition); + inverse_kinematics(homeposition); // SERIAL_ECHOPGM("base Theta= "); SERIAL_ECHO(delta[X_AXIS]); // SERIAL_ECHOPGM(" base Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]); @@ -1540,7 +1540,7 @@ static void set_axis_is_at_home(AxisEnum axis) { // SERIAL_ECHOPGM(" addhome Theta="); SERIAL_ECHO(delta[X_AXIS]); // SERIAL_ECHOPGM(" addhome Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]); - calculate_SCARA_forward_Transform(delta); + forward_kinematics_SCARA(delta); // SERIAL_ECHOPGM("Delta X="); SERIAL_ECHO(delta[X_AXIS]); // SERIAL_ECHOPGM(" Delta Y="); SERIAL_ECHOLN(delta[Y_AXIS]); @@ -1658,7 +1658,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position, if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_move_to_destination_raw", destination); #endif refresh_cmd_timeout(); - calculate_delta(destination); + inverse_kinematics(destination); planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], MMM_TO_MMS_SCALED(feedrate_mm_m), active_extruder); set_current_to_destination(); } @@ -5886,7 +5886,7 @@ inline void gcode_M303() { //gcode_get_destination(); // For X Y Z E F delta[X_AXIS] = delta_x; delta[Y_AXIS] = delta_y; - calculate_SCARA_forward_Transform(delta); + forward_kinematics_SCARA(delta); destination[X_AXIS] = delta[X_AXIS] / axis_scaling[X_AXIS]; destination[Y_AXIS] = delta[Y_AXIS] / axis_scaling[Y_AXIS]; prepare_move_to_destination(); @@ -6275,7 +6275,7 @@ inline void gcode_M503() { // Define runplan for move axes #if ENABLED(DELTA) - #define RUNPLAN(RATE_MM_S) calculate_delta(destination); \ + #define RUNPLAN(RATE_MM_S) inverse_kinematics(destination); \ planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], RATE_MM_S, active_extruder); #else #define RUNPLAN(RATE_MM_S) line_to_destination(MMS_TO_MMM(RATE_MM_S)); @@ -6397,7 +6397,7 @@ inline void gcode_M503() { #if ENABLED(DELTA) // Move XYZ to starting position, then E - calculate_delta(lastpos); + inverse_kinematics(lastpos); planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder); planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], lastpos[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder); #else @@ -7737,7 +7737,7 @@ void clamp_to_software_endstops(float target[3]) { delta_diagonal_rod_2_tower_3 = sq(diagonal_rod + delta_diagonal_rod_trim_tower_3); } - void calculate_delta(float cartesian[3]) { + void inverse_kinematics(float cartesian[3]) { delta[TOWER_1] = sqrt(delta_diagonal_rod_2_tower_1 - sq(delta_tower1_x - cartesian[X_AXIS]) @@ -7764,14 +7764,14 @@ void clamp_to_software_endstops(float target[3]) { float delta_safe_distance_from_top() { float cartesian[3] = { 0 }; - calculate_delta(cartesian); + inverse_kinematics(cartesian); float distance = delta[TOWER_3]; cartesian[Y_AXIS] = DELTA_PRINTABLE_RADIUS; - calculate_delta(cartesian); + inverse_kinematics(cartesian); return abs(distance - delta[TOWER_3]); } - void forwardKinematics(float z1, float z2, float z3) { + void forward_kinematics_DELTA(float z1, float z2, float z3) { //As discussed in Wikipedia "Trilateration" //we are establishing a new coordinate //system in the plane of the three carriage points. @@ -7840,12 +7840,12 @@ void clamp_to_software_endstops(float target[3]) { cartesian_position[Z_AXIS] = z1 + ex[2]*Xnew + ey[2]*Ynew - ez[2]*Znew; }; - void forwardKinematics(float point[3]) { - forwardKinematics(point[X_AXIS], point[Y_AXIS], point[Z_AXIS]); + void forward_kinematics_DELTA(float point[3]) { + forward_kinematics_DELTA(point[X_AXIS], point[Y_AXIS], point[Z_AXIS]); } void set_cartesian_from_steppers() { - forwardKinematics(stepper.get_axis_position_mm(X_AXIS), + forward_kinematics_DELTA(stepper.get_axis_position_mm(X_AXIS), stepper.get_axis_position_mm(Y_AXIS), stepper.get_axis_position_mm(Z_AXIS)); } @@ -7973,7 +7973,7 @@ void mesh_line_to_destination(float fr_mm_m, uint8_t x_splits = 0xff, uint8_t y_ #if ENABLED(DELTA) || ENABLED(SCARA) - inline bool prepare_delta_move_to(float target[NUM_AXIS]) { + inline bool prepare_kinematic_move_to(float target[NUM_AXIS]) { float difference[NUM_AXIS]; for (int8_t i = 0; i < NUM_AXIS; i++) difference[i] = target[i] - current_position[i]; @@ -7996,14 +7996,14 @@ void mesh_line_to_destination(float fr_mm_m, uint8_t x_splits = 0xff, uint8_t y_ for (int8_t i = 0; i < NUM_AXIS; i++) target[i] = current_position[i] + difference[i] * fraction; - calculate_delta(target); + inverse_kinematics(target); #if ENABLED(AUTO_BED_LEVELING_FEATURE) if (!bed_leveling_in_progress) adjust_delta(target); #endif - //DEBUG_POS("prepare_delta_move_to", target); - //DEBUG_POS("prepare_delta_move_to", delta); + //DEBUG_POS("prepare_kinematic_move_to", target); + //DEBUG_POS("prepare_kinematic_move_to", delta); planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], _feedrate_mm_s, active_extruder); } @@ -8012,10 +8012,6 @@ void mesh_line_to_destination(float fr_mm_m, uint8_t x_splits = 0xff, uint8_t y_ #endif // DELTA || SCARA -#if ENABLED(SCARA) - inline bool prepare_scara_move_to(float target[NUM_AXIS]) { return prepare_delta_move_to(target); } -#endif - #if ENABLED(DUAL_X_CARRIAGE) inline bool prepare_move_to_destination_dualx() { @@ -8114,10 +8110,8 @@ void prepare_move_to_destination() { prevent_dangerous_extrude(current_position[E_AXIS], destination[E_AXIS]); #endif - #if ENABLED(SCARA) - if (!prepare_scara_move_to(destination)) return; - #elif ENABLED(DELTA) - if (!prepare_delta_move_to(destination)) return; + #if ENABLED(DELTA) || ENABLED(SCARA) + if (!prepare_kinematic_move_to(destination)) return; #else #if ENABLED(DUAL_X_CARRIAGE) if (!prepare_move_to_destination_dualx()) return; @@ -8253,7 +8247,7 @@ void prepare_move_to_destination() { clamp_to_software_endstops(arc_target); #if ENABLED(DELTA) || ENABLED(SCARA) - calculate_delta(arc_target); + inverse_kinematics(arc_target); #if ENABLED(AUTO_BED_LEVELING_FEATURE) adjust_delta(arc_target); #endif @@ -8265,7 +8259,7 @@ void prepare_move_to_destination() { // Ensure last segment arrives at target location. #if ENABLED(DELTA) || ENABLED(SCARA) - calculate_delta(target); + inverse_kinematics(target); #if ENABLED(AUTO_BED_LEVELING_FEATURE) adjust_delta(target); #endif @@ -8333,7 +8327,7 @@ void prepare_move_to_destination() { #if ENABLED(SCARA) - void calculate_SCARA_forward_Transform(float f_scara[3]) { + void forward_kinematics_SCARA(float f_scara[3]) { // Perform forward kinematics, and place results in delta[3] // The maths and first version has been done by QHARLEY . Integrated into masterbranch 06/2014 and slightly restructured by Joachim Cerny in June 2014 @@ -8359,10 +8353,11 @@ void prepare_move_to_destination() { //SERIAL_ECHOPGM(" delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]); } - void calculate_delta(float cartesian[3]) { - //reverse kinematics. - // Perform reversed kinematics, and place results in delta[3] - // The maths and first version has been done by QHARLEY . Integrated into masterbranch 06/2014 and slightly restructured by Joachim Cerny in June 2014 + void inverse_kinematics(float cartesian[3]) { + // Inverse kinematics. + // Perform SCARA IK and place results in delta[3]. + // The maths and first version were done by QHARLEY. + // Integrated, tweaked by Joachim Cerny in June 2014. float SCARA_pos[2]; static float SCARA_C2, SCARA_S2, SCARA_K1, SCARA_K2, SCARA_theta, SCARA_psi; diff --git a/Marlin/planner_bezier.cpp b/Marlin/planner_bezier.cpp index ad3319a49..9ad57fbc0 100644 --- a/Marlin/planner_bezier.cpp +++ b/Marlin/planner_bezier.cpp @@ -189,7 +189,7 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS] clamp_to_software_endstops(bez_target); #if ENABLED(DELTA) || ENABLED(SCARA) - calculate_delta(bez_target); + inverse_kinematics(bez_target); #if ENABLED(AUTO_BED_LEVELING_FEATURE) adjust_delta(bez_target); #endif diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp index 7264f7b40..17d41cd3d 100755 --- a/Marlin/ultralcd.cpp +++ b/Marlin/ultralcd.cpp @@ -564,7 +564,7 @@ void kill_screen(const char* lcd_msg) { inline void line_to_current(AxisEnum axis) { #if ENABLED(DELTA) - calculate_delta(current_position); + inverse_kinematics(current_position); planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[axis]), active_extruder); #else // !DELTA planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[axis]), active_extruder); @@ -1301,7 +1301,7 @@ void kill_screen(const char* lcd_msg) { inline void manage_manual_move() { if (manual_move_axis != (int8_t)NO_AXIS && ELAPSED(millis(), manual_move_start_time) && !planner.is_full()) { #if ENABLED(DELTA) - calculate_delta(current_position); + inverse_kinematics(current_position); planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_e_index); #else planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_e_index);