diff --git a/Grbl_Esp32/commands.cpp b/Grbl_Esp32/commands.cpp index e1e69964..610de497 100644 --- a/Grbl_Esp32/commands.cpp +++ b/Grbl_Esp32/commands.cpp @@ -2035,10 +2035,13 @@ bool COMMANDS::execute_internal_command (int cmd, String cmd_params, level_authe resp += " # webcommunication: Sync: "; resp += String(web_server.port() + 1); #endif - resp += "# hostname:"; + resp += " # hostname:"; resp += wifi_config.Hostname(); if (WiFi.getMode() == WIFI_AP)resp += "(AP mode)"; #endif + //to save time in decoding `?` + resp += " # axis:"; + resp += String(N_AXIS); if (espresponse)espresponse->println (resp.c_str()); } break; diff --git a/Grbl_Esp32/config.h b/Grbl_Esp32/config.h index 057affb7..443d8445 100644 --- a/Grbl_Esp32/config.h +++ b/Grbl_Esp32/config.h @@ -46,7 +46,7 @@ Some features should not be changed. See notes below. // one configuration file by placing their specific defaults and pin map at the bottom of this file. // If doing so, simply comment out these two defines and see instructions below. #define DEFAULTS_GENERIC -#define CPU_MAP_ESP32 // these are defined in cpu_map.h +#define CPU_MAP_FOO_6X // these are defined in cpu_map.h #define VERBOSE_HELP // adds addition help info, but could confuse some senders diff --git a/Grbl_Esp32/cpu_map.h b/Grbl_Esp32/cpu_map.h index ab1f8fed..f662789a 100644 --- a/Grbl_Esp32/cpu_map.h +++ b/Grbl_Esp32/cpu_map.h @@ -1150,7 +1150,159 @@ #endif +#ifdef CPU_MAP_FOO_6X + #define CPU_MAP_NAME "CPU_MAP_FOO_6X" + + /* + #define USE_KINEMATICS + #include "kinematics.h" + */ + + // stepper motors + #define USE_RMT_STEPS + + #define X_STEP_PIN GPIO_NUM_12 + #define X_DIRECTION_PIN GPIO_NUM_26 + #define X_RMT_CHANNEL 0 + + #define Y_STEP_PIN GPIO_NUM_14 + #define Y_DIRECTION_PIN GPIO_NUM_25 + #define Y_RMT_CHANNEL 1 + // Z is a servo + + #define A_STEP_PIN GPIO_NUM_27 + #define A_DIRECTION_PIN GPIO_NUM_33 + #define A_RMT_CHANNEL 2 + + #define B_STEP_PIN GPIO_NUM_15 + #define B_DIRECTION_PIN GPIO_NUM_32 + #define B_RMT_CHANNEL 3 + + // C is a servo + + // servos + #define USE_SERVO_AXES + #define SERVO_Z_PIN GPIO_NUM_22 + #define SERVO_Z_CHANNEL_NUM 6 + #define SERVO_Z_RANGE_MIN 0.0 + #define SERVO_Z_RANGE_MAX 5.0 + + #define SERVO_C_PIN GPIO_NUM_2 + #define SERVO_C_CHANNEL_NUM 7 + #define SERVO_C_RANGE_MIN 0.0 + #define SERVO_C_RANGE_MAX 5.0 + + // limit switches + #define X_LIMIT_PIN GPIO_NUM_21 + #define Y_LIMIT_PIN GPIO_NUM_17 + #define A_LIMIT_PIN GPIO_NUM_16 + #define B_LIMIT_PIN GPIO_NUM_4 + #define LIMIT_MASK B11011 + + // OK to comment out to use pin for other features + #define STEPPERS_DISABLE_PIN GPIO_NUM_13 + + #ifdef HOMING_CYCLE_0 // undefine from config.h + #undef HOMING_CYCLE_0 + #endif + //#define HOMING_CYCLE_0 (1< + #ifdef DEFAULTS_GENERIC + #undef DEFAULTS_GENERIC // undefine generic then define each default below + #endif + #define DEFAULT_STEP_PULSE_MICROSECONDS 3 + #define DEFAULT_STEPPER_IDLE_LOCK_TIME 200 // 200ms + + #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 1 // 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_DIR_MASK 17 + #define DEFAULT_HOMING_FEED_RATE 200.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_SPINDLE_RPM_MAX 1000.0 // rpm + #define DEFAULT_SPINDLE_RPM_MIN 0.0 // rpm + + #define DEFAULT_LASER_MODE 0 // false + + #define DEFAULT_X_STEPS_PER_MM 400.0 + #define DEFAULT_Y_STEPS_PER_MM 400.0 + #define DEFAULT_Z_STEPS_PER_MM 100.0 // This is percent in servo mode + #define DEFAULT_A_STEPS_PER_MM 400.0 + #define DEFAULT_B_STEPS_PER_MM 400.0 + #define DEFAULT_C_STEPS_PER_MM 100.0 // This is percent in servo mode + + + #define DEFAULT_X_MAX_RATE 30000.0 // mm/min + #define DEFAULT_Y_MAX_RATE 30000.0 // mm/min + #define DEFAULT_Z_MAX_RATE 8000.0 // mm/min + #define DEFAULT_A_MAX_RATE 30000.0 // mm/min + #define DEFAULT_B_MAX_RATE 30000.0 // mm/min + #define DEFAULT_C_MAX_RATE 8000.0 // mm/min + + #define DEFAULT_X_ACCELERATION (1500.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 + #define DEFAULT_Y_ACCELERATION (1500.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 + #define DEFAULT_Z_ACCELERATION (100.0*60*60) + #define DEFAULT_A_ACCELERATION (1500.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 + #define DEFAULT_B_ACCELERATION (1500.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 + #define DEFAULT_C_ACCELERATION (100.0*60*60) + + #define DEFAULT_X_MAX_TRAVEL 250.0 // mm NOTE: Must be a positive value. + #define DEFAULT_Y_MAX_TRAVEL 250.0 // mm NOTE: Must be a positive value. + #define DEFAULT_Z_MAX_TRAVEL 100.0 // This is percent in servo mode + #define DEFAULT_A_MAX_TRAVEL 250.0 // mm NOTE: Must be a positive value. + #define DEFAULT_B_MAX_TRAVEL 250.0 // mm NOTE: Must be a positive value. + #define DEFAULT_C_MAX_TRAVEL 100.0 // This is percent in servo mode + +#endif // ================= common to all machines ================================ @@ -1168,17 +1320,25 @@ #define X_STEP_BIT 0 // don't change #define Y_STEP_BIT 1 // don't change - #define Z_STEP_BIT 2 // don't change - #define STEP_MASK B111 // don't change + #define Z_STEP_BIT 2 // don't change + #define A_STEP_BIT 3 // don't change + #define B_STEP_BIT 4 // don't change + #define C_STEP_BIT 5 // don't change + #define STEP_MASK B111111 // don't change #define X_DIRECTION_BIT 0 // don't change #define Y_DIRECTION_BIT 1 // don't change #define Z_DIRECTION_BIT 2 // don't change - + #define A_DIRECTION_BIT 3 // don't change + #define B_DIRECTION_BIT 4 // don't change + #define C_DIRECTION_BIT 5 // don't change + #define X_LIMIT_BIT 0 // don't change #define Y_LIMIT_BIT 1 // don't change #define Z_LIMIT_BIT 2 // don't change - + #define A_LIMIT_BIT 3 // don't change + #define B_LIMIT_BIT 4 // don't change + #define C_LIMIT_BIT 5 // don't change #define PROBE_MASK 1 // don't change diff --git a/Grbl_Esp32/data/index.html.gz b/Grbl_Esp32/data/index.html.gz index 089cd067..16af0b11 100644 Binary files a/Grbl_Esp32/data/index.html.gz and b/Grbl_Esp32/data/index.html.gz differ diff --git a/Grbl_Esp32/defaults.h b/Grbl_Esp32/defaults.h index c255e4f6..b98bb645 100644 --- a/Grbl_Esp32/defaults.h +++ b/Grbl_Esp32/defaults.h @@ -66,18 +66,31 @@ #define DEFAULT_X_STEPS_PER_MM 800.0 #define DEFAULT_Y_STEPS_PER_MM 800.0 #define DEFAULT_Z_STEPS_PER_MM 800.0 + #define DEFAULT_A_STEPS_PER_MM 100.0 + #define DEFAULT_B_STEPS_PER_MM 100.0 + #define DEFAULT_C_STEPS_PER_MM 100.0 #define DEFAULT_X_MAX_RATE 5000.0 // mm/min #define DEFAULT_Y_MAX_RATE 4000.0 // mm/min #define DEFAULT_Z_MAX_RATE 3000.0 // mm/min + #define DEFAULT_A_MAX_RATE 1000.0 // mm/min + #define DEFAULT_B_MAX_RATE 1000.0 // mm/min + #define DEFAULT_C_MAX_RATE 1000.0 // mm/min #define DEFAULT_X_ACCELERATION (200.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 #define DEFAULT_Y_ACCELERATION (200.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 #define DEFAULT_Z_ACCELERATION (200.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 + #define DEFAULT_A_ACCELERATION (100.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 + #define DEFAULT_B_ACCELERATION (100.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 + #define DEFAULT_C_ACCELERATION (100.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 + #define DEFAULT_X_MAX_TRAVEL 300.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 300.0 // mm NOTE: Must be a positive value. + #define DEFAULT_A_MAX_TRAVEL 100.0 // mm NOTE: Must be a positive value. + #define DEFAULT_B_MAX_TRAVEL 100.0 // mm NOTE: Must be a positive value. + #define DEFAULT_C_MAX_TRAVEL 100.0 // mm NOTE: Must be a positive value. #endif diff --git a/Grbl_Esp32/gcode.cpp b/Grbl_Esp32/gcode.cpp index fbcde282..d3dfa679 100644 --- a/Grbl_Esp32/gcode.cpp +++ b/Grbl_Esp32/gcode.cpp @@ -449,7 +449,7 @@ uint8_t gc_execute_line(char *line, uint8_t client) FAIL(STATUS_GCODE_MAX_VALUE_EXCEEDED); } grbl_sendf(CLIENT_ALL, "[MSG:Tool No: %d]\r\n", int_value); - gc_block.values.t = int_value; + gc_state.tool = int_value; break; case 'X': word_bit = WORD_X; @@ -1180,8 +1180,8 @@ uint8_t gc_execute_line(char *line, uint8_t client) pl_data->spindle_speed = gc_state.spindle_speed; // Record data for planner use. } // else { pl_data->spindle_speed = 0.0; } // Initialized as zero already. - // [5. Select tool ]: NOT SUPPORTED. Only tracks tool value. - gc_state.tool = gc_block.values.t; + // [5. Select tool ]: NOT SUPPORTED. Only tracks tool value. + // gc_state.tool = gc_block.values.t; // [6. Change tool ]: NOT SUPPORTED @@ -1270,7 +1270,7 @@ uint8_t gc_execute_line(char *line, uint8_t client) // and absolute and incremental modes. pl_data->condition |= PL_COND_FLAG_RAPID_MOTION; // Set rapid motion condition flag. if (axis_command) { - mc_line(gc_block.values.xyz, pl_data); + mc_line(gc_block.values.xyz, pl_data); // kinematics kinematics not used for homing righ now } mc_line(gc_block.values.ijk, pl_data); memcpy(gc_state.position, gc_block.values.ijk, N_AXIS*sizeof(float)); @@ -1300,10 +1300,12 @@ uint8_t gc_execute_line(char *line, uint8_t client) if (axis_command == AXIS_COMMAND_MOTION_MODE) { uint8_t gc_update_pos = GC_UPDATE_POS_TARGET; if (gc_state.modal.motion == MOTION_MODE_LINEAR) { - mc_line(gc_block.values.xyz, pl_data); + //mc_line(gc_block.values.xyz, pl_data); + mc_line_kins(gc_block.values.xyz, pl_data, gc_state.position); } else if (gc_state.modal.motion == MOTION_MODE_SEEK) { pl_data->condition |= PL_COND_FLAG_RAPID_MOTION; // Set rapid motion condition flag. - mc_line(gc_block.values.xyz, pl_data); + //mc_line(gc_block.values.xyz, pl_data); + mc_line_kins(gc_block.values.xyz, pl_data, gc_state.position); } else if ((gc_state.modal.motion == MOTION_MODE_CW_ARC) || (gc_state.modal.motion == MOTION_MODE_CCW_ARC)) { mc_arc(gc_block.values.xyz, pl_data, gc_state.position, gc_block.values.ijk, gc_block.values.r, axis_0, axis_1, axis_linear, bit_istrue(gc_parser_flags,GC_PARSER_ARC_IS_CLOCKWISE)); diff --git a/Grbl_Esp32/grbl_limits.cpp b/Grbl_Esp32/grbl_limits.cpp index 250db8a9..f6a100c6 100644 --- a/Grbl_Esp32/grbl_limits.cpp +++ b/Grbl_Esp32/grbl_limits.cpp @@ -280,6 +280,15 @@ void limits_init() #ifdef Z_LIMIT_PIN pinMode(Z_LIMIT_PIN, INPUT_PULLUP); #endif + #ifdef A_LIMIT_PIN + pinMode(A_LIMIT_PIN, INPUT_PULLUP); + #endif + #ifdef B_LIMIT_PIN + pinMode(B_LIMIT_PIN, INPUT_PULLUP); + #endif + #ifdef C_LIMIT_PIN + pinMode(C_LIMIT_PIN, INPUT_PULLUP); + #endif #else #ifdef X_LIMIT_PIN pinMode(X_LIMIT_PIN, INPUT); // input no pullup @@ -290,6 +299,15 @@ void limits_init() #ifdef Z_LIMIT_PIN pinMode(Z_LIMIT_PIN, INPUT); #endif + #ifdef A_LIMIT_PIN + pinMode(A_LIMIT_PIN, INPUT); // input no pullup + #endif + #ifdef B_LIMIT_PIN + pinMode(B_LIMIT_PIN, INPUT); + #endif + #ifdef C_LIMIT_PIN + pinMode(C_LIMIT_PIN, INPUT); + #endif #endif if (bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)) { @@ -303,6 +321,15 @@ void limits_init() #ifdef Z_LIMIT_PIN attachInterrupt(digitalPinToInterrupt(Z_LIMIT_PIN), isr_limit_switches, CHANGE); #endif + #ifdef A_LIMIT_PIN + attachInterrupt(digitalPinToInterrupt(A_LIMIT_PIN), isr_limit_switches, CHANGE); + #endif + #ifdef B_LIMIT_PIN + attachInterrupt(digitalPinToInterrupt(B_LIMIT_PIN), isr_limit_switches, CHANGE); + #endif + #ifdef C_LIMIT_PIN + attachInterrupt(digitalPinToInterrupt(C_LIMIT_PIN), isr_limit_switches, CHANGE); + #endif } else { limits_disable(); } @@ -327,6 +354,9 @@ void limits_disable() detachInterrupt(X_LIMIT_BIT); detachInterrupt(Y_LIMIT_BIT); detachInterrupt(Z_LIMIT_BIT); + detachInterrupt(A_LIMIT_BIT); + detachInterrupt(B_LIMIT_BIT); + detachInterrupt(C_LIMIT_BIT); } @@ -349,6 +379,20 @@ uint8_t limits_get_state() #ifdef Z_LIMIT_PIN pin += (digitalRead(Z_LIMIT_PIN) << Z_AXIS); #endif + + #ifdef A_LIMIT_PIN + pin += (digitalRead(A_LIMIT_PIN) << A_AXIS); + #endif + + #ifdef B_LIMIT_PIN + pin += (digitalRead(B_LIMIT_PIN) << B_AXIS); + #endif + + #ifdef C_LIMIT_PIN + pin += (digitalRead(C_LIMIT_PIN) << C_AXIS); + #endif + + //grbl_sendf(CLIENT_SERIAL, "[MSG: Limit pins %d]\r\n", pin); #ifdef INVERT_LIMIT_PIN_MASK // not normally used..unless you have both normal and inverted switches pin ^= INVERT_LIMIT_PIN_MASK; @@ -357,14 +401,20 @@ uint8_t limits_get_state() if (bit_istrue(settings.flags,BITFLAG_INVERT_LIMIT_PINS)) { pin ^= LIMIT_MASK; } + + //grbl_sendf(CLIENT_SERIAL, "[MSG: Limit Inverted %d]\r\n", pin); if (pin) { uint8_t idx; for (idx=0; idx. + + Inverse kinematics determine the joint parameters required to get to a position + in 3D space. Grbl will still work as 3 axes of steps, but these steps could + represent angles, etc instead of linear units. + + Unless forward kinematics are applied to the reporting, Grbl will report raw joint + values instead of the normal Cartesian positions + + How it works... + + If you tell it to go to X10 Y10 Z10 in Cartesian space, for example, the equations + will convert those values to the required joint values. In the case of a polar machine, X represents the radius, + Y represents the polar degrees and Z would be unchanged. + + In most cases, a straight line in Cartesian space could cause a curve in the new system. + To fix this, the line is broken into very small segments and each segment is converted + to the new space. While each segment is also distorted, the amount is so small it cannot be seen. + + This segmentation is how normal Grbl draws arcs. + + Feed Rate + + Feed rate is given in steps/time. Due to the new coordinate units and non linearity issues, the + feed rate may need to be adjusted. The ratio of the step distances in the original coordinate system + determined and applied to the feed rate. + + TODO: + Add y offset, for completeness + Add ZERO_NON_HOMED_AXES option + + +*/ +#include "grbl.h" + +#ifdef USE_KINEMATICS + +void inverse_kinematics(float *target, plan_line_data_t *pl_data, float *position) +{ + // use the current tool as a flag for converting for different machine mode. + + // target: This is the target location for the move in machine space millimeters. + // position: This is the previous position in machine space millimeters + + float offsets[N_AXIS]; // current work offsets + float new_system[N_AXIS]; // target location in the new system + uint8_t index; + + // skip kinematics... not implemented yet. + mc_line(target, pl_data); + return; + + // All data is in machine space and axes have different offsets, so we need to apply the offset in corrections + for (index = 0; index < N_AXIS; index++) { + offsets[index] = gc_state.coord_system[index] + gc_state.coord_offset[index]; // offset from machine coordinate system + } + + switch (gc_state.tool) { + case 1: // XYZ only ignore all ABC + + break; + case 2: // Convert XYZ to ABC (no motion on ABC) + + break; + default: // tool 0 or above 2 uses normal 6 axis code...no changes + + break; + } + + +} + +#endif \ No newline at end of file diff --git a/Grbl_Esp32/kinematics.h b/Grbl_Esp32/kinematics.h new file mode 100644 index 00000000..bb386b99 --- /dev/null +++ b/Grbl_Esp32/kinematics.h @@ -0,0 +1,27 @@ +/* + kinematics.h - Implements simple kinematics for Grbl_ESP32 + Part of Grbl_ESP32 + + Copyright (c) 2019 Barton Dring @buildlog + + + Grbl 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. If not, see . +*/ + +#ifndef kinematics_h + #define kinematics_h + +void inverse_kinematics(float *target, plan_line_data_t *pl_data, float *position); + +#endif \ No newline at end of file diff --git a/Grbl_Esp32/motion_control.cpp b/Grbl_Esp32/motion_control.cpp index da1e9fb5..709824ce 100644 --- a/Grbl_Esp32/motion_control.cpp +++ b/Grbl_Esp32/motion_control.cpp @@ -26,6 +26,18 @@ uint8_t ganged_mode = SQUARING_MODE_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 + //grbl_sendf(CLIENT_SERIAL, "[MSG:Kin Tool %d]\r\n", gc_state.tool); + 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. diff --git a/Grbl_Esp32/motion_control.h b/Grbl_Esp32/motion_control.h index 2c2d344d..dc04a439 100644 --- a/Grbl_Esp32/motion_control.h +++ b/Grbl_Esp32/motion_control.h @@ -46,6 +46,7 @@ // 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); // Execute an arc in offset mode format. position == current xyz, target == target xyz, diff --git a/Grbl_Esp32/nuts_bolts.h b/Grbl_Esp32/nuts_bolts.h index 4eef32f9..c60c749b 100644 --- a/Grbl_Esp32/nuts_bolts.h +++ b/Grbl_Esp32/nuts_bolts.h @@ -27,11 +27,16 @@ #define SOME_LARGE_VALUE 1.0E+38 // Axis array index values. Must start with 0 and be continuous. -#define N_AXIS 3 // Number of axes +// Note: You set the number of axes used by changing N_AXIS. +// Be sure to define pins or servos in cpu_map.h +#define N_AXIS 6 // Number of axes defined #define X_AXIS 0 // Axis indexing value. #define Y_AXIS 1 #define Z_AXIS 2 -// #define A_AXIS 3 +#define A_AXIS 3 +#define B_AXIS 4 +#define C_AXIS 5 + // CoreXY motor assignments. DO NOT ALTER. // NOTE: If the A and B motor axis bindings are changed, this effects the CoreXY equations. diff --git a/Grbl_Esp32/report.cpp b/Grbl_Esp32/report.cpp index 798ea668..b665092c 100644 --- a/Grbl_Esp32/report.cpp +++ b/Grbl_Esp32/report.cpp @@ -349,8 +349,8 @@ void report_probe_parameters(uint8_t client) { // Report in terms of machine position. float print_position[N_AXIS]; - char probe_rpt[50]; // the probe report we are building here - char temp[30]; + char probe_rpt[100]; // the probe report we are building here + char temp[60]; strcpy(probe_rpt, "[PRB:"); // initialize the string with the first characters @@ -374,8 +374,8 @@ void report_ngc_parameters(uint8_t client) { float coord_data[N_AXIS]; uint8_t coord_select; - char temp[50]; - char ngc_rpt[400]; + char temp[60]; + char ngc_rpt[500]; ngc_rpt[0] = '\0'; @@ -414,7 +414,7 @@ void report_ngc_parameters(uint8_t client) grbl_send(client, ngc_rpt); - report_probe_parameters(client); + report_probe_parameters(client); } diff --git a/Grbl_Esp32/servo_axis.cpp b/Grbl_Esp32/servo_axis.cpp index bb0cd6a0..835c8832 100644 --- a/Grbl_Esp32/servo_axis.cpp +++ b/Grbl_Esp32/servo_axis.cpp @@ -38,9 +38,19 @@ static TaskHandle_t servosSyncTaskHandle = 0; ServoAxis Z_Servo_Axis(Z_AXIS, SERVO_Z_PIN, SERVO_Z_CHANNEL_NUM); #endif +#ifdef SERVO_A_PIN + ServoAxis A_Servo_Axis(A_AXIS, SERVO_A_PIN, SERVO_A_CHANNEL_NUM); +#endif +#ifdef SERVO_B_PIN + ServoAxis B_Servo_Axis(B_AXIS, SERVO_B_PIN, SERVO_B_CHANNEL_NUM); +#endif +#ifdef SERVO_C_PIN + ServoAxis C_Servo_Axis(C_AXIS, SERVO_C_PIN, SERVO_C_CHANNEL_NUM); +#endif + void init_servos() { - grbl_send(CLIENT_SERIAL, "[MSG: Init Servos]\r\n"); + //grbl_send(CLIENT_SERIAL, "[MSG: Init Servos]\r\n"); #ifdef SERVO_X_PIN grbl_send(CLIENT_SERIAL, "[MSG: Init X Servo]\r\n"); X_Servo_Axis.init(); @@ -61,6 +71,28 @@ void init_servos() Z_Servo_Axis.set_homing_type(SERVO_HOMING_TARGET); Z_Servo_Axis.set_homing_position(SERVO_Z_RANGE_MAX); #endif + + #ifdef SERVO_A_PIN + grbl_send(CLIENT_SERIAL, "[MSG: Init A Servo]\r\n"); + A_Servo_Axis.init(); + A_Servo_Axis.set_range(SERVO_A_RANGE_MIN, SERVO_A_RANGE_MAX); + A_Servo_Axis.set_homing_type(SERVO_HOMING_OFF); + A_Servo_Axis.set_disable_on_alarm(false); + A_Servo_Axis.set_disable_with_steppers(false); + #endif + #ifdef SERVO_B_PIN + grbl_send(CLIENT_SERIAL, "[MSG: Init B Servo]\r\n"); + B_Servo_Axis.init(); + B_Servo_Axis.set_range(SERVO_B_RANGE_MIN, SERVO_B_RANGE_MAX); + #endif + #ifdef SERVO_C_PIN + grbl_send(CLIENT_SERIAL, "[MSG: Init C Servo]\r\n"); + C_Servo_Axis.init(); + C_Servo_Axis.set_range(SERVO_C_RANGE_MIN, SERVO_C_RANGE_MAX); + C_Servo_Axis.set_homing_type(SERVO_HOMING_TARGET); + C_Servo_Axis.set_homing_position(SERVO_C_RANGE_MAX); + #endif + // setup a task that will calculate the determine and set the servo positions xTaskCreatePinnedToCore( servosSyncTask, // task @@ -93,6 +125,16 @@ void servosSyncTask(void *pvParameters) #ifdef SERVO_Z_PIN Z_Servo_Axis.set_location(); #endif + #ifdef SERVO_A_PIN + A_Servo_Axis.set_location(); + #endif + #ifdef SERVO_B_PIN + B_Servo_Axis.set_location(); + #endif + #ifdef SERVO_C_PIN + C_Servo_Axis.set_location(); + #endif + vTaskDelayUntil(&xLastWakeTime, xServoFrequency); } } @@ -104,10 +146,12 @@ ServoAxis::ServoAxis(uint8_t axis, uint8_t pin_num, uint8_t channel_num) // cons _axis = axis; _pin_num = pin_num; _channel_num = channel_num; + _showError = true; // this will be used to show error only once } void ServoAxis::init() { + _cal_is_valid(); ledcSetup(_channel_num, _pwm_freq, _pwm_resolution_bits); ledcAttachPin(_pin_num, _channel_num); disable(); @@ -207,10 +251,9 @@ void ServoAxis::disable() bool ServoAxis::_cal_is_valid() { bool settingsOK = true; - static bool showError = true; // this will be used to show error only once - + if ( (settings.steps_per_mm[_axis] < SERVO_CAL_MIN) || (settings.steps_per_mm[_axis] > SERVO_CAL_MAX) ) { - if (showError) { + if (_showError) { grbl_sendf(CLIENT_SERIAL, "[MSG:Servo cal ($10%d) Error: %4.4f s/b between %.2f and %.2f]\r\n", _axis, settings.steps_per_mm[_axis], SERVO_CAL_MIN, SERVO_CAL_MAX); } settingsOK = false; @@ -218,13 +261,13 @@ bool ServoAxis::_cal_is_valid() // Note: Max travel is set positive via $$, but stored as a negative number if ( (settings.max_travel[_axis] < -SERVO_CAL_MAX) || (settings.max_travel[_axis] > -SERVO_CAL_MIN) ) { - if (showError) { + if (_showError) { grbl_sendf(CLIENT_SERIAL, "[MSG:Servo cal ($13%d) Error: %4.4f s/b between %.2f and %.2f]\r\n", _axis, -settings.max_travel[_axis], SERVO_CAL_MIN, SERVO_CAL_MAX); } settingsOK = false; } - //showError = false; // to show error once + _showError = false; // to show error once return settingsOK; } diff --git a/Grbl_Esp32/servo_axis.h b/Grbl_Esp32/servo_axis.h index 3c50028d..ce689711 100644 --- a/Grbl_Esp32/servo_axis.h +++ b/Grbl_Esp32/servo_axis.h @@ -108,6 +108,8 @@ class ServoAxis{ int _axis; // these should be assign in constructor using Grbl X_AXIS type values int _pin_num; // The GPIO pin being used int _channel_num; // The PWM channel + bool _showError; + uint32_t _pwm_freq = SERVO_PULSE_FREQ; uint32_t _pwm_resolution_bits = SERVO_PULSE_RES_BITS; float _pulse_min = SERVO_MIN_PULSE; // in pwm counts @@ -125,6 +127,7 @@ class ServoAxis{ bool _validate_cal_settings(); void _write_pwm(uint32_t duty); bool _cal_is_valid(); // checks to see if calibration values are in acceptable range + }; #endif diff --git a/Grbl_Esp32/settings.cpp b/Grbl_Esp32/settings.cpp index 375195c8..557fc2ce 100644 --- a/Grbl_Esp32/settings.cpp +++ b/Grbl_Esp32/settings.cpp @@ -100,6 +100,29 @@ void settings_restore(uint8_t restore_flag) { settings.max_travel[X_AXIS] = (-DEFAULT_X_MAX_TRAVEL); settings.max_travel[Y_AXIS] = (-DEFAULT_Y_MAX_TRAVEL); settings.max_travel[Z_AXIS] = (-DEFAULT_Z_MAX_TRAVEL); + + #if N_AXIS > A_AXIS + settings.steps_per_mm[A_AXIS] = DEFAULT_A_STEPS_PER_MM; + settings.max_rate[A_AXIS] = DEFAULT_A_MAX_RATE; + settings.acceleration[A_AXIS] = DEFAULT_A_ACCELERATION; + settings.max_travel[A_AXIS] = (-DEFAULT_A_MAX_TRAVEL); + #endif + + #if N_AXIS > B_AXIS + settings.steps_per_mm[B_AXIS] = DEFAULT_B_STEPS_PER_MM; + settings.max_rate[B_AXIS] = DEFAULT_B_MAX_RATE; + settings.acceleration[B_AXIS] = DEFAULT_B_ACCELERATION; + settings.max_travel[B_AXIS] = (-DEFAULT_B_MAX_TRAVEL); + #endif + + #if N_AXIS > C_AXIS + settings.steps_per_mm[C_AXIS] = DEFAULT_C_STEPS_PER_MM; + settings.max_rate[C_AXIS] = DEFAULT_C_MAX_RATE; + settings.acceleration[C_AXIS] = DEFAULT_C_ACCELERATION; + settings.max_travel[C_AXIS] = (-DEFAULT_C_MAX_TRAVEL); + #endif + + write_global_settings(); } diff --git a/Grbl_Esp32/stepper.cpp b/Grbl_Esp32/stepper.cpp index 31a53df6..7bc6b9c4 100644 --- a/Grbl_Esp32/stepper.cpp +++ b/Grbl_Esp32/stepper.cpp @@ -67,7 +67,17 @@ typedef struct { // Used by the bresenham line algorithm uint32_t counter_x, // Counter variables for the bresenham line tracer counter_y, - counter_z; + counter_z + #ifdef A_AXIS + , counter_a + #endif + #ifdef B_AXIS + , counter_b + #endif + #ifdef C_AXIS + , counter_c + #endif + ; #ifdef STEP_PULSE_DELAY uint8_t step_bits; // Stores out_bits output to complete the step pulse delay #endif @@ -198,58 +208,6 @@ static st_prep_t prep; */ - -#ifdef USE_RMT_STEPS -// Set stepper pulse output pins -inline IRAM_ATTR static void stepperRMT_Outputs() -{ - -#ifdef X_STEP_PIN - if(st.step_outbits & (1<step_event_count >> 1); + // TODO ABC } st.dir_outbits = st.exec_block->direction_bits ^ settings.dir_invert_mask; @@ -302,6 +261,17 @@ void IRAM_ATTR onStepperDriverTimer(void *para) // ISR It is time to take a ste st.steps[X_AXIS] = st.exec_block->steps[X_AXIS] >> st.exec_segment->amass_level; st.steps[Y_AXIS] = st.exec_block->steps[Y_AXIS] >> st.exec_segment->amass_level; st.steps[Z_AXIS] = st.exec_block->steps[Z_AXIS] >> st.exec_segment->amass_level; + + #ifdef A_AXIS + st.steps[A_AXIS] = st.exec_block->steps[A_AXIS] >> st.exec_segment->amass_level; + #endif + #ifdef B_AXIS + st.steps[B_AXIS] = st.exec_block->steps[B_AXIS] >> st.exec_segment->amass_level; + #endif + #ifdef C_AXIS + st.steps[C_AXIS] = st.exec_block->steps[C_AXIS] >> st.exec_segment->amass_level; + #endif + #endif #ifdef VARIABLE_SPINDLE @@ -380,6 +350,49 @@ void IRAM_ATTR onStepperDriverTimer(void *para) // ISR It is time to take a ste sys_position[Z_AXIS]++; } } + +#if N_AXIS > A_AXIS + #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + st.counter_a += st.steps[A_AXIS]; + #else + st.counter_a += st.exec_block->steps[A_AXIS]; + #endif + if (st.counter_a > st.exec_block->step_event_count) { + st.step_outbits |= (1<step_event_count; + if (st.exec_block->direction_bits & (1< B_AXIS + #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + st.counter_b += st.steps[B_AXIS]; + #else + st.counter_b += st.exec_block->steps[B_AXIS]; + #endif + if (st.counter_b > st.exec_block->step_event_count) { + st.step_outbits |= (1<step_event_count; + if (st.exec_block->direction_bits & (1< C_AXIS + #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + st.counter_c += st.steps[C_AXIS]; + #else + st.counter_c += st.exec_block->steps[C_AXIS]; + #endif + if (st.counter_c > st.exec_block->step_event_count) { + st.step_outbits |= (1<step_event_count; + if (st.exec_block->direction_bits & (1<