diff --git a/Grbl_Esp32/Custom/CoreXY.cpp b/Grbl_Esp32/Custom/CoreXY.cpp index 0acad5b4..39488722 100644 --- a/Grbl_Esp32/Custom/CoreXY.cpp +++ b/Grbl_Esp32/Custom/CoreXY.cpp @@ -43,7 +43,6 @@ const float geometry_factor = 1.0; const float geometry_factor = 2.0; #endif -static float last_motors[MAX_N_AXIS] = { 0.0 }; // A place to save the previous motor angles for distance/feed rate calcs static float last_cartesian[MAX_N_AXIS] = {}; // prototypes for helper functions @@ -59,6 +58,19 @@ void machine_init() { #endif } +// Converts Cartesian to motors with no motion control +static void cartesian_to_motors(float* position) { + float motors[MAX_N_AXIS]; + + motors[X_AXIS] = geometry_factor * position[X_AXIS] + position[Y_AXIS]; + motors[Y_AXIS] = geometry_factor * position[X_AXIS] - position[Y_AXIS]; + + position[X_AXIS] = motors[X_AXIS]; + position[Y_AXIS] = motors[Y_AXIS]; + + // Z and higher just pass through unchanged +} + // Cycle mask is 0 unless the user sends a single axis command like $HZ // This will always return true to prevent the normal Grbl homing cycle bool user_defined_homing(uint8_t cycle_mask) { @@ -131,11 +143,11 @@ bool user_defined_homing(uint8_t cycle_mask) { } for (int axis = Z_AXIS; axis < n_axis; axis++) { - target[axis] = system_convert_axis_steps_to_mpos(sys_position, axis); + target[axis] = sys_position[axis] / axis_settings[axis]->steps_per_mm->get(); } // convert back to motor steps - inverse_kinematics(target); + cartesian_to_motors(target); pl_data->feed_rate = homing_rate; // feed or seek rates plan_buffer_line(target, pl_data); // Bypass mc_line(). Directly plan homing motion. @@ -222,11 +234,11 @@ bool user_defined_homing(uint8_t cycle_mask) { last_cartesian[Y_AXIS] = target[Y_AXIS]; for (int axis = Z_AXIS; axis < n_axis; axis++) { - last_cartesian[axis] = system_convert_axis_steps_to_mpos(sys_position, axis); + last_cartesian[axis] = sys_position[axis] / axis_settings[axis]->steps_per_mm->get(); } // convert to motors - inverse_kinematics(target); + cartesian_to_motors(target); // convert to steps for (axis = X_AXIS; axis <= Y_AXIS; axis++) { sys_position[axis] = target[axis] * axis_settings[axis]->steps_per_mm->get(); @@ -242,29 +254,21 @@ bool user_defined_homing(uint8_t cycle_mask) { return true; } -// This function is used by Grbl convert Cartesian to motors -// this does not do any motion control -void inverse_kinematics(float* position) { - float motors[MAX_N_AXIS]; +static void transform_cartesian_to_motors(float* motors, float* cartesian) { + motors[X_AXIS] = geometry_factor * cartesian[X_AXIS] + cartesian[Y_AXIS]; + motors[Y_AXIS] = geometry_factor * cartesian[X_AXIS] - cartesian[Y_AXIS]; - motors[X_AXIS] = geometry_factor * position[X_AXIS] + position[Y_AXIS]; - motors[Y_AXIS] = geometry_factor * position[X_AXIS] - position[Y_AXIS]; - - position[X_AXIS] = motors[X_AXIS]; - position[Y_AXIS] = motors[Y_AXIS]; - - // Z and higher just pass through unchanged + auto n_axis = number_axis->get(); + for (uint8_t axis = Z_AXIS; axis <= n_axis; axis++) { + motors[axis] = cartesian[axis]; + } } // Inverse Kinematics calculates motor positions from real world cartesian positions -// position is the current position +// position is the old machine position, target the new machine position // Breaking into segments is not needed with CoreXY, because it is a linear system. -void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) //The target and position are provided in MPos -{ +bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { float dx, dy, dz; // distances in each cartesian axis - float motors[MAX_N_AXIS]; - - float feed_rate = pl_data->feed_rate; // save original feed rate // calculate cartesian move distance for each axis dx = target[X_AXIS] - position[X_AXIS]; @@ -272,56 +276,30 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio dz = target[Z_AXIS] - position[Z_AXIS]; float dist = sqrt((dx * dx) + (dy * dy) + (dz * dz)); - motors[X_AXIS] = geometry_factor * target[X_AXIS] + target[Y_AXIS]; - motors[Y_AXIS] = geometry_factor * target[X_AXIS] - target[Y_AXIS]; - auto n_axis = number_axis->get(); - for (uint8_t axis = Z_AXIS; axis <= n_axis; axis++) { - motors[axis] = target[axis]; - } - float motor_distance = three_axis_dist(motors, last_motors); + float motors[n_axis]; + transform_cartesian_to_motors(motors, target); if (!pl_data->motion.rapidMotion) { - pl_data->feed_rate *= (motor_distance / dist); + float last_motors[n_axis]; + transform_cartesian_to_motors(last_motors, position); + pl_data->feed_rate *= (three_axis_dist(motors, last_motors) / dist); } - memcpy(last_motors, motors, sizeof(motors)); - - mc_line(motors, pl_data); + return mc_line(motors, pl_data); } // motors -> cartesian -void forward_kinematics(float* position) { - float calc_fwd[MAX_N_AXIS]; - float wco[MAX_N_AXIS]; - float print_position[N_AXIS]; - int32_t current_position[N_AXIS]; // Copy current state of the system position variable - - memcpy(current_position, sys_position, sizeof(sys_position)); - system_convert_array_steps_to_mpos(print_position, current_position); - - // determine the Work Coordinate Offsets for each axis - auto n_axis = number_axis->get(); - for (int axis = 0; axis < n_axis; axis++) { - // Apply work coordinate offsets and tool length offset to current position. - wco[axis] = gc_state.coord_system[axis] + gc_state.coord_offset[axis]; - if (axis == TOOL_LENGTH_OFFSET_AXIS) { - wco[axis] += gc_state.tool_length_offset; - } - } - +void motors_to_cartesian(float* cartesian, float* motors, int n_axis) { // apply the forward kinemetics to the machine coordinates // https://corexy.com/theory.html //calc_fwd[X_AXIS] = 0.5 / geometry_factor * (position[X_AXIS] + position[Y_AXIS]); - calc_fwd[X_AXIS] = ((0.5 * (print_position[X_AXIS] + print_position[Y_AXIS]) / geometry_factor) - wco[X_AXIS]); - calc_fwd[Y_AXIS] = ((0.5 * (print_position[X_AXIS] - print_position[Y_AXIS])) - wco[Y_AXIS]); + cartesian[X_AXIS] = 0.5 * (motors[X_AXIS] + motors[Y_AXIS]) / geometry_factor; + cartesian[Y_AXIS] = 0.5 * (motors[X_AXIS] - motors[Y_AXIS]); - for (int axis = 0; axis < n_axis; axis++) { - if (axis > Y_AXIS) { // for axes above Y there is no kinematics - calc_fwd[axis] = print_position[axis] - wco[axis]; - } - position[axis] = calc_fwd[axis]; // this is the value returned to reporting + for (int axis = Z_AXIS; axis < n_axis; axis++) { + cartesian[axis] = motors[axis]; } } @@ -331,14 +309,12 @@ bool kinematics_pre_homing(uint8_t cycle_mask) { void kinematics_post_homing() { auto n_axis = number_axis->get(); - for (uint8_t axis = X_AXIS; axis <= n_axis; axis++) { - gc_state.position[axis] = last_cartesian[axis]; - } + memcpy(gc_state.position, last_cartesian, n_axis * sizeof(last_cartesian[0])); } -// this is used used by Grbl soft limits to see if the range of the machine is exceeded. -uint8_t kinematic_limits_check(float* target) { - return true; +// this is used used by Limits.cpp to see if the range of the machine is exceeded. +bool limitsCheckTravel(float* target) { + return false; } void user_m30() {} diff --git a/Grbl_Esp32/Custom/atari_1020.cpp b/Grbl_Esp32/Custom/atari_1020.cpp index 27a55021..8f27b4da 100644 --- a/Grbl_Esp32/Custom/atari_1020.cpp +++ b/Grbl_Esp32/Custom/atari_1020.cpp @@ -75,15 +75,12 @@ void machine_init() { // this task tracks the Z position and sets the solenoid void solenoidSyncTask(void* pvParameters) { int32_t current_position[N_AXIS]; // copy of current location - float m_pos[N_AXIS]; // machine position in mm TickType_t xLastWakeTime; const TickType_t xSolenoidFrequency = SOLENOID_TASK_FREQ; // in ticks (typically ms) xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. while (true) { // don't ever return from this or the task dies - memcpy(current_position, sys_position, sizeof(sys_position)); // get current position in step - system_convert_array_steps_to_mpos(m_pos, current_position); // convert to millimeters - calc_solenoid(m_pos[Z_AXIS]); // calculate kinematics and move the servos + calc_solenoid(system_get_mpos()[Z_AXIS]); // calculate kinematics and move the servos vTaskDelayUntil(&xLastWakeTime, xSolenoidFrequency); } } diff --git a/Grbl_Esp32/Custom/custom_code_template.cpp b/Grbl_Esp32/Custom/custom_code_template.cpp index bd468f6d..54d5f6ec 100644 --- a/Grbl_Esp32/Custom/custom_code_template.cpp +++ b/Grbl_Esp32/Custom/custom_code_template.cpp @@ -24,37 +24,39 @@ ======================================================================= This is a template for user-defined C++ code functions. Grbl can be -configured to call some optional functions, enabled by #define statements -in the machine definition .h file. Implement the functions thus enabled -herein. The possible functions are listed and described below. +configured to call some optional functions. These functions have weak definitions +in the main code. If you create your own version they will be used instead -To use this file, copy it to a name of your own choosing, and also copy -Machines/template.h to a similar name. +Put all of your functions in a .cpp file in the "Custom" folder. +Add this to your machine definition file +#define CUSTOM_CODE_FILENAME "../Custom/YourFile.cpp" -Example: -Machines/my_machine.h -Custom/my_machine.cpp - -Edit machine.h to include your Machines/my_machine.h file - -Edit Machines/my_machine.h according to the instructions therein. - -Fill in the function definitions below for the functions that you have -enabled with USE_ defines in Machines/my_machine.h +Be careful to return the correct values =============================================================================== +Below are all the current weak function + */ -#ifdef USE_MACHINE_INIT /* -machine_init() is called when Grbl_ESP32 first starts. You can use it to do any -special things your machine needs at startup. +This function is used as a one time setup for your machine. */ void machine_init() {} -#endif -#ifdef USE_CUSTOM_HOMING +/* +This is used to initialize a display. +*/ +void display_init() {} + +/* + limitsCheckTravel() is called to check soft limits + It returns true if the motion is outside the limit values +*/ +bool limitsCheckTravel() { + return false; +} + /* user_defined_homing(uint8_t cycle_mask) is called at the begining of the normal Grbl_ESP32 homing sequence. If user_defined_homing(uint8_t cycle_mask) returns false, the rest of normal Grbl_ESP32 @@ -66,17 +68,14 @@ bool user_defined_homing(uint8_t cycle_mask) { // True = done with homing, false = continue with normal Grbl_ESP32 homing return true; } -#endif -#ifdef USE_KINEMATICS /* Inverse Kinematics converts X,Y,Z cartesian coordinate to the steps on your "joint" motors. It requires the following three functions: */ /* - inverse_kinematics() looks at incoming move commands and modifies - them before Grbl_ESP32 puts them in the motion planner. + cartesian_to_motors() converts from cartesian coordinates to motor space. Grbl_ESP32 processes arcs by converting them into tiny little line segments. Kinematics in Grbl_ESP32 works the same way. Search for this function across @@ -86,9 +85,9 @@ bool user_defined_homing(uint8_t cycle_mask) { pl_data = planner data (see the definition of this type to see what it is) position = an N_AXIS array of where the machine is starting from for this move */ -void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) { +bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { // this simply moves to the target. Replace with your kinematics. - mc_line(target, pl_data); + return mc_line(target, pl_data); } /* @@ -97,8 +96,7 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio cycle_mask is a bit mask of the axes being homed this time. */ -bool kinematics_pre_homing(uint8_t cycle_mask)) -{ +bool kinematics_pre_homing(uint8_t cycle_mask) { return false; // finish normal homing cycle } @@ -106,51 +104,34 @@ bool kinematics_pre_homing(uint8_t cycle_mask)) kinematics_post_homing() is called at the end of normal homing */ void kinematics_post_homing() {} -#endif -#ifdef USE_FWD_KINEMATICS /* - The status command uses forward_kinematics() to convert + The status command uses motors_to_cartesian() to convert your motor positions to cartesian X,Y,Z... coordinates. Convert the N_AXIS array of motor positions to cartesian in your code. */ -void forward_kinematics(float* position) { +void motors_to_cartesian(float* cartesian, float* motors, int n_axis) { // position[X_AXIS] = // position[Y_AXIS] = } -#endif -#ifdef USE_TOOL_CHANGE /* user_tool_change() is called when tool change gcode is received, to perform appropriate actions for your machine. */ void user_tool_change(uint8_t new_tool) {} -#endif -#if defined(MACRO_BUTTON_0_PIN) || defined(MACRO_BUTTON_1_PIN) || defined(MACRO_BUTTON_2_PIN) /* options. user_defined_macro() is called with the button number to perform whatever actions you choose. */ void user_defined_macro(uint8_t index) {} -#endif -#ifdef USE_M30 /* user_m30() is called when an M30 gcode signals the end of a gcode file. */ void user_m30() {} -#endif - -#ifdef USE_MACHINE_TRINAMIC_INIT -/* - machine_triaminic_setup() replaces the normal setup of trinamic - drivers with your own code. For example, you could setup StallGuard -*/ -void machine_trinamic_setup() {} -#endif // If you add any additional functions specific to your machine that // require calls from common code, guard their calls in the common code with diff --git a/Grbl_Esp32/Custom/mpcnc_laser_module.cpp b/Grbl_Esp32/Custom/mpcnc_laser_module.cpp index e65ce629..52b816c4 100644 --- a/Grbl_Esp32/Custom/mpcnc_laser_module.cpp +++ b/Grbl_Esp32/Custom/mpcnc_laser_module.cpp @@ -21,7 +21,6 @@ along with Grbl. If not, see . */ -#ifdef USE_MACHINE_INIT /* machine_init() is called when Grbl_ESP32 first starts. You can use it to do any special things your machine needs at startup. @@ -32,4 +31,3 @@ void machine_init() { pinMode(LVL_SHIFT_ENABLE, OUTPUT); digitalWrite(LVL_SHIFT_ENABLE, HIGH); } -#endif diff --git a/Grbl_Esp32/Custom/oled_basic.cpp b/Grbl_Esp32/Custom/oled_basic.cpp index fedc543e..b1e2862b 100644 --- a/Grbl_Esp32/Custom/oled_basic.cpp +++ b/Grbl_Esp32/Custom/oled_basic.cpp @@ -120,7 +120,6 @@ void draw_checkbox(int16_t x, int16_t y, int16_t width, int16_t height, bool che void displayDRO() { uint8_t oled_y_pos; - float print_position[MAX_N_AXIS]; //float wco[MAX_N_AXIS]; display.setTextAlignment(TEXT_ALIGN_LEFT); @@ -135,10 +134,14 @@ void displayDRO() { ControlPins ctrl_pin_state = system_control_get_state(); bool prb_pin_state = probe_get_state(); + display.setTextAlignment(TEXT_ALIGN_RIGHT); + + float* print_position = system_get_mpos(); if (bit_istrue(status_mask->get(), RtStatus::Position)) { - calc_mpos(print_position); + display.drawString(60, 14, "M Pos"); } else { - calc_wpos(print_position); + display.drawString(60, 14, "W Pos"); + mpos_to_wpos(print_position); } for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { diff --git a/Grbl_Esp32/Custom/parallel_delta.cpp b/Grbl_Esp32/Custom/parallel_delta.cpp index a82de59b..23b28a81 100644 --- a/Grbl_Esp32/Custom/parallel_delta.cpp +++ b/Grbl_Esp32/Custom/parallel_delta.cpp @@ -91,7 +91,6 @@ static float last_cartesian[N_AXIS] = { }; // A place to save the previous motor angles for distance/feed rate calcs // Z offset of the effector from the arm centers // prototypes for helper functions -int calc_forward_kinematics(float* angles, float* cartesian); KinematicError delta_calcInverse(float* cartesian, float* angles); KinematicError delta_calcAngleYZ(float x0, float y0, float z0, float& theta); float three_axis_dist(float* point1, float* point2); @@ -108,11 +107,9 @@ void machine_init() { delta_crank_side_len = new FloatSetting(EXTENDED, WG, NULL, "Delta/CrankSideLength", LENGTH_FIXED_SIDE, 20.0, 500.0); delta_effector_side_len = new FloatSetting(EXTENDED, WG, NULL, "Delta/EffectorSideLength", LENGTH_EFF_SIDE, 20.0, 500.0); - read_settings(); - // Calculate the Z offset at the arm zero angles ... // Z offset is the z distance from the motor axes to the end effector axes at zero angle - calc_forward_kinematics(angles, cartesian); // Sets the cartesian values + motors_to_cartesian(cartesian, angles, 3); // Sets the cartesian values // print a startup message to show the kinematics are enabled. Print the offset for reference grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delta Kinematics Init: %s Z Offset:%4.3f", MACHINE_NAME, cartesian[Z_AXIS]); @@ -126,32 +123,20 @@ void machine_init() { // DXL_COUNT_MAX, // DXL_COUNT_PER_RADIAN); } -bool user_defined_homing(uint8_t cycle_mask) { // true = do not continue with normal Grbl homing -#ifdef USE_CUSTOM_HOMING - return true; -#else - return false; -#endif -} -// This function is used by Grbl -void inverse_kinematics(float* position) { - float motor_angles[3]; +// bool user_defined_homing(uint8_t cycle_mask) { // true = do not continue with normal Grbl homing +// #ifdef USE_CUSTOM_HOMING +// return true; +// #else +// return false; +// #endif +// } - read_settings(); - delta_calcInverse(position, motor_angles); - position[0] = motor_angles[0]; - position[1] = motor_angles[1]; - position[2] = motor_angles[2]; -} - -// This function is used by Grbl -void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) //The target and position are provided in MPos -{ +bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { float dx, dy, dz; // distances in each cartesian axis float motor_angles[3]; - float seg_target[3]; // The target of the current segment + float seg_target[3]; // The target of the current segment float feed_rate = pl_data->feed_rate; // save original feed rate bool show_error = true; // shows error once @@ -162,16 +147,17 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio // grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start %3.3f %3.3f %3.3f", position[0], position[1], position[2]); // grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target %3.3f %3.3f %3.3f", target[0], target[1], target[2]); - status = delta_calcInverse(position, motor_angles); + status = delta_calcInverse(position, last_angle); if (status == KinematicError::OUT_OF_RANGE) { //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start position error %3.3f %3.3f %3.3f", position[0], position[1], position[2]); - //start_position_error = true; + return false; } // Check the destination to see if it is in work area status = delta_calcInverse(target, motor_angles); if (status == KinematicError::OUT_OF_RANGE) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target unreachable error %3.3f %3.3f %3.3f", target[0], target[1], target[2]); + return false; } position[X_AXIS] += gc_state.coord_offset[X_AXIS]; @@ -198,21 +184,7 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio // calculate the delta motor angles KinematicError status = delta_calcInverse(seg_target, motor_angles); - if (status == KinematicError ::NONE) { - float delta_distance = three_axis_dist(motor_angles, last_angle); - - // save angles for next distance calc - memcpy(last_angle, motor_angles, sizeof(motor_angles)); - - if (pl_data->motion.rapidMotion) { - pl_data->feed_rate = feed_rate; - } else { - pl_data->feed_rate = (feed_rate * delta_distance / segment_dist); - } - - mc_line(motor_angles, pl_data); - - } else { + if (status != KinematicError ::NONE) { if (show_error) { // grbl_msg_sendf(CLIENT_SERIAL, // MsgLevel::Info, @@ -223,35 +195,52 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio // motor_angles[2]); show_error = false; } + return false; } + if (pl_data->motion.rapidMotion) { + pl_data->feed_rate = feed_rate; + } else { + float delta_distance = three_axis_dist(motor_angles, last_angle); + pl_data->feed_rate = (feed_rate * delta_distance / segment_dist); + } + + // mc_line() returns false if a jog is cancelled. + // In that case we stop sending segments to the planner. + if (!mc_line(motor_angles, pl_data)) { + return false; + } + + // save angles for next distance calc + // This is after mc_line() so that we do not update + // last_angle if the segment was discarded. + memcpy(last_angle, motor_angles, sizeof(motor_angles)); } + return true; } // this is used used by Grbl soft limits to see if the range of the machine is exceeded. -uint8_t kinematic_limits_check(float* target) { +bool limitsCheckTravel(float* target) { float motor_angles[3]; read_settings(); grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin Soft Check %3.3f, %3.3f, %3.3f", target[0], target[1], target[2]); - KinematicError status = delta_calcInverse(target, motor_angles); - - switch (status) { + switch (delta_calcInverse(target, motor_angles)) { case KinematicError::OUT_OF_RANGE: grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target out of range"); - break; + return true; case KinematicError::ANGLE_TOO_NEGATIVE: grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target max negative"); - break; + return true; case KinematicError::ANGLE_TOO_POSITIVE: grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target max positive"); - break; + return true; case KinematicError::NONE: - break; + return false; } - return (status == KinematicError::NONE); + return false; } // inverse kinematics: cartesian -> angles @@ -286,19 +275,21 @@ KinematicError delta_calcInverse(float* cartesian, float* angles) { } // inverse kinematics: angles -> cartesian -int calc_forward_kinematics(float* angles, float* catesian) { +void motors_to_cartesian(float* cartesian, float* motors, int n_axis) { + read_settings(); + float t = (f - e) * tan30 / 2; - float y1 = -(t + rf * cos(angles[0])); - float z1 = -rf * sin(angles[0]); + float y1 = -(t + rf * cos(motors[0])); + float z1 = -rf * sin(motors[0]); - float y2 = (t + rf * cos(angles[1])) * sin30; + float y2 = (t + rf * cos(motors[1])) * sin30; float x2 = y2 * tan60; - float z2 = -rf * sin(angles[1]); + float z2 = -rf * sin(motors[1]); - float y3 = (t + rf * cos(angles[2])) * sin30; + float y3 = (t + rf * cos(motors[2])) * sin30; float x3 = -y3 * tan60; - float z3 = -rf * sin(angles[2]); + float z3 = -rf * sin(motors[2]); float dnm = (y2 - y1) * x3 - (y3 - y1) * x2; @@ -321,14 +312,16 @@ int calc_forward_kinematics(float* angles, float* catesian) { // discriminant float d = b * b - (float)4.0 * a * c; - if (d < 0) - return -1; // non-existing point + if (d < 0) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "MSG:Fwd Kin Error"); + return; + } - catesian[Z_AXIS] = -(float)0.5 * (b + sqrt(d)) / a; - catesian[X_AXIS] = (a1 * catesian[Z_AXIS] + b1) / dnm; - catesian[Y_AXIS] = (a2 * catesian[Z_AXIS] + b2) / dnm; - return 0; + cartesian[Z_AXIS] = -(float)0.5 * (b + sqrt(d)) / a; + cartesian[X_AXIS] = (a1 * cartesian[Z_AXIS] + b1) / dnm; + cartesian[Y_AXIS] = (a2 * cartesian[Z_AXIS] + b2) / dnm; } + // helper functions, calculates angle theta1 (for YZ-pane) KinematicError delta_calcAngleYZ(float x0, float y0, float z0, float& theta) { float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30 @@ -361,43 +354,15 @@ float three_axis_dist(float* point1, float* point2) { return sqrt(((point1[0] - point2[0]) * (point1[0] - point2[0])) + ((point1[1] - point2[1]) * (point1[1] - point2[1])) + ((point1[2] - point2[2]) * (point1[2] - point2[2]))); } -// called by reporting for WPos status -void forward_kinematics(float* position) { - float calc_fwd[N_AXIS]; - int status; - read_settings(); - - // convert the system position in steps to radians - float position_radians[N_AXIS]; - int32_t position_steps[N_AXIS]; // Copy current state of the system position variable - memcpy(position_steps, sys_position, sizeof(sys_position)); - system_convert_array_steps_to_mpos(position_radians, position_steps); - - // grbl_msg_sendf( - // CLIENT_SERIAL, MsgLevel::Info, "Fwd Kin Angs %1.3f, %1.3f, %1.3f ", position_radians[0], position_radians[1], position_radians[2]); - - // detmine the position of the end effector joint center. - status = calc_forward_kinematics(position_radians, calc_fwd); - - if (status == 0) { - // apply offsets and set the returned values - position[X_AXIS] = calc_fwd[X_AXIS] - gc_state.coord_system[X_AXIS] + gc_state.coord_offset[X_AXIS]; - position[Y_AXIS] = calc_fwd[Y_AXIS] - gc_state.coord_system[Y_AXIS] + gc_state.coord_offset[Y_AXIS]; - position[Z_AXIS] = calc_fwd[Z_AXIS] - gc_state.coord_system[Z_AXIS] + gc_state.coord_offset[Z_AXIS]; - } else { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "MSG:Fwd Kin Error"); - } -} - -bool kinematics_pre_homing(uint8_t cycle_mask) { // true = do not continue with normal Grbl homing -#ifdef USE_CUSTOM_HOMING - return true; -#else - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "kinematics_pre_homing"); - return false; -#endif -} +// bool kinematics_pre_homing(uint8_t cycle_mask) { // true = do not continue with normal Grbl homing +// #ifdef USE_CUSTOM_HOMING +// return true; +// #else +// //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "kinematics_pre_homing"); +// return false; +// #endif +// } void kinematics_post_homing() { #ifdef USE_CUSTOM_HOMING @@ -407,9 +372,7 @@ void kinematics_post_homing() { last_angle[Y_AXIS] = sys_position[Y_AXIS] / axis_settings[Y_AXIS]->steps_per_mm->get(); last_angle[Z_AXIS] = sys_position[Z_AXIS] / axis_settings[Z_AXIS]->steps_per_mm->get(); - read_settings(); - - calc_forward_kinematics(last_angle, last_cartesian); + motors_to_cartesian(last_cartesian, last_angle, 3); // grbl_msg_sendf(CLIENT_SERIAL, // MsgLevel::Info, diff --git a/Grbl_Esp32/Custom/polar_coaster.cpp b/Grbl_Esp32/Custom/polar_coaster.cpp index 85cf3f8a..7041fbb0 100644 --- a/Grbl_Esp32/Custom/polar_coaster.cpp +++ b/Grbl_Esp32/Custom/polar_coaster.cpp @@ -85,9 +85,8 @@ void kinematics_post_homing() { */ -void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) { - //static float last_angle = 0; - //static float last_radius = 0; + +bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { float dx, dy, dz; // distances in each cartesian axis float p_dx, p_dy, p_dz; // distances in each polar axis float dist, polar_dist; // the distances in both systems...used to determine feed rate @@ -139,11 +138,19 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio // end determining new feed rate polar[RADIUS_AXIS] += x_offset; polar[Z_AXIS] += z_offset; + + // mc_line() returns false if a jog is cancelled. + // In that case we stop sending segments to the planner. + if (!mc_line(polar, pl_data)) { + return false; + } + + // last_radius = polar[RADIUS_AXIS]; last_angle = polar[POLAR_AXIS]; - mc_line(polar, pl_data); } // TO DO don't need a feedrate for rapids + return true; } /* @@ -159,18 +166,10 @@ position = the current machine position converted = position with forward kinematics applied. */ -void forward_kinematics(float* position) { - float original_position[N_AXIS]; // temporary storage of original - float print_position[N_AXIS]; - int32_t current_position[N_AXIS]; // Copy current state of the system position variable - memcpy(current_position, sys_position, sizeof(sys_position)); - system_convert_array_steps_to_mpos(print_position, current_position); - original_position[X_AXIS] = print_position[X_AXIS] - gc_state.coord_system[X_AXIS] + gc_state.coord_offset[X_AXIS]; - original_position[Y_AXIS] = print_position[Y_AXIS] - gc_state.coord_system[Y_AXIS] + gc_state.coord_offset[Y_AXIS]; - original_position[Z_AXIS] = print_position[Z_AXIS] - gc_state.coord_system[Z_AXIS] + gc_state.coord_offset[Z_AXIS]; - position[X_AXIS] = cos(radians(original_position[Y_AXIS])) * original_position[X_AXIS] * -1; - position[Y_AXIS] = sin(radians(original_position[Y_AXIS])) * original_position[X_AXIS]; - position[Z_AXIS] = original_position[Z_AXIS]; // unchanged +void motors_to_cartesian(float* cartesian, float* motors, int n_axis) { + cartesian[X_AXIS] = cos(radians(motors[Y_AXIS])) * motors[X_AXIS] * -1; + cartesian[Y_AXIS] = sin(radians(motors[Y_AXIS])) * motors[X_AXIS]; + cartesian[Z_AXIS] = motors[Z_AXIS]; // unchanged } // helper functions diff --git a/Grbl_Esp32/src/Error.cpp b/Grbl_Esp32/src/Error.cpp index c18ef200..70bb88d2 100644 --- a/Grbl_Esp32/src/Error.cpp +++ b/Grbl_Esp32/src/Error.cpp @@ -80,4 +80,5 @@ std::map ErrorNames = { { Error::NvsGetStatsFailed, "Failed to get setting status" }, { Error::AuthenticationFailed, "Authentication failed!" }, { Error::AnotherInterfaceBusy, "Another interface is busy" }, + { Error::JogCancelled, "Jog Cancelled" }, }; diff --git a/Grbl_Esp32/src/Error.h b/Grbl_Esp32/src/Error.h index 14b443eb..6b8fe3e0 100644 --- a/Grbl_Esp32/src/Error.h +++ b/Grbl_Esp32/src/Error.h @@ -84,6 +84,7 @@ enum class Error : uint8_t { AuthenticationFailed = 110, Eol = 111, AnotherInterfaceBusy = 120, + JogCancelled = 130, }; extern std::map ErrorNames; diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index bd1bf2b9..d55eb1ed 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -494,9 +494,7 @@ Error gc_execute_line(char* line, uint8_t client) { break; case 6: // tool change gc_block.modal.tool_change = ToolChange::Enable; -#ifdef USE_TOOL_CHANGE //user_tool_change(gc_state.tool); -#endif mg_word_bit = ModalGroup::MM6; break; case 7: @@ -1287,14 +1285,16 @@ Error gc_execute_line(char* line, uint8_t client) { FAIL(Error::InvalidJogCommand); } // Initialize planner data to current spindle and coolant modal state. - pl_data->spindle_speed = gc_state.spindle_speed; - pl_data->spindle = gc_state.modal.spindle; - pl_data->coolant = gc_state.modal.coolant; - Error status = jog_execute(pl_data, &gc_block); - if (status == Error::Ok) { + pl_data->spindle_speed = gc_state.spindle_speed; + pl_data->spindle = gc_state.modal.spindle; + pl_data->coolant = gc_state.modal.coolant; + bool cancelledInflight = false; + Error status = jog_execute(pl_data, &gc_block, &cancelledInflight); + if (status == Error::Ok && !cancelledInflight) { memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_block.values.xyz)); } - return status; + // JogCancelled is not reported as a GCode error + return status == Error::JogCancelled ? Error::Ok : status; } // If in laser mode, setup laser power based on current and past parser conditions. if (spindle->inLaserMode()) { @@ -1361,9 +1361,7 @@ Error gc_execute_line(char* line, uint8_t client) { // gc_state.tool = gc_block.values.t; // [6. Change tool ]: NOT SUPPORTED if (gc_block.modal.tool_change == ToolChange::Enable) { -#ifdef USE_TOOL_CHANGE user_tool_change(gc_state.tool); -#endif } // [7. Spindle control ]: if (gc_state.modal.spindle != gc_block.modal.spindle) { @@ -1485,9 +1483,9 @@ Error gc_execute_line(char* line, uint8_t client) { // and absolute and incremental modes. pl_data->motion.rapidMotion = 1; // Set rapid motion flag. if (axis_command != AxisCommand::None) { - mc_line_kins(gc_block.values.xyz, pl_data, gc_state.position); + cartesian_to_motors(gc_block.values.xyz, pl_data, gc_state.position); } - mc_line_kins(coord_data, pl_data, gc_state.position); + cartesian_to_motors(coord_data, pl_data, gc_state.position); memcpy(gc_state.position, coord_data, sizeof(gc_state.position)); break; case NonModal::SetHome0: @@ -1515,12 +1513,10 @@ Error gc_execute_line(char* line, uint8_t client) { if (axis_command == AxisCommand::MotionMode) { GCUpdatePos gc_update_pos = GCUpdatePos::Target; if (gc_state.modal.motion == Motion::Linear) { - //mc_line(gc_block.values.xyz, pl_data); - mc_line_kins(gc_block.values.xyz, pl_data, gc_state.position); + cartesian_to_motors(gc_block.values.xyz, pl_data, gc_state.position); } else if (gc_state.modal.motion == Motion::Seek) { pl_data->motion.rapidMotion = 1; // Set rapid motion flag. - //mc_line(gc_block.values.xyz, pl_data); - mc_line_kins(gc_block.values.xyz, pl_data, gc_state.position); + cartesian_to_motors(gc_block.values.xyz, pl_data, gc_state.position); } else if ((gc_state.modal.motion == Motion::CwArc) || (gc_state.modal.motion == Motion::CcwArc)) { mc_arc(gc_block.values.xyz, pl_data, @@ -1604,9 +1600,7 @@ Error gc_execute_line(char* line, uint8_t client) { coolant_off(); } report_feedback_message(Message::ProgramEnd); -#ifdef USE_M30 user_m30(); -#endif break; } gc_state.modal.program_flow = ProgramFlow::Running; // Reset program flow. diff --git a/Grbl_Esp32/src/Grbl.cpp b/Grbl_Esp32/src/Grbl.cpp index a553621c..ea0bc2a3 100644 --- a/Grbl_Esp32/src/Grbl.cpp +++ b/Grbl_Esp32/src/Grbl.cpp @@ -105,6 +105,10 @@ static void reset_variables() { plan_sync_position(); gc_sync_position(); report_init_message(CLIENT_ALL); + + // used to keep track of a jog command sent to mc_line() so we can cancel it. + // this is needed if a jogCancel comes along after we have already parsed a jog and it is in-flight. + sys_pl_data_inflight = NULL; } void run_once() { @@ -118,6 +122,10 @@ void run_once() { void __attribute__((weak)) machine_init() {} void __attribute__((weak)) display_init() {} + +void __attribute__((weak)) user_m30() {} + +void __attribute__((weak)) user_tool_change(uint8_t new_tool) {} /* setup() and loop() in the Arduino .ino implements this control flow: diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 3f949aae..f0dbd60d 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -22,7 +22,7 @@ // Grbl versioning system const char* const GRBL_VERSION = "1.3a"; -const char* const GRBL_VERSION_BUILD = "20210419"; +const char* const GRBL_VERSION_BUILD = "20210424"; //#include #include @@ -93,27 +93,21 @@ const char* const GRBL_VERSION_BUILD = "20210419"; void grbl_init(); void run_once(); -void machine_init(); // weak definition in Grbl.cpp -void display_init(); // weak definition in Grbl.cpp +void machine_init(); // weak definition in Grbl.cpp +void display_init(); // weak definition in Grbl.cpp +void user_m30(); // weak definition in Grbl.cpp/ +void user_tool_change(uint8_t new_tool); // weak definition in Grbl.cpp bool user_defined_homing(uint8_t cycle_mask); // weak definition in Limits.cpp -// Called if USE_KINEMATICS is defined +// weak definitions in MotionControl.cpp +bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position); +bool kinematics_pre_homing(uint8_t cycle_mask); +void kinematics_post_homing(); -void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position); -bool kinematics_pre_homing(uint8_t cycle_mask); -void kinematics_post_homing(); -uint8_t kinematic_limits_check(float* target); +bool limitsCheckTravel(float* target); // weak in Limits.cpp; true if out of range -// Called if USE_FWD_KINEMATICS is defined -void inverse_kinematics(float* position); // used to return a converted value -void forward_kinematics(float* position); // weak definition in Report.cpp +void motors_to_cartesian(float* cartestian, float* motors, int n_axis); // weak definition // Called if MACRO_BUTTON_0_PIN or MACRO_BUTTON_1_PIN or MACRO_BUTTON_2_PIN is defined void user_defined_macro(uint8_t index); - -// Called if USE_M30 is defined -void user_m30(); - -// Called if USE_TOOL_CHANGE is defined -void user_tool_change(uint8_t new_tool); diff --git a/Grbl_Esp32/src/Jog.cpp b/Grbl_Esp32/src/Jog.cpp index 1c0c3d00..03474c7a 100644 --- a/Grbl_Esp32/src/Jog.cpp +++ b/Grbl_Esp32/src/Jog.cpp @@ -24,11 +24,13 @@ #include "Grbl.h" // Sets up valid jog motion received from g-code parser, checks for soft-limits, and executes the jog. -Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block) { +// cancelledInflight will be set to true if was not added to parser due to a cancelJog. +Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block, bool* cancelledInflight) { // Initialize planner data struct for jogging motions. // NOTE: Spindle and coolant are allowed to fully function with overrides during a jog. pl_data->feed_rate = gc_block->values.f; pl_data->motion.noFeedOverride = 1; + pl_data->is_jog = true; #ifdef USE_LINE_NUMBERS pl_data->line_number = gc_block->values.n; #endif @@ -37,12 +39,10 @@ Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block) { return Error::TravelExceeded; } } -// Valid jog command. Plan, set state, and execute. -#ifndef USE_KINEMATICS - mc_line(gc_block->values.xyz, pl_data); -#else // else use kinematics - inverse_kinematics(gc_block->values.xyz, pl_data, gc_state.position); -#endif + // Valid jog command. Plan, set state, and execute. + if (!cartesian_to_motors(gc_block->values.xyz, pl_data, gc_state.position)) { + return Error::JogCancelled; + } if (sys.state == State::Idle) { if (plan_get_current_block() != NULL) { // Check if there is a block to execute. diff --git a/Grbl_Esp32/src/Jog.h b/Grbl_Esp32/src/Jog.h index 59307834..1867871e 100644 --- a/Grbl_Esp32/src/Jog.h +++ b/Grbl_Esp32/src/Jog.h @@ -28,4 +28,5 @@ const int JOG_LINE_NUMBER = 0; // Sets up valid jog motion received from g-code parser, checks for soft-limits, and executes the jog. -Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block); +// cancelledInflight will be set to true if was not added to parser due to a cancelJog. +Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block, bool* cancelledInflight); diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index cf184eb7..fec18cfd 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -104,7 +104,6 @@ void limits_go_home(uint8_t cycle_mask) { // Initialize variables used for homing computations. uint8_t n_cycle = (2 * n_homing_locate_cycle + 1); uint8_t step_pin[MAX_N_AXIS]; - float target[MAX_N_AXIS]; float max_travel = 0.0; auto n_axis = number_axis->get(); @@ -122,7 +121,7 @@ void limits_go_home(uint8_t cycle_mask) { uint8_t n_active_axis; AxisMask limit_state, axislock; do { - system_convert_array_steps_to_mpos(target, sys_position); + float* target = system_get_mpos(); // Initialize and declare variables needed for homing routine. axislock = 0; n_active_axis = 0; @@ -397,7 +396,7 @@ float limitsMinPosition(uint8_t axis) { // Checks and reports if target array exceeds machine travel limits. // Return true if exceeding limits // Set $/MaxTravel=0 to selectively remove an axis from soft limit checks -bool limitsCheckTravel(float* target) { +bool __attribute__((weak)) limitsCheckTravel(float* target) { uint8_t idx; auto n_axis = number_axis->get(); for (idx = 0; idx < n_axis; idx++) { diff --git a/Grbl_Esp32/src/Machines/6_pack_TMC2130_XYZ_Test.h b/Grbl_Esp32/src/Machines/6_pack_TMC2130_XYZ_Test.h new file mode 100644 index 00000000..fad84751 --- /dev/null +++ b/Grbl_Esp32/src/Machines/6_pack_TMC2130_XYZ_Test.h @@ -0,0 +1,110 @@ +#pragma once +// clang-format off + +/* + 6_pack_TMC2130_XYZ_PWM.h + + Part of Grbl_ESP32 + Pin assignments for the ESP32 SPI 6-axis board + + 2021-0302 B. Dring for Mike T. + + Grbl_ESP32 is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with Grbl_ESP32. If not, see . +*/ +#define MACHINE_NAME "6 Pack TMC2130 XYZ PWM" + +#define N_AXIS 3 + +// I2S (steppers & other output-only pins) +#define USE_I2S_OUT +#define USE_I2S_STEPS +//#define DEFAULT_STEPPER ST_I2S_STATIC + +#define I2S_OUT_BCK GPIO_NUM_22 +#define I2S_OUT_WS GPIO_NUM_17 +#define I2S_OUT_DATA GPIO_NUM_21 + +#define TRINAMIC_RUN_MODE Motors::TrinamicMode::CoolStep +#define TRINAMIC_HOMING_MODE Motors::TrinamicMode::CoolStep + +// Motor Socket #1 +#define X_TRINAMIC_DRIVER 2130 +#define X_DISABLE_PIN I2SO(0) +#define X_DIRECTION_PIN I2SO(1) +#define X_STEP_PIN I2SO(2) +#define X_CS_PIN I2SO(3) +#define X_RSENSE TMC2130_RSENSE_DEFAULT + +// Motor Socket #2 +#define Y_TRINAMIC_DRIVER X_TRINAMIC_DRIVER +#define Y_DIRECTION_PIN I2SO(4) +#define Y_STEP_PIN I2SO(5) +#define Y_DISABLE_PIN I2SO(7) +#define Y_CS_PIN I2SO(6) +#define Y_RSENSE X_RSENSE + +// Motor Socket #3 +#define Z_TRINAMIC_DRIVER X_TRINAMIC_DRIVER +#define Z_DISABLE_PIN I2SO(8) +#define Z_DIRECTION_PIN I2SO(9) +#define Z_STEP_PIN I2SO(10) +#define Z_CS_PIN I2SO(11) +#define Z_RSENSE X_RSENSE + + +/* + Socket I/O reference + The list of modules is here... + https://github.com/bdring/6-Pack_CNC_Controller/wiki/CNC-I-O-Module-List + Click on each module to get example for using the modules in the sockets + +Socket #1 +#1 GPIO_NUM_33 (Sg1) +#2 GPIO_NUM_32 (Sg2) +#3 GPIO_NUM_35 (Sg3) (input only) +#4 GPIO_NUM_34 (Sg4) (input only) + +Socket #2 +#1 GPIO_NUM_2 +#2 GPIO_NUM_25 +#3 GPIO_NUM_39 (Sg5) (input only) +#4 GPIO_NUM_36 (Sg6) (input only) + +Socket #3 +#1 GPIO_NUM_26 +#2 GPIO_NUM_4 +#3 GPIO_NUM_16 +#4 GPIO_NUM_27 + +Socket #4 +#1 GPIO_NUM_14 +#2 GPIO_NUM_13 +#3 GPIO_NUM_15 +#4 GPIO_NUM_12 + +Socket #5 +#1 I2SO(24) (output only) +#2 I2SO(25) (output only) +#3 I2SO26) (output only) + +*/ + +// Socket #1 (Empty) +// Install StallGuard Jumpers +#define X_LIMIT_PIN GPIO_NUM_33 // Sg1 +#define Y_LIMIT_PIN GPIO_NUM_32 // Sg2 +#define Z_LIMIT_PIN GPIO_NUM_35 // Sg3 +#define CONTROL_SAFETY_DOOR_PIN GPIO_NUM_34 // Sg4 + + +// === Default settings +#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE diff --git a/Grbl_Esp32/src/Machines/atari_1020.h b/Grbl_Esp32/src/Machines/atari_1020.h index eecc2146..50186bb8 100644 --- a/Grbl_Esp32/src/Machines/atari_1020.h +++ b/Grbl_Esp32/src/Machines/atari_1020.h @@ -140,11 +140,7 @@ #define ATARI_HOMING_ATTEMPTS 13 // tells grbl we have some special functions to call -#define USE_MACHINE_INIT -#define USE_CUSTOM_HOMING -#define USE_TOOL_CHANGE #define ATARI_TOOL_CHANGE_Z 5.0 -#define USE_M30 // use the user defined end of program #ifndef atari_h #define atari_h diff --git a/Grbl_Esp32/src/Machines/fysetc_ant.h b/Grbl_Esp32/src/Machines/fysetc_ant.h new file mode 100644 index 00000000..79607088 --- /dev/null +++ b/Grbl_Esp32/src/Machines/fysetc_ant.h @@ -0,0 +1,85 @@ +#pragma once +// clang-format off + +/* + fysetc_ant.h + https://github.com/FYSETC/FYSETC-E4 + + 2020-12-03 B. Dring + + This is a machine definition file to use the FYSETC E4 3D Printer controller + This is a 4 motor controller. This is setup for XYZA, but XYYZ, could also be used. + There are 5 inputs + The controller has outputs for a Fan, Hotbed and Extruder. There are mapped to + spindle, mist and flood coolant to drive an external relay. + + Grbl_ESP32 is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl_ESP32. If not, see . +*/ + +#define MACHINE_NAME "FYSETC E4 3D Printer Controller" + +#define N_AXIS 4 + +#define CUSTOM_CODE_FILENAME "../Custom/CoreXY.cpp" + +#define USE_KINEMATICS // there are kinematic equations for this machine + +#define TRINAMIC_RUN_MODE TrinamicMode :: StealthChop +#define TRINAMIC_HOMING_MODE TrinamicMode :: StealthChop + +#define TMC_UART UART_NUM_1 +#define TMC_UART_RX GPIO_NUM_21 +#define TMC_UART_TX GPIO_NUM_22 + +#define X_TRINAMIC_DRIVER 2209 +#define X_STEP_PIN GPIO_NUM_27 +#define X_DIRECTION_PIN GPIO_NUM_26 +#define X_RSENSE TMC2209_RSENSE_DEFAULT +#define X_DRIVER_ADDRESS 1 +#define DEFAULT_X_MICROSTEPS 16 + +#define Y_TRINAMIC_DRIVER 2209 +#define Y_STEP_PIN GPIO_NUM_33 +#define Y_DIRECTION_PIN GPIO_NUM_32 +#define Y_RSENSE TMC2209_RSENSE_DEFAULT +#define Y_DRIVER_ADDRESS 3 +#define DEFAULT_Y_MICROSTEPS 16 + +#define Z_TRINAMIC_DRIVER 2209 +#define Z_STEP_PIN GPIO_NUM_14 +#define Z_DIRECTION_PIN GPIO_NUM_12 +#define Z_RSENSE TMC2209_RSENSE_DEFAULT +#define Z_DRIVER_ADDRESS 0 +#define DEFAULT_Z_MICROSTEPS 16 + +#define A_TRINAMIC_DRIVER 2209 +#define A_STEP_PIN GPIO_NUM_16 +#define A_DIRECTION_PIN GPIO_NUM_17 +#define A_RSENSE TMC2209_RSENSE_DEFAULT +#define A_DRIVER_ADDRESS 2 +#define DEFAULT_A_MICROSTEPS 16 + +#define X_LIMIT_PIN GPIO_NUM_34 +#define Y_LIMIT_PIN GPIO_NUM_35 +#define Z_LIMIT_PIN GPIO_NUM_15 +#define A_LIMIT_PIN GPIO_NUM_36 // Labeled TB +#define PROBE_PIN GPIO_NUM_39 // Labeled TE + +// OK to comment out to use pin for other features +#define STEPPERS_DISABLE_PIN GPIO_NUM_25 + +#define SPINDLE_TYPE SpindleType::RELAY +#define SPINDLE_OUTPUT_PIN GPIO_NUM_13 // labeled Fan +#define COOLANT_MIST_PIN GPIO_NUM_2 // Labeled Hotbed +#define COOLANT_FLOOD_PIN GPIO_NUM_4 // Labeled Heater diff --git a/Grbl_Esp32/src/Machines/fystec_e4.h b/Grbl_Esp32/src/Machines/fysetc_e4.h similarity index 95% rename from Grbl_Esp32/src/Machines/fystec_e4.h rename to Grbl_Esp32/src/Machines/fysetc_e4.h index 63b47bab..1bb8800d 100644 --- a/Grbl_Esp32/src/Machines/fystec_e4.h +++ b/Grbl_Esp32/src/Machines/fysetc_e4.h @@ -2,12 +2,12 @@ // clang-format off /* - fystec_e4.h + fysetc_e4.h https://github.com/FYSETC/FYSETC-E4 2020-12-03 B. Dring - This is a machine definition file to use the FYSTEC E4 3D Printer controller + This is a machine definition file to use the FYSETC E4 3D Printer controller This is a 4 motor controller. This is setup for XYZA, but XYYZ, could also be used. There are 5 inputs The controller has outputs for a Fan, Hotbed and Extruder. There are mapped to @@ -27,7 +27,7 @@ along with Grbl_ESP32. If not, see . */ -#define MACHINE_NAME "FYSTEC E4 3D Printer Controller" +#define MACHINE_NAME "FYSETC E4 3D Printer Controller" #define N_AXIS 4 diff --git a/Grbl_Esp32/src/Machines/midtbot.h b/Grbl_Esp32/src/Machines/midtbot.h index 41e35c2a..a6bd8438 100644 --- a/Grbl_Esp32/src/Machines/midtbot.h +++ b/Grbl_Esp32/src/Machines/midtbot.h @@ -29,11 +29,7 @@ #define CUSTOM_CODE_FILENAME "../Custom/CoreXY.cpp" -#define MIDTBOT // applies the geometry correction to the kinematics -#define USE_KINEMATICS // there are kinematic equations for this machine -#define USE_FWD_KINEMATICS // report in cartesian -#define USE_MACHINE_INIT // There is some custom initialization for this machine -#define USE_CUSTOM_HOMING +#define MIDTBOT // applies the midTbot geometry correction to the CoreXY kinematics #define SPINDLE_TYPE SpindleType::NONE @@ -50,8 +46,6 @@ #define Z_SERVO_PIN GPIO_NUM_27 -// Set $Homing/Cycle0=Y and $Homing/Cycle1=X - #define SPINDLE_TYPE SpindleType::NONE // defaults diff --git a/Grbl_Esp32/src/Machines/midtbot_x2.h b/Grbl_Esp32/src/Machines/midtbot_x2.h new file mode 100644 index 00000000..9e904133 --- /dev/null +++ b/Grbl_Esp32/src/Machines/midtbot_x2.h @@ -0,0 +1,119 @@ +#pragma once +// clang-format off + +/* + midtbot.h + Part of Grbl_ESP32 + + Pin assignments for the Buildlog.net midtbot + https://github.com/bdring/midTbot_esp32 + + 2018 - Bart Dring + 2020 - Mitch Bradley + + Grbl_ESP32 is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl_ESP32. If not, see . +*/ + +#define MACHINE_NAME "midTbot" + +#define DISPLAY_CODE_FILENAME "Custom/oled_basic.cpp" + +#define CUSTOM_CODE_FILENAME "../Custom/CoreXY.cpp" +#define MIDTBOT // applies the geometry correction to the kinematics +#define USE_KINEMATICS // there are kinematic equations for this machine +#define USE_FWD_KINEMATICS // report in cartesian +#define SPINDLE_TYPE SpindleType::NONE + +#define TRINAMIC_UART_RUN_MODE TrinamicUartMode :: StealthChop +#define TRINAMIC_UART_HOMING_MODE TrinamicUartMode :: StealthChop + +#define TMC_UART UART_NUM_1 +#define TMC_UART_RX GPIO_NUM_21 +#define TMC_UART_TX GPIO_NUM_22 + +#define X_TRINAMIC_DRIVER 2209 +#define X_STEP_PIN GPIO_NUM_25 //GPIO_NUM_32 +#define X_DIRECTION_PIN GPIO_NUM_33 // GPIO_NUM_26 +#define X_RSENSE TMC2209_RSENSE_DEFAULT +#define X_DRIVER_ADDRESS 1 +#define DEFAULT_X_MICROSTEPS 16 + +#define Y_TRINAMIC_DRIVER 2209 +#define Y_STEP_PIN GPIO_NUM_32 //GPIO_NUM_25 +#define Y_DIRECTION_PIN GPIO_NUM_26 //GPIO_NUM_33 +#define Y_RSENSE TMC2209_RSENSE_DEFAULT +#define Y_DRIVER_ADDRESS 0 +#define DEFAULT_Y_MICROSTEPS 16 + +#define STEPPERS_DISABLE_PIN GPIO_NUM_27 + +#define X_LIMIT_PIN GPIO_NUM_4 +#define Y_LIMIT_PIN GPIO_NUM_12 + +#define Z_SERVO_PIN GPIO_NUM_15 + +// Set $Homing/Cycle0=Y and $Homing/Cycle1=X + +#define SPINDLE_TYPE SpindleType::NONE + +// ================== defaults ================================ +#define DEFAULT_HOMING_CYCLE_0 bit(Z_AXIS) +#define DEFAULT_HOMING_CYCLE_1 bit(Y_AXIS) +#define DEFAULT_HOMING_CYCLE_2 bit(X_AXIS) + +#define DEFAULT_HOMING_DIR_MASK (bit(X_AXIS) | bit (Z_AXIS)) // these home negative + +#define DEFAULT_STEP_PULSE_MICROSECONDS 3 +#define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // stay on + +#define DEFAULT_STEPPING_INVERT_MASK 0 // uint8_t +#define DEFAULT_DIRECTION_INVERT_MASK 2 // uint8_t +#define DEFAULT_INVERT_ST_ENABLE 0 // boolean +#define DEFAULT_INVERT_LIMIT_PINS 0 // boolean +#define DEFAULT_INVERT_PROBE_PIN 0 // boolean + +#define DEFAULT_STATUS_REPORT_MASK 1 + +#define DEFAULT_JUNCTION_DEVIATION 0.01 // mm +#define DEFAULT_ARC_TOLERANCE 0.002 // mm +#define DEFAULT_REPORT_INCHES 0 // false + +#define DEFAULT_SOFT_LIMIT_ENABLE 0 // false +#define DEFAULT_HARD_LIMIT_ENABLE 0 // false + +#define DEFAULT_HOMING_ENABLE 1 +#define DEFAULT_HOMING_FEED_RATE 500.0 // mm/min +#define DEFAULT_HOMING_SEEK_RATE 2000.0 // mm/min +#define DEFAULT_HOMING_DEBOUNCE_DELAY 250 // msec (0-65k) +#define DEFAULT_HOMING_PULLOFF 3.0 // mm + +#define DEFAULT_X_STEPS_PER_MM 100.0 +#define DEFAULT_Y_STEPS_PER_MM 100.0 +#define DEFAULT_Z_STEPS_PER_MM 100.0 // This is percent in servo mode + +#define DEFAULT_X_MAX_RATE 8000.0 // mm/min +#define DEFAULT_Y_MAX_RATE 8000.0 // mm/min +#define DEFAULT_Z_MAX_RATE 5000.0 // mm/min + +#define DEFAULT_X_ACCELERATION 200.0 // mm/sec^2. 200 mm/sec^2 = 720000 mm/min^2 +#define DEFAULT_Y_ACCELERATION 200.0 // mm/sec^2 +#define DEFAULT_Z_ACCELERATION 100.0 // mm/sec^2 + +#define DEFAULT_X_MAX_TRAVEL 100.0 // mm NOTE: Must be a positive value. +#define DEFAULT_Y_MAX_TRAVEL 100.0 // mm NOTE: Must be a positive value. +#define DEFAULT_Z_MAX_TRAVEL 5.0 // This is percent in servo mode + +#define DEFAULT_X_HOMING_MPOS DEFAULT_Z_MAX_TRAVEL // stays up after homing + + diff --git a/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h b/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h index 43f2864c..a9fecf73 100644 --- a/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h +++ b/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h @@ -30,7 +30,6 @@ #define MACHINE_NAME "MPCNC_V1P2 with Laser Module" // The laser module fires without a low signal. This keeps the enable on -#define USE_MACHINE_INIT #define LVL_SHIFT_ENABLE GPIO_NUM_32 #define CUSTOM_CODE_FILENAME "Custom/mpcnc_laser_module.cpp" diff --git a/Grbl_Esp32/src/Machines/polar_coaster.h b/Grbl_Esp32/src/Machines/polar_coaster.h index b0e45e7c..03f9f1ea 100644 --- a/Grbl_Esp32/src/Machines/polar_coaster.h +++ b/Grbl_Esp32/src/Machines/polar_coaster.h @@ -37,9 +37,6 @@ #define POLAR_AXIS 1 #define SEGMENT_LENGTH 0.5 // segment length in mm -#define USE_KINEMATICS -#define USE_FWD_KINEMATICS // report in cartesian -#define USE_M30 #define X_STEP_PIN GPIO_NUM_15 #define Y_STEP_PIN GPIO_NUM_2 @@ -123,4 +120,6 @@ #define DEFAULT_X_MAX_TRAVEL 50.0 // mm NOTE: Must be a positive value. #define DEFAULT_Y_MAX_TRAVEL 300.0 // mm NOTE: Must be a positive value. -#define DEFAULT_Z_MAX_TRAVEL 100.0 // This is percent in servo mode +#define DEFAULT_Z_MAX_TRAVEL 5.0 // This is percent in servo mode + +#define DEFAULT_Z_HOMING_MPOS DEFAULT_Z_MAX_TRAVEL // stays up after homing diff --git a/Grbl_Esp32/src/Machines/tapster_3.h b/Grbl_Esp32/src/Machines/tapster_3.h index b4de2b1c..d801aed5 100644 --- a/Grbl_Esp32/src/Machines/tapster_3.h +++ b/Grbl_Esp32/src/Machines/tapster_3.h @@ -23,12 +23,6 @@ #define N_AXIS 3 -// ================= Firmware Customization =================== - -#define USE_KINEMATICS // there are kinematic equations for this machine -#define USE_FWD_KINEMATICS // report in cartesian -#define USE_MACHINE_INIT // There is some custom initialization for this machine - // ================== Delta Geometry =========================== #define RADIUS_FIXED 70.0 // radius of the fixed side (length of motor cranks) diff --git a/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h b/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h index 6f88a816..6b4047ee 100644 --- a/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h +++ b/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h @@ -26,8 +26,6 @@ #define FWD_KINEMATICS_REPORTING // report in cartesian #define USE_RMT_STEPS // Use the RMT periferal to generate step pulses #define USE_TRINAMIC // some Trinamic motors are used on this machine -#define USE_MACHINE_TRINAMIC_INIT // there is a machine specific setup for the drivers -#define USE_MACHINE_INIT // There is some custom initialization for this machine #define SEGMENT_LENGTH 0.5 // segment length in mm #define KIN_ANGLE_CALC_OK 0 @@ -41,10 +39,6 @@ #define N_AXIS 3 -#define USE_KINEMATICS // there are kinematic equations for this machine -#define USE_FWD_KINEMATICS // report in cartesian -#define USE_MACHINE_INIT // There is some custom initialization for this machine - // ================== Delta Geometry =========================== #define RADIUS_FIXED 100.0f // radius of the fixed side (length of motor cranks) diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index 1f2dc53f..00762b97 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -33,15 +33,6 @@ SquaringMode ganged_mode = SquaringMode::Dual; -// this allows kinematics to be used. -void mc_line_kins(float* target, plan_line_data_t* pl_data, float* position) { -#ifndef USE_KINEMATICS - mc_line(target, pl_data); -#else // else use kinematics - inverse_kinematics(target, pl_data, position); -#endif -} - // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in // (1 minute)/feed_rate time. @@ -49,7 +40,11 @@ void mc_line_kins(float* target, plan_line_data_t* pl_data, float* position) { // segments, must pass through this routine before being passed to the planner. The seperation of // mc_line and plan_buffer_line is done primarily to place non-planner-type functions from being // in the planner and to let backlash compensation or canned cycle integration simple and direct. -void mc_line(float* target, plan_line_data_t* pl_data) { +// returns true if line was submitted to planner, or false if intentionally dropped. +bool mc_line(float* target, plan_line_data_t* pl_data) { + bool submitted_result = false; + // store the plan data so it can be cancelled by the protocol system if needed + sys_pl_data_inflight = pl_data; // If enabled, check for soft limit violations. Placed here all line motions are picked up // from everywhere in Grbl. if (soft_limits->get()) { @@ -60,7 +55,8 @@ void mc_line(float* target, plan_line_data_t* pl_data) { } // If in check gcode mode, prevent motion by blocking planner. Soft limits still work. if (sys.state == State::CheckMode) { - return; + sys_pl_data_inflight = NULL; + return submitted_result; } // NOTE: Backlash compensation may be installed here. It will need direction info to track when // to insert a backlash line motion(s) before the intended line motion and will require its own @@ -80,7 +76,8 @@ void mc_line(float* target, plan_line_data_t* pl_data) { do { protocol_execute_realtime(); // Check for any run-time commands if (sys.abort) { - return; // Bail, if system abort. + sys_pl_data_inflight = NULL; + return submitted_result; // Bail, if system abort. } if (plan_check_full_buffer()) { protocol_auto_cycle_start(); // Auto-cycle start when buffer is full. @@ -90,9 +87,29 @@ void mc_line(float* target, plan_line_data_t* pl_data) { } while (1); // Plan and queue motion into planner buffer // uint8_t plan_status; // Not used in normal operation. - plan_buffer_line(target, pl_data); + if (sys_pl_data_inflight == pl_data) { + plan_buffer_line(target, pl_data); + submitted_result = true; + } + sys_pl_data_inflight = NULL; + return submitted_result; } +bool __attribute__((weak)) cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { + return mc_line(target, pl_data); +} + +bool __attribute__((weak)) kinematics_pre_homing(uint8_t cycle_mask) { + return false; // finish normal homing cycle +} + +void __attribute__((weak)) kinematics_post_homing() {} + +void __attribute__((weak)) motors_to_cartesian(float* cartesian, float* motors, int n_axis) { + memcpy(cartesian, motors, n_axis * sizeof(motors[0])); +} + +void __attribute__((weak)) forward_kinematics(float* position) {} // Execute an arc in offset mode format. position == current xyz, target == target xyz, // offset == offset from current xyz, axis_X defines circle plane in tool space, axis_linear is // the direction of helical travel, radius == circle radius, isclockwise boolean. Used @@ -117,14 +134,12 @@ void mc_arc(float* target, float rt_axis1 = target[axis_1] - center_axis1; float previous_position[MAX_N_AXIS] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; -#ifdef USE_KINEMATICS uint16_t n; auto n_axis = number_axis->get(); for (n = 0; n < n_axis; n++) { previous_position[n] = position[n]; } -#endif // CCW angle between position and target from circle center. Only one atan2() trig computation required. float angular_travel = atan2(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1); if (is_clockwise_arc) { // Correct atan2 output per direction @@ -206,15 +221,11 @@ void mc_arc(float* target, position[axis_0] = center_axis0 + r_axis0; position[axis_1] = center_axis1 + r_axis1; position[axis_linear] += linear_per_segment; -#ifdef USE_KINEMATICS pl_data->feed_rate = original_feedrate; // This restores the feedrate kinematics may have altered - mc_line_kins(position, pl_data, previous_position); + cartesian_to_motors(position, pl_data, previous_position); previous_position[axis_0] = position[axis_0]; previous_position[axis_1] = position[axis_1]; previous_position[axis_linear] = position[axis_linear]; -#else - mc_line(position, pl_data); -#endif // Bail mid-circle on system abort. Runtime command check already performed by mc_line. if (sys.abort) { return; @@ -222,7 +233,7 @@ void mc_arc(float* target, } } // Ensure last segment arrives at target location. - mc_line_kins(target, pl_data, previous_position); + cartesian_to_motors(target, pl_data, previous_position); } // Execute dwell in seconds. @@ -292,11 +303,9 @@ void mc_homing_cycle(uint8_t cycle_mask) { // This give kinematics a chance to do something before normal homing // if it returns true, the homing is canceled. -#ifdef USE_KINEMATICS if (kinematics_pre_homing(cycle_mask)) { return; } -#endif // Check and abort homing cycle, if hard limits are already enabled. Helps prevent problems // with machines with limits wired on both ends of travel to one limit pin. // TODO: Move the pin-specific LIMIT_PIN call to Limits.cpp as a function. @@ -366,10 +375,8 @@ void mc_homing_cycle(uint8_t cycle_mask) { // Sync gcode parser and planner positions to homed position. gc_sync_position(); plan_sync_position(); -#ifdef USE_KINEMATICS // This give kinematics a chance to do something after normal homing kinematics_post_homing(); -#endif // If hard limits feature enabled, re-enable hard limits pin change register after homing cycle. limits_init(); } @@ -412,7 +419,7 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par } // Setup and queue probing motion. Auto cycle-start should not start the cycle. grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Found"); - mc_line_kins(target, pl_data, gc_state.position); + cartesian_to_motors(target, pl_data, gc_state.position); // Activate the probing state monitor in the stepper module. sys_probe_state = Probe::Active; // Perform probing cycle. Wait here until probe is triggered or motion completes. diff --git a/Grbl_Esp32/src/MotionControl.h b/Grbl_Esp32/src/MotionControl.h index c7adebc5..c9f26c04 100644 --- a/Grbl_Esp32/src/MotionControl.h +++ b/Grbl_Esp32/src/MotionControl.h @@ -35,8 +35,8 @@ const int PARKING_MOTION_LINE_NUMBER = 0; // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in // (1 minute)/feed_rate time. -void mc_line_kins(float* target, plan_line_data_t* pl_data, float* position); -void mc_line(float* target, plan_line_data_t* pl_data); +bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position); +bool mc_line(float* target, plan_line_data_t* pl_data); // returns true if line was submitted to planner // Execute an arc in offset mode format. position == current xyz, target == target xyz, // offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is diff --git a/Grbl_Esp32/src/Motors/Dynamixel2.cpp b/Grbl_Esp32/src/Motors/Dynamixel2.cpp index 3c3faedc..8543dac2 100644 --- a/Grbl_Esp32/src/Motors/Dynamixel2.cpp +++ b/Grbl_Esp32/src/Motors/Dynamixel2.cpp @@ -36,7 +36,7 @@ namespace Motors { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Dynamixel Error. Missing pin definitions"); _has_errors = true; } else { - _has_errors = false; // The motor can be used + _has_errors = false; // The motor can be used } } @@ -103,8 +103,8 @@ namespace Motors { } } else { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Dynamixel Servo ID %d Ping failed", reportAxisNameMsg(_axis_index, _dual_axis_index), -_id); + grbl_msg_sendf( + CLIENT_SERIAL, MsgLevel::Info, "%s Dynamixel Servo ID %d Ping failed", reportAxisNameMsg(_axis_index, _dual_axis_index), _id); return false; } @@ -197,7 +197,7 @@ _id); set_disable(false); set_location(); // force the PWM to update now - return false; // Cannot do conventional homing + return false; // Cannot do conventional homing } void Dynamixel2::dxl_goal_position(int32_t position) { @@ -345,16 +345,14 @@ _id); tx_message[++msg_index] = 4; // low order data length tx_message[++msg_index] = 0; // high order data length - auto n_axis = number_axis->get(); + auto n_axis = number_axis->get(); + float* mpos = system_get_mpos(); for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { for (uint8_t gang_index = 0; gang_index < 2; gang_index++) { current_id = ids[axis][gang_index]; if (current_id != 0) { count++; // keep track of the count for the message length - //determine the location of the axis - float target = system_convert_axis_steps_to_mpos(sys_position, axis); // get the axis machine position in mm - dxl_count_min = DXL_COUNT_MIN; dxl_count_max = DXL_COUNT_MAX; @@ -362,7 +360,8 @@ _id); swap(dxl_count_min, dxl_count_max); // map the mm range to the servo range - dxl_position = (uint32_t)mapConstrain(target, limitsMinPosition(axis), limitsMaxPosition(axis), dxl_count_min, dxl_count_max); + dxl_position = + (uint32_t)mapConstrain(mpos[axis], limitsMinPosition(axis), limitsMaxPosition(axis), dxl_count_min, dxl_count_max); tx_message[++msg_index] = current_id; // ID of the servo tx_message[++msg_index] = dxl_position & 0xFF; // data diff --git a/Grbl_Esp32/src/Motors/Motor.cpp b/Grbl_Esp32/src/Motors/Motor.cpp index 195601f8..6de1c869 100644 --- a/Grbl_Esp32/src/Motors/Motor.cpp +++ b/Grbl_Esp32/src/Motors/Motor.cpp @@ -16,7 +16,6 @@ Make sure public/private/protected is cleaned up. Only a few Unipolar axes have been setup in init() Get rid of Z_SERVO, just reply on Z_SERVO_PIN - Deal with custom machine ... machine_trinamic_setup(); Class is ready to deal with non SPI pins, but they have not been needed yet. It would be nice in the config message though Testing @@ -34,8 +33,7 @@ #include "Motor.h" namespace Motors { - Motor::Motor(uint8_t axis_index) : - _axis_index(axis_index % MAX_AXES), _dual_axis_index(axis_index / MAX_AXES) {} + Motor::Motor(uint8_t axis_index) : _axis_index(axis_index % MAX_AXES), _dual_axis_index(axis_index / MAX_AXES) {} void Motor::debug_message() {} diff --git a/Grbl_Esp32/src/Motors/Motors.cpp b/Grbl_Esp32/src/Motors/Motors.cpp index 4cb5289a..21f955bd 100644 --- a/Grbl_Esp32/src/Motors/Motors.cpp +++ b/Grbl_Esp32/src/Motors/Motors.cpp @@ -16,7 +16,6 @@ Make sure public/private/protected is cleaned up. Only a few Unipolar axes have been setup in init() Get rid of Z_SERVO, just reply on Z_SERVO_PIN - Deal with custom machine ... machine_trinamic_setup(); Class is ready to deal with non SPI pins, but they have not been needed yet. It would be nice in the config message though Testing diff --git a/Grbl_Esp32/src/Planner.h b/Grbl_Esp32/src/Planner.h index 1f5f3eb4..812229c9 100644 --- a/Grbl_Esp32/src/Planner.h +++ b/Grbl_Esp32/src/Planner.h @@ -91,6 +91,7 @@ typedef struct { #ifdef USE_LINE_NUMBERS int32_t line_number; // Desired line number to report when executing. #endif + bool is_jog; // true if this was generated due to a jog command } plan_line_data_t; // Initialize and reset the motion plan subsystem diff --git a/Grbl_Esp32/src/Protocol.cpp b/Grbl_Esp32/src/Protocol.cpp index 4d055e51..0119a153 100644 --- a/Grbl_Esp32/src/Protocol.cpp +++ b/Grbl_Esp32/src/Protocol.cpp @@ -532,7 +532,6 @@ static void protocol_exec_rt_suspend() { #ifdef PARKING_ENABLE // Declare and initialize parking local variables float restore_target[MAX_N_AXIS]; - float parking_target[MAX_N_AXIS]; float retract_waypoint = PARKING_PULLOUT_INCREMENT; plan_line_data_t plan_data; plan_line_data_t* pl_data = &plan_data; @@ -567,12 +566,21 @@ static void protocol_exec_rt_suspend() { if (sys.abort) { return; } + // if a jogCancel comes in and we have a jog "in-flight" (parsed and handed over to mc_line()), + // then we need to cancel it before it reaches the planner. otherwise we may try to move way out of + // normal bounds, especially with senders that issue a series of jog commands before sending a cancel. + if (sys.suspend.bit.jogCancel && sys_pl_data_inflight != NULL && ((plan_line_data_t*)sys_pl_data_inflight)->is_jog) { + sys_pl_data_inflight = NULL; + } // Block until initial hold is complete and the machine has stopped motion. if (sys.suspend.bit.holdComplete) { // Parking manager. Handles de/re-energizing, switch state checks, and parking motions for // the safety door and sleep states. if (sys.state == State::SafetyDoor || sys.state == State::Sleep) { // Handles retraction motions and de-energizing. +#ifdef PARKING_ENABLE + float* parking_target = system_get_mpos(); +#endif if (!sys.suspend.bit.retractComplete) { // Ensure any prior spindle stop override is disabled at start of safety door routine. sys.spindle_stop_ovr.value = 0; // Disable override @@ -581,9 +589,8 @@ static void protocol_exec_rt_suspend() { coolant_off(); #else // Get current position and store restore location and spindle retract waypoint. - system_convert_array_steps_to_mpos(parking_target, sys_position); if (!sys.suspend.bit.restartRetract) { - memcpy(restore_target, parking_target, sizeof(parking_target)); + memcpy(restore_target, parking_target, sizeof(restore_target[0]) * number_axis->get()); retract_waypoint += restore_target[PARKING_AXIS]; retract_waypoint = MIN(retract_waypoint, PARKING_TARGET); } diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index 3339a23c..9afe1c45 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -299,11 +299,11 @@ void report_grbl_help(uint8_t client) { // These values are retained until Grbl is power-cycled, whereby they will be re-zeroed. void report_probe_parameters(uint8_t client) { // Report in terms of machine position. - float print_position[MAX_N_AXIS]; - char probe_rpt[(axesStringLen + 13 + 6 + 1)]; // the probe report we are building here - char temp[axesStringLen]; + char probe_rpt[(axesStringLen + 13 + 6 + 1)]; // the probe report we are building here + char temp[axesStringLen]; strcpy(probe_rpt, "[PRB:"); // initialize the string with the first characters // get the machine position and put them into a string and append to the probe report + float print_position[MAX_N_AXIS]; system_convert_array_steps_to_mpos(print_position, sys_probe_position); report_util_axis_values(print_position, temp); strcat(probe_rpt, temp); @@ -574,28 +574,6 @@ void report_echo_line_received(char* line, uint8_t client) { // float print_position = returned position // float wco = returns the work coordinate offset // bool wpos = true for work position compensation -void report_calc_status_position(float* print_position, float* wco, bool wpos) { - int32_t current_position[MAX_N_AXIS]; // Copy current state of the system position variable - memcpy(current_position, sys_position, sizeof(sys_position)); - system_convert_array_steps_to_mpos(print_position, current_position); - - //float wco[MAX_N_AXIS]; - if (wpos || (sys.report_wco_counter == 0)) { - auto n_axis = number_axis->get(); - for (uint8_t idx = 0; idx < n_axis; idx++) { - // Apply work coordinate offsets and tool length offset to current position. - wco[idx] = gc_state.coord_system[idx] + gc_state.coord_offset[idx]; - if (idx == TOOL_LENGTH_OFFSET_AXIS) { - wco[idx] += gc_state.tool_length_offset; - } - if (wpos) { - print_position[idx] -= wco[idx]; - } - } - } - - forward_kinematics(print_position); // a weak definition does nothing. Users can provide strong version -} // Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram // and the actual location of the CNC machine. Users may change the following function to their @@ -603,20 +581,19 @@ void report_calc_status_position(float* print_position, float* wco, bool wpos) { // requires as it minimizes the computational overhead and allows grbl to keep running smoothly, // especially during g-code programs with fast, short line segments and high frequency reports (5-20Hz). void report_realtime_status(uint8_t client) { - float print_position[MAX_N_AXIS]; - char status[200]; - char temp[MAX_N_AXIS * 20]; + char status[200]; + char temp[MAX_N_AXIS * 20]; strcpy(status, "<"); strcat(status, report_state_text()); // Report position + float* print_position = system_get_mpos(); if (bit_istrue(status_mask->get(), RtStatus::Position)) { - calc_mpos(print_position); strcat(status, "|MPos:"); } else { - calc_wpos(print_position); strcat(status, "|WPos:"); + mpos_to_wpos(print_position); } report_util_axis_values(print_position, temp); strcat(status, temp); @@ -956,26 +933,14 @@ void reportTaskStackSize(UBaseType_t& saved) { #endif } -void calc_mpos(float* print_position) { - int32_t current_position[MAX_N_AXIS]; // Copy current state of the system position variable - memcpy(current_position, sys_position, sizeof(sys_position)); - system_convert_array_steps_to_mpos(print_position, current_position); - forward_kinematics(print_position); // a weak definition does nothing. Users can provide strong version -} - -void calc_wpos(float* print_position) { - int32_t current_position[MAX_N_AXIS]; // Copy current state of the system position variable - memcpy(current_position, sys_position, sizeof(sys_position)); - system_convert_array_steps_to_mpos(print_position, current_position); - +void mpos_to_wpos(float* position) { float* wco = get_wco(); auto n_axis = number_axis->get(); for (int idx = 0; idx < n_axis; idx++) { - print_position[idx] -= wco[idx]; + position[idx] -= wco[idx]; } - - forward_kinematics(print_position); // a weak definition does nothing. Users can provide strong version } + float* get_wco() { static float wco[MAX_N_AXIS]; auto n_axis = number_axis->get(); @@ -988,5 +953,3 @@ float* get_wco() { } return wco; } - -void __attribute__((weak)) forward_kinematics(float* position) {} // This version does nothing. Make your own to do something with it diff --git a/Grbl_Esp32/src/Report.h b/Grbl_Esp32/src/Report.h index 366e8d9d..eecd905a 100644 --- a/Grbl_Esp32/src/Report.h +++ b/Grbl_Esp32/src/Report.h @@ -92,9 +92,6 @@ void report_grbl_settings(uint8_t client, uint8_t show_extended); // Prints an echo of the pre-parsed line received right before execution. void report_echo_line_received(char* line, uint8_t client); -// calculate the postion for status reports -void report_calc_status_position(float* print_position, float* wco, bool wpos); - // Prints realtime status report void report_realtime_status(uint8_t client); @@ -134,5 +131,4 @@ void reportTaskStackSize(UBaseType_t& saved); char* report_state_text(); float* get_wco(); -void calc_mpos(float* print_position); -void calc_wpos(float* print_position); +void mpos_to_wpos(float* position); diff --git a/Grbl_Esp32/src/System.cpp b/Grbl_Esp32/src/System.cpp index 60a399b2..74b593fd 100644 --- a/Grbl_Esp32/src/System.cpp +++ b/Grbl_Esp32/src/System.cpp @@ -30,6 +30,7 @@ volatile ExecState sys_rt_exec_state; // Global realtime executor bitflag v volatile ExecAlarm sys_rt_exec_alarm; // Global realtime executor bitflag variable for setting various alarms. volatile ExecAccessory sys_rt_exec_accessory_override; // Global realtime executor bitflag variable for spindle/coolant overrides. volatile bool cycle_stop; // For state transitions, instead of bitflag +volatile void* sys_pl_data_inflight; // holds a plan_line_data_t while cartesian_to_motors has taken ownership of a line motion #ifdef DEBUG volatile bool sys_rt_exec_debug; #endif @@ -166,9 +167,6 @@ void system_flag_wco_change() { sys.report_wco_counter = 0; } -// Returns machine position of axis 'idx'. Must be sent a 'step' array. -// NOTE: If motor steps and machine position are not in the same coordinate frame, this function -// serves as a central place to compute the transformation. float system_convert_axis_steps_to_mpos(int32_t* steps, uint8_t idx) { float pos; float steps_per_mm = axis_settings[idx]->steps_per_mm->get(); @@ -176,14 +174,22 @@ float system_convert_axis_steps_to_mpos(int32_t* steps, uint8_t idx) { return pos; } +// Returns machine position of axis 'idx'. Must be sent a 'step' array. +// NOTE: If motor steps and machine position are not in the same coordinate frame, this function +// serves as a central place to compute the transformation. void system_convert_array_steps_to_mpos(float* position, int32_t* steps) { - uint8_t idx; - auto n_axis = number_axis->get(); - for (idx = 0; idx < n_axis; idx++) { - position[idx] = system_convert_axis_steps_to_mpos(steps, idx); + auto n_axis = number_axis->get(); + float motors[n_axis]; + for (int idx = 0; idx < n_axis; idx++) { + motors[idx] = (float)steps[idx] / axis_settings[idx]->steps_per_mm->get(); } - return; + motors_to_cartesian(position, motors, n_axis); } +float* system_get_mpos() { + static float position[MAX_N_AXIS]; + system_convert_array_steps_to_mpos(position, sys_position); + return position; +}; // Returns control pin state as a uint8 bitfield. Each bit indicates the input pin state, where // triggered is 1 and not triggered is 0. Invert mask is applied. Bitfield organization is diff --git a/Grbl_Esp32/src/System.h b/Grbl_Esp32/src/System.h index d955f01e..c9f222ff 100644 --- a/Grbl_Esp32/src/System.h +++ b/Grbl_Esp32/src/System.h @@ -138,6 +138,7 @@ extern volatile Percent sys_rt_f_override; // Feed override extern volatile Percent sys_rt_r_override; // Rapid feed override value in percent extern volatile Percent sys_rt_s_override; // Spindle override value in percent extern volatile bool cycle_stop; +extern volatile void* sys_pl_data_inflight; // holds a plan_line_data_t while cartesian_to_motors has taken ownership of a line motion #ifdef DEBUG extern volatile bool sys_rt_exec_debug; #endif @@ -164,7 +165,8 @@ void system_flag_wco_change(); float system_convert_axis_steps_to_mpos(int32_t* steps, uint8_t idx); // Updates a machine 'position' array based on the 'step' array sent. -void system_convert_array_steps_to_mpos(float* position, int32_t* steps); +void system_convert_array_steps_to_mpos(float* position, int32_t* steps); +float* system_get_mpos(); // A task that runs after a control switch interrupt for debouncing. void controlCheckTask(void* pvParameters);