mirror/Grbl_Esp32
1
0
Fork 0
mirror of https://github.com/bdring/Grbl_Esp32.git synced 2025-09-02 19:02:35 +02:00
Code Issues Releases Wiki Activity

running with state machine, repeatable results in test environment

Browse Source
This commit is contained in:
Jens Hauser
2020-07-29 18:27:45 +02:00
parent 0956184e22
commit ed7ec882d3
4 changed files with 281 additions and 137 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
.vs/Grbl_Esp32_JH/v16/.suo
View File

Binary file not shown.

BIN
.vs/slnx.sqlite
View File

Binary file not shown.

417
Grbl_Esp32/Custom/4axis_xyxz.cpp
View File

@@ -3,44 +3,44 @@
Part of Grbl_ESP32
*/
#define DEBUG 0; // do I want debug messages? yes/no
/*
VARIABLES
*/
uint8_t AmountOfToolChanges; // Each new tool increases this by 1. Before first tool, it<69>s 0.
float ZPosOld, ZPosNew;
uint8_t current_toolNo, new_toolNo;
float firstZPos, newZPos, Zdiff;
static TaskHandle_t zProbeSyncTaskHandle = 0;
// Finite state machine.
uint8_t state; // final state machine
// return current Z machine position
float getCurrentZPos()
{
int32_t current_position[N_AXIS]; // copy of current location
float m_pos[N_AXIS]; // machine position in mm
#define TOOLCHANGE_IDLE 0 // initial state. tool change switched off. set during machine_init()
#define TOOLCHANGE_INIT 1 // do some reporting at first. tool change number x and so on
#define TOOLCHANGE_START 2 // tool change function called by G code "T[1..n] M06". Not triggered by G38.2 or ESP3D probe function :-)
memcpy(current_position, sys_position, sizeof(sys_position)); // get current position in step
system_convert_array_steps_to_mpos(m_pos, sys_position); // convert to millimeters
// first Z probe before tool change
#define TOOLCHANGE_ZPROBE_1a 3 // Z probe #1 (quick). Touch down and order to press the button
#define TOOLCHANGE_ZPROBE_1b 4 // Z probe #1 (quick). Wait for button press.
return m_pos[Z_AXIS];
}
#define TOOLCHANGE_MANUAL 5 // go to tool change position
// return last Z probe machine position
float getLastZProbePos()
{
float m_pos[N_AXIS]; // machine position in mm
char output[200];
// second Z probe after tool change. Now we can compare
#define TOOLCHANGE_ZPROBE_2a 6 // Z probe #2 (slow ). Touch down and order to press the button
#define TOOLCHANGE_ZPROBE_2b 7 // Z probe #2 (slow ). Wait for button press.
//memcpy(current_position, sys_position, sizeof(sys_position)); // get current position in step
system_convert_array_steps_to_mpos(m_pos, sys_probe_position); // convert to millimeters
#define TOOLCHANGE_FINISH 99 // tool change finish. do some reporting, clean up, etc.
return m_pos[Z_AXIS];
}
// declare functions
float getCurrentZPos();
float getLastZProbePos();
float getG54Zvalue();
int8_t getState();
bool checkState(uint8_t checkState);
void setState(int8_t newState);
void setState(int8_t newState, uint8_t newtool);
// return the stored G54/Z value
float getG54Zvalue()
{
float coord_data[N_AXIS];
settings_read_coord_data(0, coord_data); //0=G54
return coord_data[Z_AXIS];
}
#ifdef USE_MACHINE_INIT
/*
@@ -53,49 +53,233 @@ void machine_init()
{
// We start with no tool changes yet
AmountOfToolChanges=0;
ZPosOld=0;
xTaskCreatePinnedToCore(zProbeSyncTask, // task
"zProbeSyncTask", // name for task
4096, // size of task stack
NULL, // parameters
1, // priority
&zProbeSyncTaskHandle,
0 // core
// unknown at the beginning. But this will change, if the first tool is loaded
current_toolNo = 0;
// Initialize state machine
state=TOOLCHANGE_IDLE;
// TODO this task runs permanently. Alternative?
xTaskCreatePinnedToCore(zProbeSyncTask, // task
"zProbeSyncTask", // name for task
4096, // size of task stack
NULL, // parameters
1, // priority
&zProbeSyncTaskHandle, // handle
0 // core
);
}
#endif
void zProbeSyncTask(void* pvParameters) {
int32_t sys_POS_steps[N_AXIS], sys_PROBE_steps[N_AXIS];
float sys_POS[N_AXIS], sys_PROBEPOS[N_AXIS];
float m_pos[N_AXIS]; // machine position in mm
TickType_t xLastWakeTime;
char temp[200];
// return current state machine status
int8_t getState()
{
return state;
}
const TickType_t xProbeFrequency = ZPROBE_TASK_FREQ; // in ticks (typically ms)
// check, if given state is the active
bool checkState(uint8_t checkState)
{
if (state == checkState) return 1;
else return 0;
}
// setnew state machine status
void setState(int8_t newState)
{
state = newState;
}
// setnew state machine status
void setState(int8_t newState, uint8_t newtool)
{
setState(newState);
new_toolNo = newtool;
}
// state machine
void zProbeSyncTask(void* pvParameters)
{
TickType_t xLastWakeTime;
const TickType_t xProbeFrequency = 100; // 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(sys_POS_steps, sys_position, sizeof(sys_position));
memcpy(sys_PROBE_steps, sys_probe_position, sizeof(sys_probe_position));
system_convert_array_steps_to_mpos(sys_POS, sys_POS_steps);
system_convert_array_steps_to_mpos(sys_PROBEPOS, sys_PROBE_steps);
//sprintf(temp, "lprobegestate=%d\r\n", probe_get_state());
//grbl_send(CLIENT_ALL, temp);
if ( (probe_get_state()==1) )
for( ;; )
{
switch ( getState() )
{
grbl_send(CLIENT_ALL, "Button pressed !");
//while running
sprintf(temp, "syspos/Z=%4.3f, probepos/Z=%4.3f\r\n", sys_POS[Z_AXIS], sys_PROBEPOS[Z_AXIS]);
case TOOLCHANGE_INIT:
#ifdef DEBUG
grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_INIT. State=%d\r\n", getState());
#else
grbl_sendf (CLIENT_ALL, "Tool change procedure started.\r\n");
#endif
// TODO set AmountOfToolChanges to 0 after job finish
AmountOfToolChanges++; // Report tool change amount.
#ifdef DEBUG
grbl_sendf (CLIENT_ALL, "This is the %d. tool change in this job\r\n", AmountOfToolChanges);
grbl_sendf (CLIENT_ALL, "Old tool is #%d (0 means unknown), new tool is #%d\r\n", current_toolNo, new_toolNo);
#endif
// Init. Safe start block. G54, XY plane, mm mode, relative addressing mode
inputBuffer.push("G49\r\n");
//inputBuffer.push("G54\r\n");
inputBuffer.push("G53\r\n");
inputBuffer.push("G17 G21 G90\r\n");
inputBuffer.push("G0 F100\r\n");
// Switch off spindle
inputBuffer.push("M05\r\n");
setState (TOOLCHANGE_START);
break;
case TOOLCHANGE_START:
#ifdef DEBUG
grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_START. State=%d\r\n", getState());
#endif
// nur beim ersten Mal die L<>nge des Fr<46>sers messen. Mit dem Wert alle Weiteren vergleichen und G43.2 berechnen.
if (AmountOfToolChanges == 1)
setState(TOOLCHANGE_ZPROBE_1a);
else
setState(TOOLCHANGE_MANUAL);
break;
// First Z Probe
case TOOLCHANGE_ZPROBE_1a:
#ifdef DEBUG
grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_ZPROBE_1a. State=%d\r\n", getState());
#endif
// Place spindle directly above button in X/Y and a few mm above Z
inputBuffer.push("G53 G0 Z-5\r\n");
inputBuffer.push("G53 G0 X-29 Y-410\r\n");
// Z probe, max. 50mm to press button, quick
inputBuffer.push("G91 G38.2 Z-110 F500\r\n");
inputBuffer.push("G4 P1.0\r\n");
setState(TOOLCHANGE_ZPROBE_1b);
break;
case TOOLCHANGE_ZPROBE_1b: // wait for button press
#ifdef DEBUG
// grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_ZPROBE_1b. State=%d\r\n", getState());
#endif
// TODO Error handling. What happens in case the button is not pressed?
if ( probe_get_state() )
{
// TODO neue Tooll<6C>nge immer gegen das erste Tool, richtig?
if (AmountOfToolChanges == 1)
firstZPos = getLastZProbePos(); // save Z pos for comparison later
// hit the probe
#ifdef DEBUG
grbl_sendf(CLIENT_ALL, "Button pressed first time, quickly. Z probe pos=%4.3f\r\n", firstZPos);
#endif
inputBuffer.push("G91 G0 Z2\r\n");
vTaskDelay (xProbeFrequency * 20); // wait until button is released
setState (TOOLCHANGE_MANUAL);
}
break;
// go to manual tool change position
case TOOLCHANGE_MANUAL:
#ifdef DEBUG
grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_MANUAL. State=%d\r\n", getState());
#endif
// Go to tool change position
inputBuffer.push("G53 G0 Z-5\r\n");
inputBuffer.push("G53 G0 X-5 Y-210\r\n");
// Hold
inputBuffer.push("M0\r\n");
setState (TOOLCHANGE_ZPROBE_2a);
break;
case TOOLCHANGE_ZPROBE_2a:
#ifdef DEBUG
grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_ZPROBE_2a. State=%d\r\n", getState());
#endif
// Place spindle directly above button in X/Y and a few mm above Z
inputBuffer.push("G53 G0 Z-5\r\n");
inputBuffer.push("G53 G0 X-29 Y-410\r\n");
// Z probe, max. 50mm to press button, quick
inputBuffer.push("G91 G38.2 Z-110 F500\r\n");
inputBuffer.push("G4 P2.0\r\n");
setState(TOOLCHANGE_ZPROBE_2b);
break;
case TOOLCHANGE_ZPROBE_2b: // wait for button press
#ifdef DEBUG
// grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_ZPROBE_2b. State=%d\r\n", getState());
#endif
// TODO Error handling. What happens in case the button is not pressed?
if ( probe_get_state() )
{
newZPos = getLastZProbePos(); // save Z pos for comparison later
// hit the probe
#ifdef DEBUG
grbl_sendf (CLIENT_ALL, "Button pressed second time, slowly. new Z probe pos=%4.3f\r\n", newZPos);
#endif
// send out G43.1 adjustment
char gcode_line[20];
sprintf(gcode_line, "G54 G43.1 Z%4.3f\r\n", newZPos-firstZPos);
inputBuffer.push(gcode_line);
grbl_sendf (CLIENT_ALL, gcode_line);
inputBuffer.push("G91 G0 Z2\r\n");
setState (TOOLCHANGE_FINISH);
}
break;
// That<61>s it
case TOOLCHANGE_FINISH:
#ifdef DEBUG
grbl_sendf (CLIENT_ALL, "zProbeSyncTask. TOOLCHANGE_FINISH. State=%d\r\n", getState());
#endif
grbl_send (CLIENT_ALL, "Tool change procedure finished.\r\n");
grbl_send (CLIENT_ALL, "Go to G54 after hold.\r\n");
// Hold
//inputBuffer.push("M0\r\n");
// Clean up
//inputBuffer.push("G90\r\n");
// Go to G54
// TODO G54 needs to be set before, otherwise bad things could happen
inputBuffer.push("G53 G0 Z-5\r\n");
inputBuffer.push("G54");
inputBuffer.push("G90");
inputBuffer.push("G0 X0 Y0\r\n");
inputBuffer.push("G0 Z0\r\n");
setState (TOOLCHANGE_IDLE);
break;
//sprintf(temp, "last probe/Z =%4.3f\r\n", ZPosNew);
grbl_send(CLIENT_ALL, temp);
}
vTaskDelayUntil(&xLastWakeTime, xProbeFrequency);
@@ -111,14 +295,7 @@ void zProbeSyncTask(void* pvParameters) {
*/
void user_tool_change(uint8_t new_tool)
{
// Variables
char temp[200];
float value1, value2, diff;
// Start of function
// Increase AmountOfToolChanges every time by 1
AmountOfToolChanges++;
setState (TOOLCHANGE_INIT, new_tool); // let<65>s start with the state machine
/*
Prerequisites
@@ -149,73 +326,41 @@ void user_tool_change(uint8_t new_tool)
- Proceed with job
*/
// Report tool change amount. During first call it<69>s 0.
sprintf(temp, "Tool change #=%d\r\n", AmountOfToolChanges);
grbl_send(CLIENT_ALL, temp);
// Init. Safe start block. G54, XY plane, mm mode, relative addressing mode
inputBuffer.push("G54\r\n");
inputBuffer.push("G17 G21 G91\r\n");
inputBuffer.push("G0 F100\r\n");
protocol_buffer_synchronize();
// Switch off spindle
inputBuffer.push("M05\r\n");
protocol_buffer_synchronize();
// Go to tool change position
inputBuffer.push("G53 G0 Z-5\r\n");
inputBuffer.push("G53 G0 X-5 Y-210\r\n");
protocol_buffer_synchronize();
// Hold
inputBuffer.push("M0\r\n");
protocol_buffer_synchronize();
/*
do some reporting
*/
//current Z pos. Should be, where the button is pressed
sprintf(temp, "(1) z pos current at hold=%4.3f\r\n", getCurrentZPos());
grbl_send(CLIENT_ALL, temp);
// Place spindle directly above button in X/Y and a few mm above Z
inputBuffer.push("G53 G0 X-29 Y-410\r\n");
inputBuffer.push("G53 G0 Z-60 \r\n");
protocol_buffer_synchronize();
// Z probe, max. 50mm to press button
inputBuffer.push("G38.2 Z-50 F250\r\n");
protocol_buffer_synchronize();
// Raise spindle a little bit for slower try to increase accuracy
inputBuffer.push("G0 Z1.5\r\n");
protocol_buffer_synchronize();
// Z probe again
inputBuffer.push("G38.2 Z-2 F30\r\n");
inputBuffer.push("G4 P2.0\r\n");
protocol_buffer_synchronize(); // wait for all previous moves to complete
// Clean up
inputBuffer.push("G90\r\n");
protocol_buffer_synchronize(); // wait for all previous moves to complete
/*
do some reporting
*/
//current Z pos. Should be, where the button is pressed
sprintf(temp, "(2) z pos current=%4.3f\r\n", getCurrentZPos());
grbl_send(CLIENT_ALL, temp);
//Z probe position. Should be identical to getCurrentZPos() here
sprintf(temp, "(3) z probe pos=%4.3f\r\n", getLastZProbePos());
grbl_send(CLIENT_ALL, temp);
//get G54/Z position, as manually configured by jogging. WORKS!
sprintf(temp, "(4) z G54 pos=%4.3f\r\n", getG54Zvalue());
grbl_send(CLIENT_ALL, temp);
return;
}
#endif
#endif
// return current Z machine position
float getCurrentZPos()
{
int32_t current_position[N_AXIS]; // copy of current location
float m_pos[N_AXIS]; // machine position in mm
memcpy(current_position, sys_position, sizeof(sys_position)); // get current position in step
system_convert_array_steps_to_mpos(m_pos, sys_position); // convert to millimeters
return m_pos[Z_AXIS];
}
// return last Z probe machine position
float getLastZProbePos()
{
int32_t lastZPosition[N_AXIS]; // copy of current location
float m_pos[N_AXIS]; // machine position in mm
char output[200];
memcpy(lastZPosition, sys_probe_position, sizeof(sys_probe_position)); // get current position in step
system_convert_array_steps_to_mpos(m_pos, lastZPosition); // convert to millimeters
return m_pos[Z_AXIS];
}
// return the stored G54/Z value
float getG54Zvalue()
{
float coord_data[N_AXIS];
settings_read_coord_data(0, coord_data); //0=G54
return coord_data[Z_AXIS];
}

1
Grbl_Esp32/Machines/4axis_xyxz.h
View File

@@ -4,7 +4,6 @@
*/
#define MACHINE_NAME "MACHINE_ESP32 Jens XYZA"
#define ZPROBE_TASK_FREQ 1000 // this is milliseconds
#ifdef N_AXIS
#undef N_AXIS