From d65f5d816f85f5f4cfb286ea5e874bbc0450173b Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Mon, 12 Sep 2016 03:48:29 -0500 Subject: [PATCH] Patch to fix kinematics --- Marlin/Marlin.h | 24 ++++----- Marlin/Marlin_main.cpp | 107 +++++++++++++++++++++++------------------ 2 files changed, 73 insertions(+), 58 deletions(-) diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h index fbea4dc8b..536a8cf79 100644 --- a/Marlin/Marlin.h +++ b/Marlin/Marlin.h @@ -302,22 +302,24 @@ int code_value_int(); float code_value_temp_abs(); float code_value_temp_diff(); -#if ENABLED(DELTA) +#if IS_KINEMATIC extern float delta[ABC]; - extern float endstop_adj[ABC]; // axis[n].endstop_adj - extern float delta_radius; - extern float delta_diagonal_rod; - extern float delta_segments_per_second; - 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 inverse_kinematics(const float cartesian[XYZ]); +#endif + +#if ENABLED(DELTA) + extern float delta[ABC], + endstop_adj[ABC], + delta_radius, + delta_diagonal_rod, + delta_segments_per_second, + delta_diagonal_rod_trim_tower_1, + delta_diagonal_rod_trim_tower_2, + delta_diagonal_rod_trim_tower_3; void recalc_delta_settings(float radius, float diagonal_rod); #elif IS_SCARA - extern float delta[ABC]; extern float axis_scaling[ABC]; // Build size scaling - void inverse_kinematics(const float cartesian[XYZ]); - void forward_kinematics_SCARA(float f_scara[ABC]); + void forward_kinematics_SCARA(const float &a, const float &b); #endif #if ENABLED(AUTO_BED_LEVELING_NONLINEAR) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index f0ab000e0..3e83ea606 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -465,7 +465,6 @@ static uint8_t target_extruder; #define COS_60 0.5 float delta[ABC], - cartes[XYZ] = { 0 }, endstop_adj[ABC] = { 0 }; // these are the default values, can be overriden with M665 @@ -487,7 +486,6 @@ static uint8_t target_extruder; delta_clip_start_height = Z_MAX_POS; float delta_safe_distance_from_top(); - void get_cartesian_from_steppers(); #else @@ -508,11 +506,11 @@ static uint8_t target_extruder; float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND, delta[ABC], - axis_scaling[ABC] = { 1, 1, 1 }, // Build size scaling, default to 1 - cartes[XYZ] = { 0 }; - void get_cartesian_from_steppers() { } // to be written later + axis_scaling[ABC] = { 1, 1, 1 }; // Build size scaling, default to 1 #endif +float cartes[XYZ] = { 0 }; + #if ENABLED(FILAMENT_WIDTH_SENSOR) bool filament_sensor = false; //M405 turns on filament_sensor control, M406 turns it off float filament_width_nominal = DEFAULT_NOMINAL_FILAMENT_DIA, // Nominal filament width. Change with M404 @@ -598,6 +596,8 @@ void stop(); void get_available_commands(); void process_next_command(); void prepare_move_to_destination(); + +void get_cartesian_from_steppers(); void set_current_from_steppers_for_axis(AxisEnum axis); #if ENABLED(ARC_SUPPORT) @@ -1347,7 +1347,7 @@ static void set_axis_is_at_home(AxisEnum axis) { } #endif - #if ENABLED(SCARA) + #if ENABLED(MORGAN_SCARA) if (axis == X_AXIS || axis == Y_AXIS) { @@ -1362,19 +1362,19 @@ static void set_axis_is_at_home(AxisEnum axis) { * and calculates homing offset using forward kinematics */ inverse_kinematics(homeposition); - forward_kinematics_SCARA(delta); + forward_kinematics_SCARA(delta[A_AXIS], delta[B_AXIS]); - // SERIAL_ECHOPAIR("Delta X=", delta[X_AXIS]); - // SERIAL_ECHOPGM(" Delta Y="); SERIAL_ECHOLN(delta[Y_AXIS]); + // SERIAL_ECHOPAIR("Delta X=", cartes[X_AXIS]); + // SERIAL_ECHOPGM(" Delta Y="); SERIAL_ECHOLN(cartes[Y_AXIS]); - current_position[axis] = LOGICAL_POSITION(delta[axis], axis); + current_position[axis] = LOGICAL_POSITION(cartes[axis], axis); /** * SCARA home positions are based on configuration since the actual * limits are determined by the inverse kinematic transform. */ - soft_endstop_min[axis] = base_min_pos(axis); // + (delta[axis] - base_home_pos(axis)); - soft_endstop_max[axis] = base_max_pos(axis); // + (delta[axis] - base_home_pos(axis)); + soft_endstop_min[axis] = base_min_pos(axis); // + (cartes[axis] - base_home_pos(axis)); + soft_endstop_max[axis] = base_max_pos(axis); // + (cartes[axis] - base_home_pos(axis)); } else #endif @@ -5089,7 +5089,7 @@ static void report_current_position() { stepper.report_positions(); - #if ENABLED(SCARA) + #if IS_SCARA SERIAL_PROTOCOLPGM("SCARA Theta:"); SERIAL_PROTOCOL(delta[X_AXIS]); SERIAL_PROTOCOLPGM(" Psi+Theta:"); @@ -5327,7 +5327,7 @@ inline void gcode_M206() { if (code_seen(axis_codes[i])) set_home_offset((AxisEnum)i, code_value_axis_units(i)); - #if ENABLED(SCARA) + #if IS_SCARA if (code_seen('T')) set_home_offset(X_AXIS, code_value_axis_units(X_AXIS)); // Theta if (code_seen('P')) set_home_offset(Y_AXIS, code_value_axis_units(Y_AXIS)); // Psi #endif @@ -5808,17 +5808,16 @@ inline void gcode_M303() { #endif } -#if ENABLED(SCARA) - bool SCARA_move_to_cal(uint8_t delta_x, uint8_t delta_y) { +#if ENABLED(MORGAN_SCARA) + bool SCARA_move_to_cal(uint8_t delta_a, uint8_t delta_b) { //SoftEndsEnabled = false; // Ignore soft endstops during calibration //SERIAL_ECHOLNPGM(" Soft endstops disabled"); if (IsRunning()) { //gcode_get_destination(); // For X Y Z E F - delta[X_AXIS] = delta_x; - delta[Y_AXIS] = delta_y; - forward_kinematics_SCARA(delta); - destination[X_AXIS] = delta[X_AXIS] / axis_scaling[X_AXIS]; - destination[Y_AXIS] = delta[Y_AXIS] / axis_scaling[Y_AXIS]; + forward_kinematics_SCARA(delta_a, delta_b); + destination[X_AXIS] = cartes[X_AXIS] / axis_scaling[X_AXIS]; + destination[Y_AXIS] = cartes[Y_AXIS] / axis_scaling[Y_AXIS]; + destination[Z_AXIS] = current_position[Z_AXIS]; prepare_move_to_destination(); //ok_to_send(); return true; @@ -7456,7 +7455,7 @@ void process_next_command() { gcode_M303(); break; - #if ENABLED(SCARA) + #if ENABLED(MORGAN_SCARA) case 360: // M360 SCARA Theta pos1 if (gcode_M360()) return; break; @@ -7794,12 +7793,6 @@ void ok_to_send() { forward_kinematics_DELTA(point[A_AXIS], point[B_AXIS], point[C_AXIS]); } - void get_cartesian_from_steppers() { - forward_kinematics_DELTA(stepper.get_axis_position_mm(A_AXIS), - stepper.get_axis_position_mm(B_AXIS), - stepper.get_axis_position_mm(C_AXIS)); - } - #if ENABLED(AUTO_BED_LEVELING_NONLINEAR) // Adjust print surface height by linear interpolation over the bed_level array. @@ -8274,32 +8267,32 @@ void prepare_move_to_destination() { #endif // HAS_CONTROLLERFAN -#if ENABLED(SCARA) +#if IS_SCARA - void forward_kinematics_SCARA(float f_scara[ABC]) { - // Perform forward kinematics, and place results in delta[] + void forward_kinematics_SCARA(const float &a, const float &b) { + // Perform forward kinematics, and place results in cartes[] // The maths and first version has been done by QHARLEY . Integrated into masterbranch 06/2014 and slightly restructured by Joachim Cerny in June 2014 - float x_sin, x_cos, y_sin, y_cos; + float a_sin, a_cos, b_sin, b_cos; - //SERIAL_ECHOPGM("f_delta x="); SERIAL_ECHO(f_scara[X_AXIS]); - //SERIAL_ECHOPGM(" y="); SERIAL_ECHO(f_scara[Y_AXIS]); + //SERIAL_ECHOPGM("f_delta x="); SERIAL_ECHO(a); + //SERIAL_ECHOPGM(" y="); SERIAL_ECHO(b); - x_sin = sin(RADIANS(f_scara[X_AXIS])) * L1; - x_cos = cos(RADIANS(f_scara[X_AXIS])) * L1; - y_sin = sin(RADIANS(f_scara[Y_AXIS])) * L2; - y_cos = cos(RADIANS(f_scara[Y_AXIS])) * L2; + a_sin = sin(RADIANS(a)) * L1; + a_cos = cos(RADIANS(a)) * L1; + b_sin = sin(RADIANS(b)) * L2; + b_cos = cos(RADIANS(b)) * L2; - //SERIAL_ECHOPGM(" x_sin="); SERIAL_ECHO(x_sin); - //SERIAL_ECHOPGM(" x_cos="); SERIAL_ECHO(x_cos); - //SERIAL_ECHOPGM(" y_sin="); SERIAL_ECHO(y_sin); - //SERIAL_ECHOPGM(" y_cos="); SERIAL_ECHOLN(y_cos); + //SERIAL_ECHOPGM(" a_sin="); SERIAL_ECHO(a_sin); + //SERIAL_ECHOPGM(" a_cos="); SERIAL_ECHO(a_cos); + //SERIAL_ECHOPGM(" b_sin="); SERIAL_ECHO(b_sin); + //SERIAL_ECHOPGM(" b_cos="); SERIAL_ECHOLN(b_cos); - delta[X_AXIS] = x_cos + y_cos + SCARA_OFFSET_X; //theta - delta[Y_AXIS] = x_sin + y_sin + SCARA_OFFSET_Y; //theta+phi + cartes[X_AXIS] = a_cos + b_cos + SCARA_OFFSET_X; //theta + cartes[Y_AXIS] = a_sin + b_sin + SCARA_OFFSET_Y; //theta+phi - //SERIAL_ECHOPGM(" delta[X_AXIS]="); SERIAL_ECHO(delta[X_AXIS]); - //SERIAL_ECHOPGM(" delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]); + //SERIAL_ECHOPGM(" cartes[X_AXIS]="); SERIAL_ECHO(cartes[X_AXIS]); + //SERIAL_ECHOPGM(" cartes[Y_AXIS]="); SERIAL_ECHOLN(cartes[Y_AXIS]); } void inverse_kinematics(const float cartesian[XYZ]) { @@ -8343,7 +8336,27 @@ void prepare_move_to_destination() { //*/ } -#endif // MORGAN_SCARA +#endif // IS_SCARA + +void get_cartesian_from_steppers() { + #if ENABLED(DELTA) + forward_kinematics_DELTA( + stepper.get_axis_position_mm(A_AXIS), + stepper.get_axis_position_mm(B_AXIS), + stepper.get_axis_position_mm(C_AXIS) + ); + #elif IS_SCARA + forward_kinematics_SCARA( + stepper.get_axis_position_degrees(A_AXIS), + stepper.get_axis_position_degrees(B_AXIS) + ); + cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS); + #else + cartes[X_AXIS] = stepper.get_axis_position_mm(X_AXIS); + cartes[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS); + cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS); + #endif +} #if ENABLED(TEMP_STAT_LEDS)