Minimal probing cycle working. Supports both G38.2 for error and G38.3 when no errors are desired.
This commit is contained in:
parent
1fd45791a5
commit
0a46dfe0b9
@ -107,10 +107,11 @@
|
|||||||
#define PIN_RESET 0 // Uno Analog Pin 0
|
#define PIN_RESET 0 // Uno Analog Pin 0
|
||||||
#define PIN_FEED_HOLD 1 // Uno Analog Pin 1
|
#define PIN_FEED_HOLD 1 // Uno Analog Pin 1
|
||||||
#define PIN_CYCLE_START 2 // Uno Analog Pin 2
|
#define PIN_CYCLE_START 2 // Uno Analog Pin 2
|
||||||
|
#define PIN_PROBE 5 // Uno Analog Pin 5
|
||||||
#define PINOUT_INT PCIE1 // Pin change interrupt enable pin
|
#define PINOUT_INT PCIE1 // Pin change interrupt enable pin
|
||||||
#define PINOUT_INT_vect PCINT1_vect
|
#define PINOUT_INT_vect PCINT1_vect
|
||||||
#define PINOUT_PCMSK PCMSK1 // Pin change interrupt register
|
#define PINOUT_PCMSK PCMSK1 // Pin change interrupt register
|
||||||
#define PINOUT_MASK ((1<<PIN_RESET)|(1<<PIN_FEED_HOLD)|(1<<PIN_CYCLE_START))
|
#define PINOUT_MASK ((1<<PIN_RESET)|(1<<PIN_FEED_HOLD)|(1<<PIN_CYCLE_START)|(1<<PIN_PROBE))
|
||||||
|
|
||||||
#ifdef VARIABLE_SPINDLE
|
#ifdef VARIABLE_SPINDLE
|
||||||
// Advanced Configuration Below You should not need to touch these variables
|
// Advanced Configuration Below You should not need to touch these variables
|
||||||
|
26
gcode.c
26
gcode.c
@ -147,6 +147,14 @@ uint8_t gc_execute_line(char *line)
|
|||||||
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
|
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
case 38:
|
||||||
|
int_value = trunc(10*value); // Multiply by 10 to pick up Gxx.1
|
||||||
|
switch(int_value) {
|
||||||
|
case 382: non_modal_action = NON_MODAL_PROBE_WITH_ERROR; break;
|
||||||
|
case 383: non_modal_action = NON_MODAL_PROBE_NO_ERROR; break;
|
||||||
|
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
|
||||||
|
}
|
||||||
|
break;
|
||||||
case 53: absolute_override = true; break;
|
case 53: absolute_override = true; break;
|
||||||
case 54: case 55: case 56: case 57: case 58: case 59:
|
case 54: case 55: case 56: case 57: case 58: case 59:
|
||||||
gc.coord_select = int_value-54;
|
gc.coord_select = int_value-54;
|
||||||
@ -344,6 +352,24 @@ uint8_t gc_execute_line(char *line)
|
|||||||
memcpy(gc.position, coord_data, sizeof(coord_data)); // gc.position[] = coord_data[];
|
memcpy(gc.position, coord_data, sizeof(coord_data)); // gc.position[] = coord_data[];
|
||||||
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
|
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
|
||||||
break;
|
break;
|
||||||
|
case NON_MODAL_PROBE_WITH_ERROR:
|
||||||
|
if (!axis_words) { // No axis words
|
||||||
|
FAIL(STATUS_INVALID_STATEMENT);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
if(mc_probe_cycle(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, line_number)){
|
||||||
|
FAIL(STATUS_PROBE_ERROR);
|
||||||
|
}
|
||||||
|
axis_words = 0;
|
||||||
|
break;
|
||||||
|
case NON_MODAL_PROBE_NO_ERROR:
|
||||||
|
if (!axis_words) { // No axis words
|
||||||
|
FAIL(STATUS_INVALID_STATEMENT);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
mc_probe_cycle(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, line_number);
|
||||||
|
axis_words = 0;
|
||||||
|
break;
|
||||||
case NON_MODAL_SET_HOME_0: case NON_MODAL_SET_HOME_1:
|
case NON_MODAL_SET_HOME_0: case NON_MODAL_SET_HOME_1:
|
||||||
if (non_modal_action == NON_MODAL_SET_HOME_0) {
|
if (non_modal_action == NON_MODAL_SET_HOME_0) {
|
||||||
settings_write_coord_data(SETTING_INDEX_G28,gc.position);
|
settings_write_coord_data(SETTING_INDEX_G28,gc.position);
|
||||||
|
6
gcode.h
6
gcode.h
@ -60,8 +60,10 @@
|
|||||||
#define NON_MODAL_SET_HOME_0 4 // G28.1
|
#define NON_MODAL_SET_HOME_0 4 // G28.1
|
||||||
#define NON_MODAL_GO_HOME_1 5 // G30
|
#define NON_MODAL_GO_HOME_1 5 // G30
|
||||||
#define NON_MODAL_SET_HOME_1 6 // G30.1
|
#define NON_MODAL_SET_HOME_1 6 // G30.1
|
||||||
#define NON_MODAL_SET_COORDINATE_OFFSET 7 // G92
|
#define NON_MODAL_PROBE_WITH_ERROR 7 //G38.2
|
||||||
#define NON_MODAL_RESET_COORDINATE_OFFSET 8 //G92.1
|
#define NON_MODAL_PROBE_NO_ERROR 8 //G38.3
|
||||||
|
#define NON_MODAL_SET_COORDINATE_OFFSET 9 // G92
|
||||||
|
#define NON_MODAL_RESET_COORDINATE_OFFSET 10 //G92.1
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
uint8_t status_code; // Parser status for current block
|
uint8_t status_code; // Parser status for current block
|
||||||
|
@ -30,6 +30,7 @@
|
|||||||
#include "spindle_control.h"
|
#include "spindle_control.h"
|
||||||
#include "coolant_control.h"
|
#include "coolant_control.h"
|
||||||
#include "limits.h"
|
#include "limits.h"
|
||||||
|
#include "report.h"
|
||||||
|
|
||||||
|
|
||||||
// Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
|
// Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
|
||||||
@ -246,6 +247,102 @@ void mc_homing_cycle()
|
|||||||
limits_init();
|
limits_init();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
uint8_t mc_probe_cycle(float *t, float feed_rate, uint8_t invert_feed_rate, int32_t line_number)
|
||||||
|
{
|
||||||
|
protocol_buffer_synchronize(); //finish all queued commands
|
||||||
|
|
||||||
|
if (sys.abort) { return STATUS_OK; } // Return if system reset has been issued.
|
||||||
|
|
||||||
|
uint8_t i;
|
||||||
|
float target[N_AXIS];
|
||||||
|
|
||||||
|
//copy target position since we'll be modifying it with the probe position on a successful move
|
||||||
|
//The gc_sync_position() at the end may elimiante the need for this. Not sure though.
|
||||||
|
for(i=0; i<N_AXIS; ++i){
|
||||||
|
target[i] = t[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
plan_reset(); // Reset planner buffer to zero planner current position and to clear previous motions.
|
||||||
|
|
||||||
|
// Perform probing cycle. Planner buffer should be empty at this point.
|
||||||
|
// An empty buffer is needed because we need to enable the probe pin along the same move that we're about to execute.
|
||||||
|
|
||||||
|
sys.state = STATE_CYCLE;
|
||||||
|
plan_buffer_line(target, feed_rate, invert_feed_rate, line_number); // Bypass mc_line(). Directly plan homing motion.
|
||||||
|
st_prep_buffer(); // Prep and fill segment buffer from newly planned block.
|
||||||
|
st_wake_up(); // Initiate motion
|
||||||
|
|
||||||
|
sys.probe_state = PROBE_ACTIVE;
|
||||||
|
//TODO - make sure the probe isn't already closed
|
||||||
|
do {
|
||||||
|
|
||||||
|
if( sys.probe_state == PROBE_OFF ){
|
||||||
|
sys.execute |= EXEC_FEED_HOLD;
|
||||||
|
protocol_execute_runtime();
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
protocol_execute_runtime();
|
||||||
|
st_prep_buffer(); // Check and prep segment buffer. NOTE: Should take no longer than 200us.
|
||||||
|
|
||||||
|
if (sys.execute & EXEC_RESET) {
|
||||||
|
sys.probe_state = PROBE_OFF;
|
||||||
|
protocol_execute_runtime();
|
||||||
|
return STATUS_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
//Check for motion ended because switch never triggered
|
||||||
|
if(sys.state != STATE_CYCLE && sys.state != STATE_HOLD){
|
||||||
|
sys.probe_state = PROBE_OFF;
|
||||||
|
report_realtime_status_probe();
|
||||||
|
return STATUS_PROBE_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
} while (1);
|
||||||
|
|
||||||
|
//report_realtime_status(); //debug
|
||||||
|
|
||||||
|
while((sys.execute & EXEC_CYCLE_STOP) == 0 && (sys.state == STATE_CYCLE || sys.state == STATE_HOLD)){
|
||||||
|
protocol_execute_runtime();
|
||||||
|
if (sys.abort) { return STATUS_OK; } // Check for system abort
|
||||||
|
}
|
||||||
|
|
||||||
|
//Prep the new target based on the position that the probe triggered
|
||||||
|
for(i=0; i<N_AXIS; ++i){
|
||||||
|
target[i] = (float)sys.probe_position[i]/settings.steps_per_mm[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
protocol_execute_runtime();
|
||||||
|
|
||||||
|
st_reset(); // Immediately force kill steppers and reset step segment buffer.
|
||||||
|
plan_reset(); // Reset planner buffer. Zero planner positions. Ensure homing motion is cleared.
|
||||||
|
plan_sync_position(); // Sync planner position to current machine position for pull-off move.
|
||||||
|
|
||||||
|
//report_realtime_status(); //debug
|
||||||
|
|
||||||
|
plan_buffer_line(target, feed_rate, invert_feed_rate, line_number); // Bypass mc_line(). Directly plan motion.
|
||||||
|
st_prep_buffer(); // Prep and fill segment buffer from newly planned block.
|
||||||
|
st_wake_up(); // Initiate motion
|
||||||
|
|
||||||
|
protocol_execute_runtime();
|
||||||
|
sys.execute |= EXEC_CYCLE_START;
|
||||||
|
protocol_buffer_synchronize(); // Complete pull-off motion.
|
||||||
|
|
||||||
|
//report_realtime_status(); //debug
|
||||||
|
|
||||||
|
protocol_execute_runtime(); // Check for reset and set system abort.
|
||||||
|
if (sys.abort) { return STATUS_OK; } // Did not complete. Alarm state set by mc_alarm.
|
||||||
|
|
||||||
|
// Gcode parser position was circumvented by the this routine, so sync position now.
|
||||||
|
gc_sync_position();
|
||||||
|
|
||||||
|
// Set idle state after probing completes and before returning to main program.
|
||||||
|
sys.state = STATE_IDLE;
|
||||||
|
st_go_idle();
|
||||||
|
|
||||||
|
//TODO - ouput a mandatory status update with the probe position. What if another was recently sent?
|
||||||
|
report_realtime_status_probe();
|
||||||
|
return STATUS_OK;
|
||||||
|
}
|
||||||
|
|
||||||
// Method to ready the system to reset by setting the runtime reset command and killing any
|
// Method to ready the system to reset by setting the runtime reset command and killing any
|
||||||
// active processes in the system. This also checks if a system reset is issued while Grbl
|
// active processes in the system. This also checks if a system reset is issued while Grbl
|
||||||
|
@ -43,6 +43,11 @@ void mc_dwell(float seconds);
|
|||||||
// Perform homing cycle to locate machine zero. Requires limit switches.
|
// Perform homing cycle to locate machine zero. Requires limit switches.
|
||||||
void mc_homing_cycle();
|
void mc_homing_cycle();
|
||||||
|
|
||||||
|
// Perform tool length probe cycle. Requires probe switch.
|
||||||
|
// Returns STATUS_OK in all cases except when the motion is completed without the probe being triggered.
|
||||||
|
// In that case, it returns a STATUS_PROBE_ERROR
|
||||||
|
uint8_t mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate, int32_t line_number);
|
||||||
|
|
||||||
// Performs system reset. If in motion state, kills all motion and sets system alarm.
|
// Performs system reset. If in motion state, kills all motion and sets system alarm.
|
||||||
void mc_reset();
|
void mc_reset();
|
||||||
|
|
||||||
|
51
report.c
51
report.c
@ -79,6 +79,8 @@ void report_status_message(uint8_t status_code)
|
|||||||
printPgmString(PSTR("Homing not enabled")); break;
|
printPgmString(PSTR("Homing not enabled")); break;
|
||||||
case STATUS_OVERFLOW:
|
case STATUS_OVERFLOW:
|
||||||
printPgmString(PSTR("Line overflow")); break;
|
printPgmString(PSTR("Line overflow")); break;
|
||||||
|
case STATUS_PROBE_ERROR:
|
||||||
|
printPgmString(PSTR("Probe error")); break;
|
||||||
}
|
}
|
||||||
printPgmString(PSTR("\r\n"));
|
printPgmString(PSTR("\r\n"));
|
||||||
}
|
}
|
||||||
@ -364,3 +366,52 @@ void report_realtime_status()
|
|||||||
|
|
||||||
printPgmString(PSTR(">\r\n"));
|
printPgmString(PSTR(">\r\n"));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 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
|
||||||
|
// specific needs. It is kept separate from the "normal" report_realtime_status() to allow customization.
|
||||||
|
void report_realtime_status_probe()
|
||||||
|
{
|
||||||
|
// **Under construction** Bare-bones status report. Provides real-time machine position relative to
|
||||||
|
// the system power on location (0,0,0) and work coordinate position (G54 and G92 applied).
|
||||||
|
uint8_t i;
|
||||||
|
int32_t current_position[N_AXIS]; // Copy current state of the system position variable
|
||||||
|
memcpy(current_position,sys.position,sizeof(sys.position));
|
||||||
|
float print_position[N_AXIS];
|
||||||
|
|
||||||
|
printPgmString(PSTR("<Probe"));
|
||||||
|
|
||||||
|
// Report machine position
|
||||||
|
printPgmString(PSTR(",MPos:"));
|
||||||
|
for (i=0; i< N_AXIS; i++) {
|
||||||
|
print_position[i] = current_position[i]/settings.steps_per_mm[i];
|
||||||
|
if (bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)) { print_position[i] *= INCH_PER_MM; }
|
||||||
|
printFloat(print_position[i]);
|
||||||
|
printPgmString(PSTR(","));
|
||||||
|
}
|
||||||
|
|
||||||
|
// Report work position
|
||||||
|
printPgmString(PSTR("WPos:"));
|
||||||
|
for (i=0; i< N_AXIS; i++) {
|
||||||
|
if (bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)) {
|
||||||
|
print_position[i] -= (gc.coord_system[i]+gc.coord_offset[i])*INCH_PER_MM;
|
||||||
|
} else {
|
||||||
|
print_position[i] -= gc.coord_system[i]+gc.coord_offset[i];
|
||||||
|
}
|
||||||
|
printFloat(print_position[i]);
|
||||||
|
if (i < (N_AXIS-1)) { printPgmString(PSTR(",")); }
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef USE_LINE_NUMBERS
|
||||||
|
// Report current line number
|
||||||
|
printPgmString(PSTR(",Ln:"));
|
||||||
|
int32_t ln=0;
|
||||||
|
plan_block_t * pb = plan_get_current_block();
|
||||||
|
if(pb != NULL) {
|
||||||
|
ln = pb->line_number;
|
||||||
|
}
|
||||||
|
printInteger(ln);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
printPgmString(PSTR(">\r\n"));
|
||||||
|
}
|
||||||
|
6
report.h
6
report.h
@ -36,6 +36,7 @@
|
|||||||
#define STATUS_ALARM_LOCK 12
|
#define STATUS_ALARM_LOCK 12
|
||||||
#define STATUS_SOFT_LIMIT_ERROR 13
|
#define STATUS_SOFT_LIMIT_ERROR 13
|
||||||
#define STATUS_OVERFLOW 14
|
#define STATUS_OVERFLOW 14
|
||||||
|
#define STATUS_PROBE_ERROR 15
|
||||||
|
|
||||||
// Define Grbl alarm codes. Less than zero to distinguish alarm error from status error.
|
// Define Grbl alarm codes. Less than zero to distinguish alarm error from status error.
|
||||||
#define ALARM_LIMIT_ERROR -1
|
#define ALARM_LIMIT_ERROR -1
|
||||||
@ -69,6 +70,11 @@ void report_grbl_settings();
|
|||||||
// Prints realtime status report
|
// Prints realtime status report
|
||||||
void report_realtime_status();
|
void report_realtime_status();
|
||||||
|
|
||||||
|
// Prints realtime position status report at the end of a probe cycle
|
||||||
|
// This is in leiu of saving the probe position to internal variables like an
|
||||||
|
// EMC machine
|
||||||
|
void report_realtime_status_probe();
|
||||||
|
|
||||||
// Prints Grbl persistent coordinate parameters
|
// Prints Grbl persistent coordinate parameters
|
||||||
void report_gcode_parameters();
|
void report_gcode_parameters();
|
||||||
|
|
||||||
|
@ -282,6 +282,12 @@ ISR(TIMER1_COMPA_vect)
|
|||||||
{
|
{
|
||||||
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR
|
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR
|
||||||
if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
|
if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
|
||||||
|
|
||||||
|
//Check if we need to copy the current position to the probe_position
|
||||||
|
if(sys.probe_state == PROBE_COPY_POSITION){
|
||||||
|
sys.probe_state = PROBE_OFF;
|
||||||
|
memcpy(sys.probe_position, sys.position, sizeof(float)*N_AXIS);
|
||||||
|
}
|
||||||
|
|
||||||
// Set the direction pins a couple of nanoseconds before we step the steppers
|
// Set the direction pins a couple of nanoseconds before we step the steppers
|
||||||
DIRECTION_PORT = (DIRECTION_PORT & ~DIRECTION_MASK) | (st.dir_outbits & DIRECTION_MASK);
|
DIRECTION_PORT = (DIRECTION_PORT & ~DIRECTION_MASK) | (st.dir_outbits & DIRECTION_MASK);
|
||||||
|
5
system.c
5
system.c
@ -49,6 +49,11 @@ ISR(PINOUT_INT_vect)
|
|||||||
sys.execute |= EXEC_FEED_HOLD;
|
sys.execute |= EXEC_FEED_HOLD;
|
||||||
} else if (bit_isfalse(PINOUT_PIN,bit(PIN_CYCLE_START))) {
|
} else if (bit_isfalse(PINOUT_PIN,bit(PIN_CYCLE_START))) {
|
||||||
sys.execute |= EXEC_CYCLE_START;
|
sys.execute |= EXEC_CYCLE_START;
|
||||||
|
} else if (bit_isfalse(PINOUT_PIN,bit(PIN_PROBE))) {
|
||||||
|
if(sys.probe_state == PROBE_ACTIVE){
|
||||||
|
sys.probe_state = PROBE_COPY_POSITION;
|
||||||
|
//sys.execute |= EXEC_FEED_HOLD; //Probably OK to call a feedhold here. I'd prefer to do it in the main probe loop for now
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
9
system.h
9
system.h
@ -67,6 +67,13 @@
|
|||||||
#define STATE_HOLD bit(5) // Executing feed hold
|
#define STATE_HOLD bit(5) // Executing feed hold
|
||||||
// #define STATE_JOG bit(6) // Jogging mode is unique like homing.
|
// #define STATE_JOG bit(6) // Jogging mode is unique like homing.
|
||||||
|
|
||||||
|
// Values that define the probing state machine.
|
||||||
|
#define PROBE_OFF 0 //No probing
|
||||||
|
#define PROBE_ACTIVE 1 //Actively watching the input pin. If it is triggered, the stante is changed to PROBE_COPY_POSITION
|
||||||
|
#define PROBE_COPY_POSITION 2 //In this state, the current position will be copied to probe_position in the stepper ISR. State is then changed to PROBE_OFF.
|
||||||
|
//Copying to a separate set of variables ensures that no race condition can occur if the ISR updates the main position variables
|
||||||
|
//while the probing routine is copying them.
|
||||||
|
|
||||||
// Define global system variables
|
// Define global system variables
|
||||||
typedef struct {
|
typedef struct {
|
||||||
uint8_t abort; // System abort flag. Forces exit back to main loop for reset.
|
uint8_t abort; // System abort flag. Forces exit back to main loop for reset.
|
||||||
@ -76,6 +83,8 @@ typedef struct {
|
|||||||
int32_t position[N_AXIS]; // Real-time machine (aka home) position vector in steps.
|
int32_t position[N_AXIS]; // Real-time machine (aka home) position vector in steps.
|
||||||
// NOTE: This may need to be a volatile variable, if problems arise.
|
// NOTE: This may need to be a volatile variable, if problems arise.
|
||||||
uint8_t auto_start; // Planner auto-start flag. Toggled off during feed hold. Defaulted by settings.
|
uint8_t auto_start; // Planner auto-start flag. Toggled off during feed hold. Defaulted by settings.
|
||||||
|
uint8_t probe_state; // Probing state value. Used in the mc_probe_cycle(), the PINOUT_PIN IRT, and the stepper ISR to coordinate the probing cycle.
|
||||||
|
int32_t probe_position[N_AXIS]; // Copy of the position when the probe is triggered that can be read/copied without worring about changes in the middle of a read.
|
||||||
} system_t;
|
} system_t;
|
||||||
extern system_t sys;
|
extern system_t sys;
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user