mirror of
https://github.com/bdring/Grbl_Esp32.git
synced 2025-09-02 10:53:01 +02:00
Braces (#576)
* Fixed all the braces * Weeded out all `return (...);` with no additional value to the parenthesis. * Fixed coolant control flags Co-authored-by: Stefan de Bruijn <stefan@nubilosoft.com>
This commit is contained in:
@@ -35,14 +35,14 @@ void coolant_init() {
|
||||
|
||||
// Returns current coolant output state. Overrides may alter it from programmed state.
|
||||
CoolantMode coolant_get_state() {
|
||||
CoolantMode cl_state = CoolantMode::Disable;
|
||||
CoolantMode cl_state = {};
|
||||
#ifdef COOLANT_FLOOD_PIN
|
||||
# ifdef INVERT_COOLANT_FLOOD_PIN
|
||||
if (!digitalRead(COOLANT_FLOOD_PIN)) {
|
||||
# else
|
||||
if (digitalRead(COOLANT_FLOOD_PIN)) {
|
||||
# endif
|
||||
cl_state |= CoolantMode::Flood;
|
||||
cl_state.Flood = 1;
|
||||
}
|
||||
#endif
|
||||
#ifdef COOLANT_MIST_PIN
|
||||
@@ -51,10 +51,10 @@ CoolantMode coolant_get_state() {
|
||||
# else
|
||||
if (digitalRead(COOLANT_MIST_PIN)) {
|
||||
# endif
|
||||
cl_state |= CoolantMode::Mist;
|
||||
cl_state.Mist = 1;
|
||||
}
|
||||
#endif
|
||||
return (cl_state);
|
||||
return cl_state;
|
||||
}
|
||||
|
||||
// Directly called by coolant_init(), coolant_set_state(), and mc_reset(), which can be at
|
||||
@@ -81,13 +81,14 @@ void coolant_stop() {
|
||||
// Called by coolant toggle override, parking restore, parking retract, sleep mode, g-code
|
||||
// parser program end, and g-code parser coolant_sync().
|
||||
void coolant_set_state(CoolantMode mode) {
|
||||
if (sys.abort)
|
||||
if (sys.abort) {
|
||||
return; // Block during abort.
|
||||
if (mode == CoolantMode::Disable)
|
||||
}
|
||||
if (mode.IsDisabled()) {
|
||||
coolant_stop();
|
||||
else {
|
||||
} else {
|
||||
#ifdef COOLANT_FLOOD_PIN
|
||||
if (mode & CoolantMode::Enable) {
|
||||
if (mode.Flood) {
|
||||
# ifdef INVERT_COOLANT_FLOOD_PIN
|
||||
digitalWrite(COOLANT_FLOOD_PIN, 0);
|
||||
# else
|
||||
@@ -96,7 +97,7 @@ void coolant_set_state(CoolantMode mode) {
|
||||
}
|
||||
#endif
|
||||
#ifdef COOLANT_MIST_PIN
|
||||
if (mode & CoolantMode::Mist) {
|
||||
if (mode.Mist) {
|
||||
# ifdef INVERT_COOLANT_MIST_PIN
|
||||
digitalWrite(COOLANT_MIST_PIN, 0);
|
||||
# else
|
||||
@@ -111,8 +112,9 @@ void coolant_set_state(CoolantMode mode) {
|
||||
// G-code parser entry-point for setting coolant state. Forces a planner buffer sync and bails
|
||||
// if an abort or check-mode is active.
|
||||
void coolant_sync(CoolantMode mode) {
|
||||
if (sys.state == STATE_CHECK_MODE)
|
||||
if (sys.state == STATE_CHECK_MODE) {
|
||||
return;
|
||||
}
|
||||
protocol_buffer_synchronize(); // Ensure coolant turns on when specified in program.
|
||||
coolant_set_state(mode);
|
||||
}
|
||||
|
@@ -453,12 +453,18 @@ uint8_t gc_execute_line(char* line, uint8_t client) {
|
||||
case 9:
|
||||
switch (int_value) {
|
||||
#ifdef COOLANT_MIST_PIN
|
||||
case 7: gc_block.modal.coolant = CoolantMode::Mist; break;
|
||||
case 7:
|
||||
gc_block.modal.coolant = {};
|
||||
gc_block.modal.coolant.Mist = 1;
|
||||
break;
|
||||
#endif
|
||||
#ifdef COOLANT_FLOOD_PIN
|
||||
case 8: gc_block.modal.coolant = CoolantMode::Flood; break;
|
||||
case 8:
|
||||
gc_block.modal.coolant = {};
|
||||
gc_block.modal.coolant.Flood = 1;
|
||||
break;
|
||||
#endif
|
||||
case 9: gc_block.modal.coolant = CoolantMode::Disable; break;
|
||||
case 9: gc_block.modal.coolant = {}; break;
|
||||
}
|
||||
mg_word_bit = ModalGroup::MM8;
|
||||
break;
|
||||
@@ -1183,7 +1189,7 @@ uint8_t gc_execute_line(char* line, uint8_t client) {
|
||||
if (status == STATUS_OK) {
|
||||
memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_block.values.xyz));
|
||||
}
|
||||
return (status);
|
||||
return status;
|
||||
}
|
||||
// If in laser mode, setup laser power based on current and past parser conditions.
|
||||
if (laser_mode->get()) {
|
||||
@@ -1268,11 +1274,10 @@ uint8_t gc_execute_line(char* line, uint8_t client) {
|
||||
// NOTE: Coolant M-codes are modal. Only one command per line is allowed. But, multiple states
|
||||
// can exist at the same time, while coolant disable clears all states.
|
||||
coolant_sync(gc_block.modal.coolant);
|
||||
if (gc_block.modal.coolant == CoolantMode::Disable) {
|
||||
gc_state.modal.coolant = CoolantMode::Disable;
|
||||
if (gc_block.modal.coolant.IsDisabled()) {
|
||||
gc_state.modal.coolant = {};
|
||||
} else {
|
||||
gc_state.modal.coolant =
|
||||
static_cast<CoolantMode>(static_cast<uint8_t>(gc_state.modal.coolant) | static_cast<uint8_t>(gc_block.modal.coolant));
|
||||
gc_state.modal.coolant = CoolantMode(gc_state.modal.coolant, gc_block.modal.coolant);
|
||||
}
|
||||
}
|
||||
pl_data->coolant = gc_state.modal.coolant; // Set state for planner use.
|
||||
@@ -1448,7 +1453,7 @@ uint8_t gc_execute_line(char* line, uint8_t client) {
|
||||
// gc_state.modal.cutter_comp = CutterComp::Disable; // Not supported.
|
||||
gc_state.modal.coord_select = 0; // G54
|
||||
gc_state.modal.spindle = SpindleState::Disable;
|
||||
gc_state.modal.coolant = CoolantMode::Disable;
|
||||
gc_state.modal.coolant = {};
|
||||
#ifdef ENABLE_PARKING_OVERRIDE_CONTROL
|
||||
# ifdef DEACTIVATE_PARKING_UPON_INIT
|
||||
gc_state.modal.override = Override::Disabled;
|
||||
@@ -1469,7 +1474,7 @@ uint8_t gc_execute_line(char* line, uint8_t client) {
|
||||
}
|
||||
system_flag_wco_change(); // Set to refresh immediately just in case something altered.
|
||||
spindle->set_state(SpindleState::Disable, 0);
|
||||
coolant_set_state(CoolantMode::Disable);
|
||||
coolant_set_state(CoolantMode());
|
||||
}
|
||||
report_feedback_message(MESSAGE_PROGRAM_END);
|
||||
#ifdef USE_M30
|
||||
@@ -1480,7 +1485,7 @@ uint8_t gc_execute_line(char* line, uint8_t client) {
|
||||
gc_state.modal.program_flow = ProgramFlow::Running; // Reset program flow.
|
||||
|
||||
// TODO: % to denote start of program.
|
||||
return (STATUS_OK);
|
||||
return STATUS_OK;
|
||||
}
|
||||
|
||||
/*
|
||||
|
@@ -145,10 +145,18 @@ enum class SpindleState : uint8_t {
|
||||
};
|
||||
|
||||
// Modal Group M8: Coolant control
|
||||
enum class CoolantMode : uint8_t {
|
||||
Disable = 0, // M9 (Default: Must be zero)
|
||||
Flood = 1, // M8
|
||||
Mist = 2, // M7
|
||||
class CoolantMode {
|
||||
public:
|
||||
inline CoolantMode() : Flood(0), Mist(0) {} // M9 (Default: Must be zero)
|
||||
inline CoolantMode(CoolantMode lhs, CoolantMode rhs) : Flood(lhs.Flood | rhs.Flood), Mist(lhs.Mist | rhs.Mist) {}
|
||||
|
||||
uint8_t Flood : 1; // M8
|
||||
uint8_t Mist : 1; // M7
|
||||
|
||||
inline bool IsDisabled() const { return Flood == 0 && Mist == 0; }
|
||||
|
||||
inline bool operator==(const CoolantMode& o) const { return Flood == o.Flood && Mist == o.Mist; }
|
||||
inline bool operator!=(const CoolantMode& o) const { return !operator==(o); }
|
||||
};
|
||||
|
||||
// Modal Group M9: Override control
|
||||
|
@@ -64,8 +64,9 @@ void grbl_init() {
|
||||
// not after disabling the alarm locks. Prevents motion startup blocks from crashing into
|
||||
// things uncontrollably. Very bad.
|
||||
#ifdef HOMING_INIT_LOCK
|
||||
if (homing_enable->get())
|
||||
if (homing_enable->get()) {
|
||||
sys.state = STATE_ALARM;
|
||||
}
|
||||
#endif
|
||||
Spindles::Spindle::select();
|
||||
#ifdef ENABLE_WIFI
|
||||
|
@@ -457,8 +457,9 @@ static void IRAM_ATTR i2s_out_intr_handler(void* arg) {
|
||||
xQueueSendFromISR(o_dma.queue, &finish_desc, &high_priority_task_awoken);
|
||||
}
|
||||
|
||||
if (high_priority_task_awoken == pdTRUE)
|
||||
if (high_priority_task_awoken == pdTRUE) {
|
||||
portYIELD_FROM_ISR();
|
||||
}
|
||||
|
||||
// clear interrupt
|
||||
I2S0.int_clr.val = I2S0.int_st.val; //clear pending interrupt
|
||||
@@ -724,26 +725,30 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) {
|
||||
# ifdef USE_I2S_OUT_STREAM_IMPL
|
||||
// Allocate the array of pointers to the buffers
|
||||
o_dma.buffers = (uint32_t**)malloc(sizeof(uint32_t*) * I2S_OUT_DMABUF_COUNT);
|
||||
if (o_dma.buffers == nullptr)
|
||||
if (o_dma.buffers == nullptr) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Allocate each buffer that can be used by the DMA controller
|
||||
for (int buf_idx = 0; buf_idx < I2S_OUT_DMABUF_COUNT; buf_idx++) {
|
||||
o_dma.buffers[buf_idx] = (uint32_t*)heap_caps_calloc(1, I2S_OUT_DMABUF_LEN, MALLOC_CAP_DMA);
|
||||
if (o_dma.buffers[buf_idx] == nullptr)
|
||||
if (o_dma.buffers[buf_idx] == nullptr) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
// Allocate the array of DMA descriptors
|
||||
o_dma.desc = (lldesc_t**)malloc(sizeof(lldesc_t*) * I2S_OUT_DMABUF_COUNT);
|
||||
if (o_dma.desc == nullptr)
|
||||
if (o_dma.desc == nullptr) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Allocate each DMA descriptor that will be used by the DMA controller
|
||||
for (int buf_idx = 0; buf_idx < I2S_OUT_DMABUF_COUNT; buf_idx++) {
|
||||
o_dma.desc[buf_idx] = (lldesc_t*)heap_caps_malloc(sizeof(lldesc_t), MALLOC_CAP_DMA);
|
||||
if (o_dma.desc[buf_idx] == nullptr)
|
||||
if (o_dma.desc[buf_idx] == nullptr) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
// Initialize
|
||||
|
@@ -33,8 +33,9 @@ uint8_t jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block) {
|
||||
pl_data->line_number = gc_block->values.n;
|
||||
#endif
|
||||
if (soft_limits->get()) {
|
||||
if (system_check_travel_limits(gc_block->values.xyz))
|
||||
return (STATUS_TRAVEL_EXCEEDED);
|
||||
if (system_check_travel_limits(gc_block->values.xyz)) {
|
||||
return STATUS_TRAVEL_EXCEEDED;
|
||||
}
|
||||
}
|
||||
// Valid jog command. Plan, set state, and execute.
|
||||
mc_line(gc_block->values.xyz, pl_data);
|
||||
@@ -45,5 +46,5 @@ uint8_t jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block) {
|
||||
st_wake_up(); // NOTE: Manual start. No state machine required.
|
||||
}
|
||||
}
|
||||
return (STATUS_OK);
|
||||
return STATUS_OK;
|
||||
}
|
||||
|
@@ -75,8 +75,9 @@ void IRAM_ATTR isr_limit_switches() {
|
||||
// NOTE: Only the abort realtime command can interrupt this process.
|
||||
// TODO: Move limit pin-specific calls to a general function for portability.
|
||||
void limits_go_home(uint8_t cycle_mask) {
|
||||
if (sys.abort)
|
||||
if (sys.abort) {
|
||||
return; // Block if system reset has been issued.
|
||||
}
|
||||
// Initialize plan data struct for homing motion. Spindle and coolant are disabled.
|
||||
motors_set_homing_mode(cycle_mask, true); // tell motors homing is about to start
|
||||
plan_line_data_t plan_data;
|
||||
@@ -96,8 +97,9 @@ void limits_go_home(uint8_t cycle_mask) {
|
||||
// Initialize step pin masks
|
||||
step_pin[idx] = get_step_pin_mask(idx);
|
||||
#ifdef COREXY
|
||||
if ((idx == A_MOTOR) || (idx == B_MOTOR))
|
||||
if ((idx == A_MOTOR) || (idx == B_MOTOR)) {
|
||||
step_pin[idx] = (get_step_pin_mask(X_AXIS) | get_step_pin_mask(Y_AXIS));
|
||||
}
|
||||
#endif
|
||||
if (bit_istrue(cycle_mask, bit(idx))) {
|
||||
// Set target based on max_travel setting. Ensure homing switches engaged with search scalar.
|
||||
@@ -125,8 +127,9 @@ void limits_go_home(uint8_t cycle_mask) {
|
||||
} else if (idx == Y_AXIS) {
|
||||
int32_t axis_position = system_convert_corexy_to_x_axis_steps(sys_position);
|
||||
sys_position[A_MOTOR] = sys_position[B_MOTOR] = axis_position;
|
||||
} else
|
||||
} else {
|
||||
sys_position[Z_AXIS] = 0;
|
||||
}
|
||||
#else
|
||||
sys_position[idx] = 0;
|
||||
#endif
|
||||
@@ -134,15 +137,17 @@ void limits_go_home(uint8_t cycle_mask) {
|
||||
// NOTE: This happens to compile smaller than any other implementation tried.
|
||||
auto mask = homing_dir_mask->get();
|
||||
if (bit_istrue(mask, bit(idx))) {
|
||||
if (approach)
|
||||
if (approach) {
|
||||
target[idx] = -max_travel;
|
||||
else
|
||||
} else {
|
||||
target[idx] = max_travel;
|
||||
}
|
||||
} else {
|
||||
if (approach)
|
||||
if (approach) {
|
||||
target[idx] = max_travel;
|
||||
else
|
||||
} else {
|
||||
target[idx] = -max_travel;
|
||||
}
|
||||
}
|
||||
// Apply axislock to the step port pins active in this cycle.
|
||||
axislock |= step_pin[idx];
|
||||
@@ -164,10 +169,11 @@ void limits_go_home(uint8_t cycle_mask) {
|
||||
if (axislock & step_pin[idx]) {
|
||||
if (limit_state & bit(idx)) {
|
||||
#ifdef COREXY
|
||||
if (idx == Z_AXIS)
|
||||
if (idx == Z_AXIS) {
|
||||
axislock &= ~(step_pin[Z_AXIS]);
|
||||
else
|
||||
} else {
|
||||
axislock &= ~(step_pin[A_MOTOR] | step_pin[B_MOTOR]);
|
||||
}
|
||||
#else
|
||||
axislock &= ~(step_pin[idx]);
|
||||
#endif
|
||||
@@ -181,17 +187,22 @@ void limits_go_home(uint8_t cycle_mask) {
|
||||
if (sys_rt_exec_state & (EXEC_SAFETY_DOOR | EXEC_RESET | EXEC_CYCLE_STOP)) {
|
||||
uint8_t rt_exec = sys_rt_exec_state;
|
||||
// Homing failure condition: Reset issued during cycle.
|
||||
if (rt_exec & EXEC_RESET)
|
||||
if (rt_exec & EXEC_RESET) {
|
||||
system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_RESET);
|
||||
}
|
||||
// Homing failure condition: Safety door was opened.
|
||||
if (rt_exec & EXEC_SAFETY_DOOR)
|
||||
if (rt_exec & EXEC_SAFETY_DOOR) {
|
||||
system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_DOOR);
|
||||
}
|
||||
// Homing failure condition: Limit switch still engaged after pull-off motion
|
||||
if (!approach && (limits_get_state() & cycle_mask))
|
||||
if (!approach && (limits_get_state() & cycle_mask)) {
|
||||
system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_PULLOFF);
|
||||
}
|
||||
// Homing failure condition: Limit switch not found during approach.
|
||||
if (approach && (rt_exec & EXEC_CYCLE_STOP))
|
||||
if (approach && (rt_exec & EXEC_CYCLE_STOP)) {
|
||||
system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_APPROACH);
|
||||
}
|
||||
|
||||
if (sys_rt_exec_alarm) {
|
||||
motors_set_homing_mode(cycle_mask, false); // tell motors homing is done...failed
|
||||
mc_reset(); // Stop motors, if they are running.
|
||||
@@ -264,8 +275,9 @@ void limits_go_home(uint8_t cycle_mask) {
|
||||
int32_t off_axis_position = system_convert_corexy_to_x_axis_steps(sys_position);
|
||||
sys_position[A_MOTOR] = off_axis_position + set_axis_position;
|
||||
sys_position[B_MOTOR] = off_axis_position - set_axis_position;
|
||||
} else
|
||||
} else {
|
||||
sys_position[idx] = set_axis_position;
|
||||
}
|
||||
#else
|
||||
sys_position[idx] = set_axis_position;
|
||||
#endif
|
||||
@@ -352,7 +364,7 @@ uint8_t limits_get_state() {
|
||||
if (limit_invert->get()) {
|
||||
pinMask ^= limit_mask;
|
||||
}
|
||||
return (pinMask);
|
||||
return pinMask;
|
||||
}
|
||||
|
||||
// Performs a soft limit check. Called from mc_line() only. Assumes the machine has been homed,
|
||||
@@ -376,8 +388,9 @@ void limits_soft_check(float* target) {
|
||||
system_set_exec_state_flag(EXEC_FEED_HOLD);
|
||||
do {
|
||||
protocol_execute_realtime();
|
||||
if (sys.abort)
|
||||
if (sys.abort) {
|
||||
return;
|
||||
}
|
||||
} while (sys.state != STATE_IDLE);
|
||||
}
|
||||
mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown.
|
||||
|
@@ -54,12 +54,14 @@ void mc_line(float* target, plan_line_data_t* pl_data) {
|
||||
// from everywhere in Grbl.
|
||||
if (soft_limits->get()) {
|
||||
// NOTE: Block jog state. Jogging is a special case and soft limits are handled independently.
|
||||
if (sys.state != STATE_JOG)
|
||||
if (sys.state != STATE_JOG) {
|
||||
limits_soft_check(target);
|
||||
}
|
||||
}
|
||||
// If in check gcode mode, prevent motion by blocking planner. Soft limits still work.
|
||||
if (sys.state == STATE_CHECK_MODE)
|
||||
if (sys.state == STATE_CHECK_MODE) {
|
||||
return;
|
||||
}
|
||||
// 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
|
||||
// plan_check_full_buffer() and check for system abort loop. Also for position reporting
|
||||
@@ -77,12 +79,14 @@ void mc_line(float* target, plan_line_data_t* pl_data) {
|
||||
// Remain in this loop until there is room in the buffer.
|
||||
do {
|
||||
protocol_execute_realtime(); // Check for any run-time commands
|
||||
if (sys.abort)
|
||||
if (sys.abort) {
|
||||
return; // Bail, if system abort.
|
||||
if (plan_check_full_buffer())
|
||||
}
|
||||
if (plan_check_full_buffer()) {
|
||||
protocol_auto_cycle_start(); // Auto-cycle start when buffer is full.
|
||||
else
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
} while (1);
|
||||
// Plan and queue motion into planner buffer
|
||||
// uint8_t plan_status; // Not used in normal operation.
|
||||
@@ -114,17 +118,20 @@ void mc_arc(float* target,
|
||||
#ifdef USE_KINEMATICS
|
||||
float previous_position[N_AXIS];
|
||||
uint16_t n;
|
||||
for (n = 0; n < N_AXIS; n++)
|
||||
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
|
||||
if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON)
|
||||
if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) {
|
||||
angular_travel -= 2 * M_PI;
|
||||
}
|
||||
} else {
|
||||
if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON)
|
||||
if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) {
|
||||
angular_travel += 2 * M_PI;
|
||||
}
|
||||
}
|
||||
// NOTE: Segment end points are on the arc, which can lead to the arc diameter being smaller by up to
|
||||
// (2x) arc_tolerance. For 99% of users, this is just fine. If a different arc segment fit
|
||||
@@ -204,8 +211,9 @@ void mc_arc(float* target,
|
||||
mc_line(position, pl_data);
|
||||
#endif
|
||||
// Bail mid-circle on system abort. Runtime command check already performed by mc_line.
|
||||
if (sys.abort)
|
||||
if (sys.abort) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Ensure last segment arrives at target location.
|
||||
@@ -218,8 +226,9 @@ void mc_arc(float* target,
|
||||
|
||||
// Execute dwell in seconds.
|
||||
void mc_dwell(float seconds) {
|
||||
if (sys.state == STATE_CHECK_MODE)
|
||||
if (sys.state == STATE_CHECK_MODE) {
|
||||
return;
|
||||
}
|
||||
protocol_buffer_synchronize();
|
||||
delay_sec(seconds, DELAY_MODE_DWELL);
|
||||
}
|
||||
@@ -255,18 +264,20 @@ static bool axis_is_squared(uint8_t axis_mask) {
|
||||
// executing the homing cycle. This prevents incorrect buffered plans after homing.
|
||||
void mc_homing_cycle(uint8_t cycle_mask) {
|
||||
#ifdef USE_CUSTOM_HOMING
|
||||
if (user_defined_homing())
|
||||
if (user_defined_homing()) {
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
// This give kinematics a chance to do something before normal homing
|
||||
// if it returns true, the homing is canceled.
|
||||
// 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))
|
||||
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.
|
||||
// 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.
|
||||
#ifdef LIMITS_TWO_SWITCHES_ON_AXES
|
||||
if (limits_get_state()) {
|
||||
mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown.
|
||||
@@ -284,9 +295,9 @@ void mc_homing_cycle(uint8_t cycle_mask) {
|
||||
else
|
||||
*/
|
||||
if (cycle_mask) {
|
||||
if (!axis_is_squared(cycle_mask))
|
||||
if (!axis_is_squared(cycle_mask)) {
|
||||
limits_go_home(cycle_mask); // Homing cycle 0
|
||||
else {
|
||||
} else {
|
||||
ganged_mode = SquaringMode::Dual;
|
||||
n_homing_locate_cycle = 0; // don't do a second touch cycle
|
||||
limits_go_home(cycle_mask);
|
||||
@@ -302,9 +313,9 @@ void mc_homing_cycle(uint8_t cycle_mask) {
|
||||
#endif
|
||||
{
|
||||
// Search to engage all axes limit switches at faster homing seek rate.
|
||||
if (!axis_is_squared(HOMING_CYCLE_0))
|
||||
if (!axis_is_squared(HOMING_CYCLE_0)) {
|
||||
limits_go_home(HOMING_CYCLE_0); // Homing cycle 0
|
||||
else {
|
||||
} else {
|
||||
ganged_mode = SquaringMode::Dual;
|
||||
n_homing_locate_cycle = 0; // don't do a second touch cycle
|
||||
limits_go_home(HOMING_CYCLE_0);
|
||||
@@ -316,9 +327,9 @@ void mc_homing_cycle(uint8_t cycle_mask) {
|
||||
ganged_mode = SquaringMode::Dual; // always return to dual
|
||||
}
|
||||
#ifdef HOMING_CYCLE_1
|
||||
if (!axis_is_squared(HOMING_CYCLE_1))
|
||||
if (!axis_is_squared(HOMING_CYCLE_1)) {
|
||||
limits_go_home(HOMING_CYCLE_1);
|
||||
else {
|
||||
} else {
|
||||
ganged_mode = SquaringMode::Dual;
|
||||
n_homing_locate_cycle = 0; // don't do a second touch cycle
|
||||
limits_go_home(HOMING_CYCLE_1);
|
||||
@@ -331,9 +342,9 @@ void mc_homing_cycle(uint8_t cycle_mask) {
|
||||
}
|
||||
#endif
|
||||
#ifdef HOMING_CYCLE_2
|
||||
if (!axis_is_squared(HOMING_CYCLE_2))
|
||||
if (!axis_is_squared(HOMING_CYCLE_2)) {
|
||||
limits_go_home(HOMING_CYCLE_2);
|
||||
else {
|
||||
} else {
|
||||
ganged_mode = SquaringMode::Dual;
|
||||
n_homing_locate_cycle = 0; // don't do a second touch cycle
|
||||
limits_go_home(HOMING_CYCLE_2);
|
||||
@@ -378,15 +389,16 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par
|
||||
// TODO: Need to update this cycle so it obeys a non-auto cycle start.
|
||||
if (sys.state == STATE_CHECK_MODE) {
|
||||
#ifdef SET_CHECK_MODE_PROBE_TO_START
|
||||
return (GCUpdatePos::None);
|
||||
return GCUpdatePos::None;
|
||||
#else
|
||||
return (GCUpdatePos::Target);
|
||||
return GCUpdatePos::Target;
|
||||
#endif
|
||||
}
|
||||
// Finish all queued commands and empty planner buffer before starting probe cycle.
|
||||
protocol_buffer_synchronize();
|
||||
if (sys.abort)
|
||||
return (GCUpdatePos::None); // Return if system reset has been issued.
|
||||
if (sys.abort) {
|
||||
return GCUpdatePos::None; // Return if system reset has been issued.
|
||||
}
|
||||
// Initialize probing control variables
|
||||
uint8_t is_probe_away = bit_istrue(parser_flags, GCParserProbeIsAway);
|
||||
uint8_t is_no_error = bit_istrue(parser_flags, GCParserProbeIsNoError);
|
||||
@@ -398,7 +410,7 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par
|
||||
system_set_exec_alarm(EXEC_ALARM_PROBE_FAIL_INITIAL);
|
||||
protocol_execute_realtime();
|
||||
probe_configure_invert_mask(false); // Re-initialize invert mask before returning.
|
||||
return (GCUpdatePos::None); // Nothing else to do but bail.
|
||||
return GCUpdatePos::None; // Nothing else to do but bail.
|
||||
}
|
||||
// Setup and queue probing motion. Auto cycle-start should not start the cycle.
|
||||
mc_line(target, pl_data);
|
||||
@@ -408,16 +420,18 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par
|
||||
system_set_exec_state_flag(EXEC_CYCLE_START);
|
||||
do {
|
||||
protocol_execute_realtime();
|
||||
if (sys.abort)
|
||||
return (GCUpdatePos::None); // Check for system abort
|
||||
if (sys.abort) {
|
||||
return GCUpdatePos::None; // Check for system abort
|
||||
}
|
||||
} while (sys.state != STATE_IDLE);
|
||||
// Probing cycle complete!
|
||||
// Set state variables and error out, if the probe failed and cycle with error is enabled.
|
||||
if (sys_probe_state == PROBE_ACTIVE) {
|
||||
if (is_no_error)
|
||||
if (is_no_error) {
|
||||
memcpy(sys_probe_position, sys_position, sizeof(sys_position));
|
||||
else
|
||||
} else {
|
||||
system_set_exec_alarm(EXEC_ALARM_PROBE_FAIL_CONTACT);
|
||||
}
|
||||
} else {
|
||||
sys.probe_succeeded = true; // Indicate to system the probing cycle completed successfully.
|
||||
}
|
||||
@@ -432,17 +446,19 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par
|
||||
// All done! Output the probe position as message.
|
||||
report_probe_parameters(CLIENT_ALL);
|
||||
#endif
|
||||
if (sys.probe_succeeded)
|
||||
return (GCUpdatePos::System); // Successful probe cycle.
|
||||
else
|
||||
return (GCUpdatePos::Target); // Failed to trigger probe within travel. With or without error.
|
||||
if (sys.probe_succeeded) {
|
||||
return GCUpdatePos::System; // Successful probe cycle.
|
||||
} else {
|
||||
return GCUpdatePos::Target; // Failed to trigger probe within travel. With or without error.
|
||||
}
|
||||
}
|
||||
|
||||
// Plans and executes the single special motion case for parking. Independent of main planner buffer.
|
||||
// NOTE: Uses the always free planner ring buffer head to store motion parameters for execution.
|
||||
void mc_parking_motion(float* parking_target, plan_line_data_t* pl_data) {
|
||||
if (sys.abort)
|
||||
if (sys.abort) {
|
||||
return; // Block during abort.
|
||||
}
|
||||
uint8_t plan_status = plan_buffer_line(parking_target, pl_data);
|
||||
if (plan_status) {
|
||||
bit_true(sys.step_control, STEP_CONTROL_EXECUTE_SYS_MOTION);
|
||||
@@ -452,8 +468,9 @@ void mc_parking_motion(float* parking_target, plan_line_data_t* pl_data) {
|
||||
st_wake_up();
|
||||
do {
|
||||
protocol_exec_rt_system();
|
||||
if (sys.abort)
|
||||
if (sys.abort) {
|
||||
return;
|
||||
}
|
||||
} while (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION);
|
||||
st_parking_restore_buffer(); // Restore step segment buffer to normal run state.
|
||||
} else {
|
||||
@@ -466,8 +483,9 @@ void mc_parking_motion(float* parking_target, plan_line_data_t* pl_data) {
|
||||
void mc_override_ctrl_update(uint8_t override_state) {
|
||||
// Finish all queued commands before altering override control state
|
||||
protocol_buffer_synchronize();
|
||||
if (sys.abort)
|
||||
if (sys.abort) {
|
||||
return;
|
||||
}
|
||||
sys.override_ctrl = override_state;
|
||||
}
|
||||
#endif
|
||||
@@ -502,10 +520,12 @@ void mc_reset() {
|
||||
if ((sys.state & (STATE_CYCLE | STATE_HOMING | STATE_JOG)) ||
|
||||
(sys.step_control & (STEP_CONTROL_EXECUTE_HOLD | STEP_CONTROL_EXECUTE_SYS_MOTION))) {
|
||||
if (sys.state == STATE_HOMING) {
|
||||
if (!sys_rt_exec_alarm)
|
||||
if (!sys_rt_exec_alarm) {
|
||||
system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_RESET);
|
||||
} else
|
||||
}
|
||||
} else {
|
||||
system_set_exec_alarm(EXEC_ALARM_ABORT_CYCLE);
|
||||
}
|
||||
st_go_idle(); // Force kill steppers. Position has likely been lost.
|
||||
}
|
||||
ganged_mode = SquaringMode::Dual; // in case an error occurred during squaring
|
||||
|
@@ -250,13 +250,15 @@ void init_motors() {
|
||||
// certain motors need features to be turned on. Check them here
|
||||
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) {
|
||||
for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
|
||||
if (myMotor[axis][gang_index]->type_id == UNIPOLAR_MOTOR)
|
||||
if (myMotor[axis][gang_index]->type_id == UNIPOLAR_MOTOR) {
|
||||
motor_class_steps = true;
|
||||
}
|
||||
|
||||
// CS Pins of all TMC motors need to be setup before any can be talked to
|
||||
// ...so init cannot be called via the constructors. This inits them all.
|
||||
if (myMotor[axis][gang_index]->type_id == TRINAMIC_SPI_MOTOR)
|
||||
if (myMotor[axis][gang_index]->type_id == TRINAMIC_SPI_MOTOR) {
|
||||
myMotor[axis][gang_index]->init();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -273,8 +275,9 @@ void init_motors() {
|
||||
&readSgTaskHandle,
|
||||
0 // core
|
||||
);
|
||||
if (stallguard_debug_mask->get() != 0)
|
||||
if (stallguard_debug_mask->get() != 0) {
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Stallguard debug enabled: %d", stallguard_debug_mask->get());
|
||||
}
|
||||
}
|
||||
|
||||
if (motors_have_type_id(RC_SERVO_MOTOR)) {
|
||||
@@ -299,8 +302,9 @@ void servoUpdateTask(void* pvParameters) {
|
||||
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Servo update");
|
||||
|
||||
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) {
|
||||
for (uint8_t gang_index = 0; gang_index < 2; gang_index++)
|
||||
for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
|
||||
myMotor[axis][gang_index]->update();
|
||||
}
|
||||
}
|
||||
|
||||
vTaskDelayUntil(&xLastWakeTime, xUpdate);
|
||||
@@ -311,8 +315,9 @@ void servoUpdateTask(void* pvParameters) {
|
||||
bool motors_have_type_id(motor_class_id_t id) {
|
||||
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) {
|
||||
for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
|
||||
if (myMotor[axis][gang_index]->type_id == id)
|
||||
if (myMotor[axis][gang_index]->type_id == id) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
@@ -321,8 +326,9 @@ bool motors_have_type_id(motor_class_id_t id) {
|
||||
void motors_set_disable(bool disable) {
|
||||
static bool previous_state = false;
|
||||
|
||||
if (previous_state == disable)
|
||||
if (previous_state == disable) {
|
||||
return;
|
||||
}
|
||||
|
||||
previous_state = disable;
|
||||
|
||||
@@ -343,8 +349,9 @@ void motors_set_disable(bool disable) {
|
||||
void motors_read_settings() {
|
||||
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Read Settings");
|
||||
for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
|
||||
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++)
|
||||
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) {
|
||||
myMotor[axis][gang_index]->read_settings();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -353,23 +360,27 @@ void motors_read_settings() {
|
||||
void motors_set_homing_mode(uint8_t homing_mask, bool isHoming) {
|
||||
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "motors_set_homing_mode(%d)", is_homing);
|
||||
for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
|
||||
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++)
|
||||
if (bit(axis) & homing_mask)
|
||||
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) {
|
||||
if (bit(axis) & homing_mask) {
|
||||
myMotor[axis][gang_index]->set_homing_mode(homing_mask, isHoming);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void motors_set_direction_pins(uint8_t onMask) {
|
||||
static uint8_t previous_val = 255; // should never be this value
|
||||
if (previous_val == onMask)
|
||||
if (previous_val == onMask) {
|
||||
return;
|
||||
}
|
||||
previous_val = onMask;
|
||||
|
||||
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "motors_set_direction_pins:0x%02X", onMask);
|
||||
|
||||
for (uint8_t gang_index = 0; gang_index < MAX_GANGED; gang_index++) {
|
||||
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++)
|
||||
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) {
|
||||
myMotor[axis][gang_index]->set_direction_pins(onMask);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -388,8 +399,9 @@ uint8_t get_next_trinamic_driver_index() {
|
||||
void motors_step(uint8_t step_mask, uint8_t dir_mask) {
|
||||
if (motor_class_steps) { // determined in init_motors if any motors need to handle steps
|
||||
for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
|
||||
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++)
|
||||
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) {
|
||||
myMotor[axis][gang_index]->step(step_mask, dir_mask);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -413,8 +425,9 @@ void readSgTask(void* pvParameters) {
|
||||
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) {
|
||||
if (stallguard_debug_mask->get() & bit(axis)) {
|
||||
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "SG:%d", stallguard_debug_mask->get());
|
||||
for (uint8_t gang_index = 0; gang_index < 2; gang_index++)
|
||||
for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
|
||||
myMotor[axis][gang_index]->debug_message();
|
||||
}
|
||||
}
|
||||
}
|
||||
} // sys.state
|
||||
|
@@ -83,8 +83,9 @@ namespace Motors {
|
||||
|
||||
void RcServo::_write_pwm(uint32_t duty) {
|
||||
// to prevent excessive calls to ledcWrite, make sure duty hass changed
|
||||
if (duty == _current_pwm_duty)
|
||||
if (duty == _current_pwm_duty) {
|
||||
return;
|
||||
}
|
||||
|
||||
_current_pwm_duty = duty;
|
||||
|
||||
@@ -96,20 +97,23 @@ namespace Motors {
|
||||
void RcServo::set_disable(bool disable) {
|
||||
return;
|
||||
_disabled = disable;
|
||||
if (_disabled)
|
||||
if (_disabled) {
|
||||
_write_pwm(0);
|
||||
}
|
||||
}
|
||||
|
||||
void RcServo::set_homing_mode(bool is_homing, bool isHoming) {
|
||||
float home_pos = 0.0;
|
||||
|
||||
if (!is_homing)
|
||||
if (!is_homing) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (bit_istrue(homing_dir_mask->get(), bit(axis_index)))
|
||||
if (bit_istrue(homing_dir_mask->get(), bit(axis_index))) {
|
||||
home_pos = _position_min;
|
||||
else
|
||||
} else {
|
||||
home_pos = _position_max;
|
||||
}
|
||||
|
||||
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Servo set home %d %3.2f", is_homing, home_pos);
|
||||
sys_position[axis_index] = home_pos * axis_settings[axis_index]->steps_per_mm->get(); // convert to steps
|
||||
|
@@ -46,15 +46,15 @@ namespace Motors {
|
||||
|
||||
set_axis_name();
|
||||
|
||||
if (_driver_part_number == 2130)
|
||||
if (_driver_part_number == 2130) {
|
||||
tmcstepper = new TMC2130Stepper(cs_pin, _r_sense, spi_index);
|
||||
else if (_driver_part_number == 5160)
|
||||
} else if (_driver_part_number == 5160) {
|
||||
tmcstepper = new TMC5160Stepper(cs_pin, _r_sense, spi_index);
|
||||
else {
|
||||
} else {
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "%s Axis Unsupported Trinamic part number TMC%d", _axis_name, _driver_part_number);
|
||||
has_errors = true; // as opposed to NullMotors, this is a real motor
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
init_step_dir_pins(); // from StandardStepper
|
||||
|
||||
@@ -62,8 +62,9 @@ namespace Motors {
|
||||
pinMode(cs_pin, OUTPUT);
|
||||
|
||||
// use slower speed if I2S
|
||||
if (cs_pin >= I2S_OUT_PIN_BASE)
|
||||
if (cs_pin >= I2S_OUT_PIN_BASE) {
|
||||
tmcstepper->setSPISpeed(TRINAMIC_SPI_FREQ);
|
||||
}
|
||||
|
||||
config_message();
|
||||
|
||||
@@ -71,8 +72,9 @@ namespace Motors {
|
||||
}
|
||||
|
||||
void TrinamicDriver::init() {
|
||||
if (has_errors)
|
||||
if (has_errors) {
|
||||
return;
|
||||
}
|
||||
|
||||
SPI.begin(); // this will get called for each motor, but does not seem to hurt anything
|
||||
|
||||
@@ -82,7 +84,6 @@ namespace Motors {
|
||||
set_mode(false);
|
||||
|
||||
_homing_mask = 0;
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
@@ -102,8 +103,9 @@ namespace Motors {
|
||||
}
|
||||
|
||||
bool TrinamicDriver::test() {
|
||||
if (has_errors)
|
||||
if (has_errors) {
|
||||
return false;
|
||||
}
|
||||
switch (tmcstepper->test_connection()) {
|
||||
case 1:
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "%s Trinamic driver test failed. Check connection", _axis_name);
|
||||
@@ -139,8 +141,9 @@ namespace Motors {
|
||||
err = true;
|
||||
}
|
||||
|
||||
if (err)
|
||||
if (err) {
|
||||
return false;
|
||||
}
|
||||
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "%s Trinamic driver test passed", _axis_name);
|
||||
return true;
|
||||
@@ -154,17 +157,19 @@ namespace Motors {
|
||||
float hold (as a percentage of run)
|
||||
*/
|
||||
void TrinamicDriver::read_settings() {
|
||||
if (has_errors)
|
||||
if (has_errors) {
|
||||
return;
|
||||
}
|
||||
uint16_t run_i_ma = (uint16_t)(axis_settings[axis_index]->run_current->get() * 1000.0);
|
||||
float hold_i_percent;
|
||||
|
||||
if (axis_settings[axis_index]->run_current->get() == 0)
|
||||
if (axis_settings[axis_index]->run_current->get() == 0) {
|
||||
hold_i_percent = 0;
|
||||
else {
|
||||
} else {
|
||||
hold_i_percent = axis_settings[axis_index]->hold_current->get() / axis_settings[axis_index]->run_current->get();
|
||||
if (hold_i_percent > 1.0)
|
||||
if (hold_i_percent > 1.0) {
|
||||
hold_i_percent = 1.0;
|
||||
}
|
||||
}
|
||||
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "%s Current run %d hold %f", _axis_name, run_i_ma, hold_i_percent);
|
||||
|
||||
@@ -183,15 +188,18 @@ namespace Motors {
|
||||
Coolstep mode, so it will need to switch to Coolstep when homing
|
||||
*/
|
||||
void TrinamicDriver::set_mode(bool isHoming) {
|
||||
if (has_errors)
|
||||
if (has_errors) {
|
||||
return;
|
||||
if (isHoming)
|
||||
}
|
||||
if (isHoming) {
|
||||
_mode = TRINAMIC_HOMING_MODE;
|
||||
else
|
||||
} else {
|
||||
_mode = TRINAMIC_RUN_MODE;
|
||||
}
|
||||
|
||||
if (_lastMode == _mode)
|
||||
if (_lastMode == _mode) {
|
||||
return;
|
||||
}
|
||||
_lastMode = _mode;
|
||||
|
||||
switch (_mode) {
|
||||
@@ -226,12 +234,14 @@ namespace Motors {
|
||||
This is the stallguard tuning info. It is call debug, so it could be generic across all classes.
|
||||
*/
|
||||
void TrinamicDriver::debug_message() {
|
||||
if (has_errors)
|
||||
if (has_errors) {
|
||||
return;
|
||||
}
|
||||
uint32_t tstep = tmcstepper->TSTEP();
|
||||
|
||||
if (tstep == 0xFFFFF || tstep < 1) // if axis is not moving return
|
||||
if (tstep == 0xFFFFF || tstep < 1) { // if axis is not moving return
|
||||
return;
|
||||
}
|
||||
float feedrate = st_get_realtime_rate(); //* settings.microsteps[axis_index] / 60.0 ; // convert mm/min to Hz
|
||||
|
||||
grbl_msg_sendf(CLIENT_SERIAL,
|
||||
@@ -253,25 +263,27 @@ namespace Motors {
|
||||
speed / 60.0 * axis_settings[axis_index]->steps_per_mm->get() * (float)(256 / axis_settings[axis_index]->microsteps->get());
|
||||
tstep = TRINAMIC_FCLK / tstep * percent / 100.0;
|
||||
|
||||
return (uint32_t)tstep;
|
||||
return static_cast<uint32_t>(tstep);
|
||||
}
|
||||
|
||||
// this can use the enable feature over SPI. The dedicated pin must be in the enable mode,
|
||||
// but that can be hardwired that way.
|
||||
void TrinamicDriver::set_disable(bool disable) {
|
||||
if (has_errors)
|
||||
return;
|
||||
if (has_errors) {
|
||||
return;
|
||||
}
|
||||
|
||||
digitalWrite(disable_pin, disable);
|
||||
|
||||
#ifdef USE_TRINAMIC_ENABLE
|
||||
if (disable)
|
||||
if (disable) {
|
||||
tmcstepper->toff(TRINAMIC_TOFF_DISABLE);
|
||||
else {
|
||||
if (_mode == TRINAMIC_MODE_STEALTHCHOP)
|
||||
} else {
|
||||
if (_mode == TRINAMIC_MODE_STEALTHCHOP) {
|
||||
tmcstepper->toff(TRINAMIC_TOFF_STEALTHCHOP);
|
||||
else
|
||||
} else {
|
||||
tmcstepper->toff(TRINAMIC_TOFF_COOLSTEP);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
// the pin based enable could be added here.
|
||||
|
@@ -52,27 +52,32 @@ namespace Motors {
|
||||
uint8_t _phase[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; // temporary phase values...all start as off
|
||||
uint8_t phase_max;
|
||||
|
||||
if (!(step_mask & bit(axis_index)))
|
||||
if (!(step_mask & bit(axis_index))) {
|
||||
return; // a step is not required on this interrupt
|
||||
}
|
||||
|
||||
if (!_enabled)
|
||||
if (!_enabled) {
|
||||
return; // don't do anything, phase is not changed or lost
|
||||
}
|
||||
|
||||
if (_half_step)
|
||||
if (_half_step) {
|
||||
phase_max = 7;
|
||||
else
|
||||
} else {
|
||||
phase_max = 3;
|
||||
}
|
||||
|
||||
if (dir_mask & bit(axis_index)) { // count up
|
||||
if (_current_phase == phase_max)
|
||||
if (_current_phase == phase_max) {
|
||||
_current_phase = 0;
|
||||
else
|
||||
} else {
|
||||
_current_phase++;
|
||||
}
|
||||
} else { // count down
|
||||
if (_current_phase == 0)
|
||||
if (_current_phase == 0) {
|
||||
_current_phase = phase_max;
|
||||
else
|
||||
} else {
|
||||
_current_phase--;
|
||||
}
|
||||
}
|
||||
/*
|
||||
8 Step : A – AB – B – BC – C – CD – D – DA
|
||||
|
@@ -44,8 +44,10 @@ uint8_t read_float(const char* line, uint8_t* char_counter, float* float_ptr) {
|
||||
if (c == '-') {
|
||||
isnegative = true;
|
||||
c = *ptr++;
|
||||
} else if (c == '+')
|
||||
} else if (c == '+') {
|
||||
c = *ptr++;
|
||||
}
|
||||
|
||||
// Extract number into fast integer. Track decimal in terms of exponent value.
|
||||
uint32_t intval = 0;
|
||||
int8_t exp = 0;
|
||||
@@ -56,23 +58,27 @@ uint8_t read_float(const char* line, uint8_t* char_counter, float* float_ptr) {
|
||||
if (c <= 9) {
|
||||
ndigit++;
|
||||
if (ndigit <= MAX_INT_DIGITS) {
|
||||
if (isdecimal)
|
||||
if (isdecimal) {
|
||||
exp--;
|
||||
intval = (((intval << 2) + intval) << 1) + c; // intval*10 + c
|
||||
}
|
||||
intval = intval * 10 + c;
|
||||
} else {
|
||||
if (!(isdecimal))
|
||||
if (!(isdecimal)) {
|
||||
exp++; // Drop overflow digits
|
||||
}
|
||||
}
|
||||
} else if (c == (('.' - '0') & 0xff) && !(isdecimal))
|
||||
} else if (c == (('.' - '0') & 0xff) && !(isdecimal)) {
|
||||
isdecimal = true;
|
||||
else
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
c = *ptr++;
|
||||
}
|
||||
// Return if no digits have been read.
|
||||
if (!ndigit)
|
||||
return (false);
|
||||
;
|
||||
if (!ndigit) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Convert integer into floating point.
|
||||
float fval;
|
||||
fval = (float)intval;
|
||||
@@ -83,21 +89,22 @@ uint8_t read_float(const char* line, uint8_t* char_counter, float* float_ptr) {
|
||||
fval *= 0.01;
|
||||
exp += 2;
|
||||
}
|
||||
if (exp < 0)
|
||||
if (exp < 0) {
|
||||
fval *= 0.1;
|
||||
else if (exp > 0) {
|
||||
} else if (exp > 0) {
|
||||
do {
|
||||
fval *= 10.0;
|
||||
} while (--exp > 0);
|
||||
}
|
||||
}
|
||||
// Assign floating point value with correct sign.
|
||||
if (isnegative)
|
||||
if (isnegative) {
|
||||
*float_ptr = -fval;
|
||||
else
|
||||
} else {
|
||||
*float_ptr = fval;
|
||||
}
|
||||
*char_counter = ptr - line - 1; // Set char_counter to next statement
|
||||
return (true);
|
||||
return true;
|
||||
}
|
||||
|
||||
void delay_ms(uint16_t ms) {
|
||||
@@ -108,15 +115,17 @@ void delay_ms(uint16_t ms) {
|
||||
void delay_sec(float seconds, uint8_t mode) {
|
||||
uint16_t i = ceil(1000 / DWELL_TIME_STEP * seconds);
|
||||
while (i-- > 0) {
|
||||
if (sys.abort)
|
||||
if (sys.abort) {
|
||||
return;
|
||||
if (mode == DELAY_MODE_DWELL)
|
||||
}
|
||||
if (mode == DELAY_MODE_DWELL) {
|
||||
protocol_execute_realtime();
|
||||
else { // DELAY_MODE_SYS_SUSPEND
|
||||
} else { // DELAY_MODE_SYS_SUSPEND
|
||||
// Execute rt_system() only to avoid nesting suspend loops.
|
||||
protocol_exec_rt_system();
|
||||
if (sys.suspend & SUSPEND_RESTART_RETRACT)
|
||||
if (sys.suspend & SUSPEND_RESTART_RETRACT) {
|
||||
return; // Bail, if safety door reopens.
|
||||
}
|
||||
}
|
||||
delay(DWELL_TIME_STEP); // Delay DWELL_TIME_STEP increment
|
||||
}
|
||||
@@ -124,45 +133,49 @@ void delay_sec(float seconds, uint8_t mode) {
|
||||
|
||||
// Simple hypotenuse computation function.
|
||||
float hypot_f(float x, float y) {
|
||||
return (sqrt(x * x + y * y));
|
||||
return sqrt(x * x + y * y);
|
||||
}
|
||||
|
||||
float convert_delta_vector_to_unit_vector(float* vector) {
|
||||
uint8_t idx;
|
||||
float magnitude = 0.0;
|
||||
for (idx = 0; idx < N_AXIS; idx++) {
|
||||
if (vector[idx] != 0.0)
|
||||
if (vector[idx] != 0.0) {
|
||||
magnitude += vector[idx] * vector[idx];
|
||||
}
|
||||
}
|
||||
magnitude = sqrt(magnitude);
|
||||
float inv_magnitude = 1.0 / magnitude;
|
||||
for (idx = 0; idx < N_AXIS; idx++)
|
||||
for (idx = 0; idx < N_AXIS; idx++) {
|
||||
vector[idx] *= inv_magnitude;
|
||||
return (magnitude);
|
||||
}
|
||||
return magnitude;
|
||||
}
|
||||
|
||||
float limit_acceleration_by_axis_maximum(float* unit_vec) {
|
||||
uint8_t idx;
|
||||
float limit_value = SOME_LARGE_VALUE;
|
||||
for (idx = 0; idx < N_AXIS; idx++) {
|
||||
if (unit_vec[idx] != 0) // Avoid divide by zero.
|
||||
if (unit_vec[idx] != 0) { // Avoid divide by zero.
|
||||
limit_value = MIN(limit_value, fabs(axis_settings[idx]->acceleration->get() / unit_vec[idx]));
|
||||
}
|
||||
}
|
||||
// The acceleration setting is stored and displayed in units of mm/sec^2,
|
||||
// but used in units of mm/min^2. It suffices to perform the conversion once on
|
||||
// exit, since the limit computation above is independent of units - it simply
|
||||
// finds the smallest value.
|
||||
return (limit_value * SEC_PER_MIN_SQ);
|
||||
return limit_value * SEC_PER_MIN_SQ;
|
||||
}
|
||||
|
||||
float limit_rate_by_axis_maximum(float* unit_vec) {
|
||||
uint8_t idx;
|
||||
float limit_value = SOME_LARGE_VALUE;
|
||||
for (idx = 0; idx < N_AXIS; idx++) {
|
||||
if (unit_vec[idx] != 0) // Avoid divide by zero.
|
||||
if (unit_vec[idx] != 0) { // Avoid divide by zero.
|
||||
limit_value = MIN(limit_value, fabs(axis_settings[idx]->max_rate->get() / unit_vec[idx]));
|
||||
}
|
||||
}
|
||||
return (limit_value);
|
||||
return limit_value;
|
||||
}
|
||||
|
||||
float map_float(float x, float in_min, float in_max, float out_min, float out_max) { // DrawBot_Badge
|
||||
@@ -174,10 +187,12 @@ uint32_t map_uint32_t(uint32_t x, uint32_t in_min, uint32_t in_max, uint32_t out
|
||||
}
|
||||
|
||||
float constrain_float(float in, float min, float max) { // DrawBot_Badge
|
||||
if (in < min)
|
||||
if (in < min) {
|
||||
return min;
|
||||
if (in > max)
|
||||
}
|
||||
if (in > max) {
|
||||
return max;
|
||||
}
|
||||
return in;
|
||||
}
|
||||
|
||||
@@ -187,7 +202,7 @@ float mapConstrain(float x, float in_min, float in_max, float out_min, float out
|
||||
}
|
||||
|
||||
bool char_is_numeric(char value) {
|
||||
return (value >= '0' && value <= '9');
|
||||
return value >= '0' && value <= '9';
|
||||
}
|
||||
|
||||
char* trim(char* str) {
|
||||
|
@@ -34,8 +34,9 @@ void IRAM_ATTR pinMode(uint8_t pin, uint8_t mode) {
|
||||
if (pin == UNDEFINED_PIN) {
|
||||
return;
|
||||
}
|
||||
if (pin < I2S_OUT_PIN_BASE)
|
||||
if (pin < I2S_OUT_PIN_BASE) {
|
||||
__pinMode(pin, mode);
|
||||
}
|
||||
// I2S out pins cannot be configured, hence there
|
||||
// is nothing to do here for them.
|
||||
}
|
||||
|
@@ -45,17 +45,19 @@ static planner_t pl;
|
||||
// Returns the index of the next block in the ring buffer. Also called by stepper segment buffer.
|
||||
uint8_t plan_next_block_index(uint8_t block_index) {
|
||||
block_index++;
|
||||
if (block_index == BLOCK_BUFFER_SIZE)
|
||||
if (block_index == BLOCK_BUFFER_SIZE) {
|
||||
block_index = 0;
|
||||
return (block_index);
|
||||
}
|
||||
return block_index;
|
||||
}
|
||||
|
||||
// Returns the index of the previous block in the ring buffer
|
||||
static uint8_t plan_prev_block_index(uint8_t block_index) {
|
||||
if (block_index == 0)
|
||||
if (block_index == 0) {
|
||||
block_index = BLOCK_BUFFER_SIZE;
|
||||
}
|
||||
block_index--;
|
||||
return (block_index);
|
||||
return block_index;
|
||||
}
|
||||
|
||||
/* PLANNER SPEED DEFINITION
|
||||
@@ -127,8 +129,9 @@ static void planner_recalculate() {
|
||||
// Initialize block index to the last block in the planner buffer.
|
||||
uint8_t block_index = plan_prev_block_index(block_buffer_head);
|
||||
// Bail. Can't do anything with one only one plan-able block.
|
||||
if (block_index == block_buffer_planned)
|
||||
if (block_index == block_buffer_planned) {
|
||||
return;
|
||||
}
|
||||
// Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
|
||||
// block in buffer. Cease planning when the last optimal planned or tail pointer is reached.
|
||||
// NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
|
||||
@@ -140,23 +143,26 @@ static void planner_recalculate() {
|
||||
block_index = plan_prev_block_index(block_index);
|
||||
if (block_index == block_buffer_planned) { // Only two plannable blocks in buffer. Reverse pass complete.
|
||||
// Check if the first block is the tail. If so, notify stepper to update its current parameters.
|
||||
if (block_index == block_buffer_tail)
|
||||
if (block_index == block_buffer_tail) {
|
||||
st_update_plan_block_parameters();
|
||||
}
|
||||
} else { // Three or more plan-able blocks
|
||||
while (block_index != block_buffer_planned) {
|
||||
next = current;
|
||||
current = &block_buffer[block_index];
|
||||
block_index = plan_prev_block_index(block_index);
|
||||
// Check if next block is the tail block(=planned block). If so, update current stepper parameters.
|
||||
if (block_index == block_buffer_tail)
|
||||
if (block_index == block_buffer_tail) {
|
||||
st_update_plan_block_parameters();
|
||||
}
|
||||
// Compute maximum entry speed decelerating over the current block from its exit speed.
|
||||
if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
|
||||
entry_speed_sqr = next->entry_speed_sqr + 2 * current->acceleration * current->millimeters;
|
||||
if (entry_speed_sqr < current->max_entry_speed_sqr)
|
||||
if (entry_speed_sqr < current->max_entry_speed_sqr) {
|
||||
current->entry_speed_sqr = entry_speed_sqr;
|
||||
else
|
||||
} else {
|
||||
current->entry_speed_sqr = current->max_entry_speed_sqr;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -182,8 +188,9 @@ static void planner_recalculate() {
|
||||
// point in the buffer. When the plan is bracketed by either the beginning of the
|
||||
// buffer and a maximum entry speed or two maximum entry speeds, every block in between
|
||||
// cannot logically be further improved. Hence, we don't have to recompute them anymore.
|
||||
if (next->entry_speed_sqr == next->max_entry_speed_sqr)
|
||||
if (next->entry_speed_sqr == next->max_entry_speed_sqr) {
|
||||
block_buffer_planned = block_index;
|
||||
}
|
||||
block_index = plan_next_block_index(block_index);
|
||||
}
|
||||
}
|
||||
@@ -204,65 +211,75 @@ void plan_discard_current_block() {
|
||||
if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer.
|
||||
uint8_t block_index = plan_next_block_index(block_buffer_tail);
|
||||
// Push block_buffer_planned pointer, if encountered.
|
||||
if (block_buffer_tail == block_buffer_planned)
|
||||
if (block_buffer_tail == block_buffer_planned) {
|
||||
block_buffer_planned = block_index;
|
||||
}
|
||||
block_buffer_tail = block_index;
|
||||
}
|
||||
}
|
||||
|
||||
// Returns address of planner buffer block used by system motions. Called by segment generator.
|
||||
plan_block_t* plan_get_system_motion_block() {
|
||||
return (&block_buffer[block_buffer_head]);
|
||||
return &block_buffer[block_buffer_head];
|
||||
}
|
||||
|
||||
// Returns address of first planner block, if available. Called by various main program functions.
|
||||
plan_block_t* plan_get_current_block() {
|
||||
if (block_buffer_head == block_buffer_tail)
|
||||
return (NULL); // Buffer empty
|
||||
return (&block_buffer[block_buffer_tail]);
|
||||
if (block_buffer_head == block_buffer_tail) {
|
||||
return NULL; // Buffer empty
|
||||
}
|
||||
return &block_buffer[block_buffer_tail];
|
||||
}
|
||||
|
||||
float plan_get_exec_block_exit_speed_sqr() {
|
||||
uint8_t block_index = plan_next_block_index(block_buffer_tail);
|
||||
if (block_index == block_buffer_head)
|
||||
return (0.0);
|
||||
return (block_buffer[block_index].entry_speed_sqr);
|
||||
if (block_index == block_buffer_head) {
|
||||
return 0.0f;
|
||||
}
|
||||
return block_buffer[block_index].entry_speed_sqr;
|
||||
}
|
||||
|
||||
// Returns the availability status of the block ring buffer. True, if full.
|
||||
uint8_t plan_check_full_buffer() {
|
||||
if (block_buffer_tail == next_buffer_head)
|
||||
return (true);
|
||||
return (false);
|
||||
if (block_buffer_tail == next_buffer_head) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// Computes and returns block nominal speed based on running condition and override values.
|
||||
// NOTE: All system motion commands, such as homing/parking, are not subject to overrides.
|
||||
float plan_compute_profile_nominal_speed(plan_block_t* block) {
|
||||
float nominal_speed = block->programmed_rate;
|
||||
if (block->motion & PL_MOTION_RAPID_MOTION)
|
||||
if (block->motion & PL_MOTION_RAPID_MOTION) {
|
||||
nominal_speed *= (0.01 * sys.r_override);
|
||||
else {
|
||||
if (!(block->motion & PL_MOTION_NO_FEED_OVERRIDE))
|
||||
} else {
|
||||
if (!(block->motion & PL_MOTION_NO_FEED_OVERRIDE)) {
|
||||
nominal_speed *= (0.01 * sys.f_override);
|
||||
if (nominal_speed > block->rapid_rate)
|
||||
}
|
||||
if (nominal_speed > block->rapid_rate) {
|
||||
nominal_speed = block->rapid_rate;
|
||||
}
|
||||
}
|
||||
if (nominal_speed > MINIMUM_FEED_RATE)
|
||||
return (nominal_speed);
|
||||
return (MINIMUM_FEED_RATE);
|
||||
if (nominal_speed > MINIMUM_FEED_RATE) {
|
||||
return nominal_speed;
|
||||
}
|
||||
return MINIMUM_FEED_RATE;
|
||||
}
|
||||
|
||||
// Computes and updates the max entry speed (sqr) of the block, based on the minimum of the junction's
|
||||
// previous and current nominal speeds and max junction speed.
|
||||
static void plan_compute_profile_parameters(plan_block_t* block, float nominal_speed, float prev_nominal_speed) {
|
||||
// Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds.
|
||||
if (nominal_speed > prev_nominal_speed)
|
||||
if (nominal_speed > prev_nominal_speed) {
|
||||
block->max_entry_speed_sqr = prev_nominal_speed * prev_nominal_speed;
|
||||
else
|
||||
} else {
|
||||
block->max_entry_speed_sqr = nominal_speed * nominal_speed;
|
||||
if (block->max_entry_speed_sqr > block->max_junction_speed_sqr)
|
||||
}
|
||||
|
||||
if (block->max_entry_speed_sqr > block->max_junction_speed_sqr) {
|
||||
block->max_entry_speed_sqr = block->max_junction_speed_sqr;
|
||||
}
|
||||
}
|
||||
|
||||
// Re-calculates buffered motions profile parameters upon a motion-based override change.
|
||||
@@ -306,8 +323,9 @@ uint8_t plan_buffer_line(float* target, plan_line_data_t* pl_data) {
|
||||
#else
|
||||
memcpy(position_steps, sys_position, sizeof(sys_position));
|
||||
#endif
|
||||
} else
|
||||
} else {
|
||||
memcpy(position_steps, pl.position, sizeof(pl.position));
|
||||
}
|
||||
#ifdef COREXY
|
||||
target_steps[A_MOTOR] = lround(target[A_MOTOR] * axis_settings[A_MOTOR]->steps_per_mm->get());
|
||||
target_steps[B_MOTOR] = lround(target[B_MOTOR] * axis_settings[B_MOTOR]->steps_per_mm->get());
|
||||
@@ -324,14 +342,15 @@ uint8_t plan_buffer_line(float* target, plan_line_data_t* pl_data) {
|
||||
block->steps[idx] = labs(target_steps[idx] - position_steps[idx]);
|
||||
}
|
||||
block->step_event_count = MAX(block->step_event_count, block->steps[idx]);
|
||||
if (idx == A_MOTOR)
|
||||
if (idx == A_MOTOR) {
|
||||
delta_mm = (target_steps[X_AXIS] - position_steps[X_AXIS] + target_steps[Y_AXIS] - position_steps[Y_AXIS]) /
|
||||
axis_settings[idx]->steps_per_mm->get();
|
||||
else if (idx == B_MOTOR)
|
||||
} else if (idx == B_MOTOR) {
|
||||
delta_mm = (target_steps[X_AXIS] - position_steps[X_AXIS] - target_steps[Y_AXIS] + position_steps[Y_AXIS]) /
|
||||
axis_settings[idx]->steps_per_mm->get();
|
||||
else
|
||||
} else {
|
||||
delta_mm = (target_steps[idx] - position_steps[idx]) / axis_settings[idx]->steps_per_mm->get();
|
||||
}
|
||||
#else
|
||||
target_steps[idx] = lround(target[idx] * axis_settings[idx]->steps_per_mm->get());
|
||||
block->steps[idx] = labs(target_steps[idx] - position_steps[idx]);
|
||||
@@ -340,12 +359,15 @@ uint8_t plan_buffer_line(float* target, plan_line_data_t* pl_data) {
|
||||
#endif
|
||||
unit_vec[idx] = delta_mm; // Store unit vector numerator
|
||||
// Set direction bits. Bit enabled always means direction is negative.
|
||||
if (delta_mm < 0.0)
|
||||
if (delta_mm < 0.0) {
|
||||
block->direction_bits |= get_direction_pin_mask(idx);
|
||||
}
|
||||
}
|
||||
// Bail if this is a zero-length block. Highly unlikely to occur.
|
||||
if (block->step_event_count == 0)
|
||||
return (PLAN_EMPTY_BLOCK);
|
||||
if (block->step_event_count == 0) {
|
||||
return PLAN_EMPTY_BLOCK;
|
||||
}
|
||||
|
||||
// Calculate the unit vector of the line move and the block maximum feed rate and acceleration scaled
|
||||
// down such that no individual axes maximum values are exceeded with respect to the line direction.
|
||||
// NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes,
|
||||
@@ -354,12 +376,13 @@ uint8_t plan_buffer_line(float* target, plan_line_data_t* pl_data) {
|
||||
block->acceleration = limit_acceleration_by_axis_maximum(unit_vec);
|
||||
block->rapid_rate = limit_rate_by_axis_maximum(unit_vec);
|
||||
// Store programmed rate.
|
||||
if (block->motion & PL_MOTION_RAPID_MOTION)
|
||||
if (block->motion & PL_MOTION_RAPID_MOTION) {
|
||||
block->programmed_rate = block->rapid_rate;
|
||||
else {
|
||||
} else {
|
||||
block->programmed_rate = pl_data->feed_rate;
|
||||
if (block->motion & PL_MOTION_INVERSE_TIME)
|
||||
if (block->motion & PL_MOTION_INVERSE_TIME) {
|
||||
block->programmed_rate *= block->millimeters;
|
||||
}
|
||||
}
|
||||
// TODO: Need to check this method handling zero junction speeds when starting from rest.
|
||||
if ((block_buffer_head == block_buffer_tail) || (block->motion & PL_MOTION_SYSTEM_MOTION)) {
|
||||
@@ -427,7 +450,7 @@ uint8_t plan_buffer_line(float* target, plan_line_data_t* pl_data) {
|
||||
// Finish up by recalculating the plan with the new block.
|
||||
planner_recalculate();
|
||||
}
|
||||
return (PLAN_OK);
|
||||
return PLAN_OK;
|
||||
}
|
||||
|
||||
// Reset the planner position vectors. Called by the system abort/initialization routine.
|
||||
@@ -437,12 +460,13 @@ void plan_sync_position() {
|
||||
uint8_t idx;
|
||||
for (idx = 0; idx < N_AXIS; idx++) {
|
||||
#ifdef COREXY
|
||||
if (idx == X_AXIS)
|
||||
if (idx == X_AXIS) {
|
||||
pl.position[X_AXIS] = system_convert_corexy_to_x_axis_steps(sys_position);
|
||||
else if (idx == Y_AXIS)
|
||||
} else if (idx == Y_AXIS) {
|
||||
pl.position[Y_AXIS] = system_convert_corexy_to_y_axis_steps(sys_position);
|
||||
else
|
||||
} else {
|
||||
pl.position[idx] = sys_position[idx];
|
||||
}
|
||||
#else
|
||||
pl.position[idx] = sys_position[idx];
|
||||
#endif
|
||||
@@ -451,17 +475,21 @@ void plan_sync_position() {
|
||||
|
||||
// Returns the number of available blocks are in the planner buffer.
|
||||
uint8_t plan_get_block_buffer_available() {
|
||||
if (block_buffer_head >= block_buffer_tail)
|
||||
return ((BLOCK_BUFFER_SIZE - 1) - (block_buffer_head - block_buffer_tail));
|
||||
return ((block_buffer_tail - block_buffer_head - 1));
|
||||
if (block_buffer_head >= block_buffer_tail) {
|
||||
return (BLOCK_BUFFER_SIZE - 1) - (block_buffer_head - block_buffer_tail);
|
||||
} else {
|
||||
return block_buffer_tail - block_buffer_head - 1;
|
||||
}
|
||||
}
|
||||
|
||||
// Returns the number of active blocks are in the planner buffer.
|
||||
// NOTE: Deprecated. Not used unless classic status reports are enabled in config.h
|
||||
uint8_t plan_get_block_buffer_count() {
|
||||
if (block_buffer_head >= block_buffer_tail)
|
||||
return (block_buffer_head - block_buffer_tail);
|
||||
return (BLOCK_BUFFER_SIZE - (block_buffer_tail - block_buffer_head));
|
||||
if (block_buffer_head >= block_buffer_tail) {
|
||||
return block_buffer_head - block_buffer_tail;
|
||||
} else {
|
||||
return BLOCK_BUFFER_SIZE - (block_buffer_tail - block_buffer_head);
|
||||
}
|
||||
}
|
||||
|
||||
// Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail.
|
||||
|
@@ -46,16 +46,18 @@ void probe_init() {
|
||||
// and the probing cycle modes for toward-workpiece/away-from-workpiece.
|
||||
void probe_configure_invert_mask(uint8_t is_probe_away) {
|
||||
probe_invert_mask = 0; // Initialize as zero.
|
||||
if (probe_invert->get())
|
||||
if (probe_invert->get()) {
|
||||
probe_invert_mask ^= PROBE_MASK;
|
||||
if (is_probe_away)
|
||||
}
|
||||
if (is_probe_away) {
|
||||
probe_invert_mask ^= PROBE_MASK;
|
||||
}
|
||||
}
|
||||
|
||||
// Returns the probe pin state. Triggered = true. Called by gcode parser and probe state monitor.
|
||||
uint8_t probe_get_state() {
|
||||
#ifdef PROBE_PIN
|
||||
return ((digitalRead(PROBE_PIN)) ^ probe_invert_mask);
|
||||
return digitalRead(PROBE_PIN) ^ probe_invert_mask;
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
|
@@ -46,10 +46,11 @@ void settings_restore(uint8_t restore_flag) {
|
||||
for (Setting* s = Setting::List; s; s = s->next()) {
|
||||
if (!s->getDescription()) {
|
||||
const char* name = s->getName();
|
||||
if (restore_startup) // all settings get restored
|
||||
if (restore_startup) { // all settings get restored
|
||||
s->setDefault();
|
||||
else if ((strcmp(name, "Line0") != 0) && (strcmp(name, "Line1") != 0)) // non startup settings get restored
|
||||
} else if ((strcmp(name, "Line0") != 0) && (strcmp(name, "Line1") != 0)) { // non startup settings get restored
|
||||
s->setDefault();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -57,8 +58,9 @@ void settings_restore(uint8_t restore_flag) {
|
||||
uint8_t idx;
|
||||
float coord_data[N_AXIS];
|
||||
memset(&coord_data, 0, sizeof(coord_data));
|
||||
for (idx = 0; idx <= SETTING_INDEX_NCOORD; idx++)
|
||||
for (idx = 0; idx <= SETTING_INDEX_NCOORD; idx++) {
|
||||
settings_write_coord_data(idx, coord_data);
|
||||
}
|
||||
}
|
||||
if (restore_flag & SETTINGS_RESTORE_BUILD_INFO) {
|
||||
EEPROM.write(EEPROM_ADDR_BUILD_INFO, 0);
|
||||
@@ -96,10 +98,11 @@ void settings_init() {
|
||||
// it early is probably prudent.
|
||||
uint8_t jog_set(uint8_t* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) {
|
||||
// Execute only if in IDLE or JOG states.
|
||||
if (sys.state != STATE_IDLE && sys.state != STATE_JOG)
|
||||
if (sys.state != STATE_IDLE && sys.state != STATE_JOG) {
|
||||
return STATUS_IDLE_ERROR;
|
||||
}
|
||||
|
||||
// restore the $J= prefix because gc_execute_line() expects it
|
||||
// restore the $J= prefix because gc_execute_line() expects it
|
||||
#define MAXLINE 128
|
||||
char line[MAXLINE];
|
||||
strcpy(line, "$J=");
|
||||
@@ -183,8 +186,9 @@ err_t toggle_check_mode(const char* value, WebUI::AuthenticationLevel auth_level
|
||||
mc_reset();
|
||||
report_feedback_message(MESSAGE_DISABLED);
|
||||
} else {
|
||||
if (sys.state)
|
||||
return (STATUS_IDLE_ERROR); // Requires no alarm mode.
|
||||
if (sys.state) {
|
||||
return STATUS_IDLE_ERROR; // Requires no alarm mode.
|
||||
}
|
||||
sys.state = STATE_CHECK_MODE;
|
||||
report_feedback_message(MESSAGE_ENABLED);
|
||||
}
|
||||
@@ -193,8 +197,9 @@ err_t toggle_check_mode(const char* value, WebUI::AuthenticationLevel auth_level
|
||||
err_t disable_alarm_lock(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) {
|
||||
if (sys.state == STATE_ALARM) {
|
||||
// Block if safety door is ajar.
|
||||
if (system_check_safety_door_ajar())
|
||||
return (STATUS_CHECK_DOOR);
|
||||
if (system_check_safety_door_ajar()) {
|
||||
return STATUS_CHECK_DOOR;
|
||||
}
|
||||
report_feedback_message(MESSAGE_ALARM_UNLOCK);
|
||||
sys.state = STATE_IDLE;
|
||||
// Don't run startup script. Prevents stored moves in startup from causing accidents.
|
||||
@@ -206,11 +211,13 @@ err_t report_ngc(const char* value, WebUI::AuthenticationLevel auth_level, WebUI
|
||||
return STATUS_OK;
|
||||
}
|
||||
err_t home(int cycle) {
|
||||
if (homing_enable->get() == false)
|
||||
return (STATUS_SETTING_DISABLED);
|
||||
if (system_check_safety_door_ajar())
|
||||
return (STATUS_CHECK_DOOR); // Block if safety door is ajar.
|
||||
sys.state = STATE_HOMING; // Set system state variable
|
||||
if (homing_enable->get() == false) {
|
||||
return STATUS_SETTING_DISABLED;
|
||||
}
|
||||
if (system_check_safety_door_ajar()) {
|
||||
return STATUS_CHECK_DOOR; // Block if safety door is ajar.
|
||||
}
|
||||
sys.state = STATE_HOMING; // Set system state variable
|
||||
#ifdef USE_I2S_STEPS
|
||||
stepper_id_t save_stepper = current_stepper;
|
||||
if (save_stepper == ST_I2S_STREAM) {
|
||||
|
@@ -45,8 +45,9 @@ static void empty_line(uint8_t client) {
|
||||
cl->buffer[0] = '\0';
|
||||
}
|
||||
static void empty_lines() {
|
||||
for (uint8_t client = 0; client < CLIENT_COUNT; client++)
|
||||
for (uint8_t client = 0; client < CLIENT_COUNT; client++) {
|
||||
empty_line(client);
|
||||
}
|
||||
}
|
||||
|
||||
err_t add_char_to_line(char c, uint8_t client) {
|
||||
@@ -60,8 +61,9 @@ err_t add_char_to_line(char c, uint8_t client) {
|
||||
}
|
||||
return STATUS_OK;
|
||||
}
|
||||
if (cl->len == (LINE_BUFFER_SIZE - 1))
|
||||
if (cl->len == (LINE_BUFFER_SIZE - 1)) {
|
||||
return STATUS_OVERFLOW;
|
||||
}
|
||||
if (c == '\r' || c == '\n') {
|
||||
cl->len = 0;
|
||||
cl->line_number++;
|
||||
@@ -75,14 +77,17 @@ err_t add_char_to_line(char c, uint8_t client) {
|
||||
err_t execute_line(char* line, uint8_t client, WebUI::AuthenticationLevel auth_level) {
|
||||
err_t result = STATUS_OK;
|
||||
// Empty or comment line. For syncing purposes.
|
||||
if (line[0] == 0)
|
||||
if (line[0] == 0) {
|
||||
return STATUS_OK;
|
||||
}
|
||||
// Grbl '$' or WebUI '[ESPxxx]' system command
|
||||
if (line[0] == '$' || line[0] == '[')
|
||||
if (line[0] == '$' || line[0] == '[') {
|
||||
return system_execute_line(line, client, auth_level);
|
||||
}
|
||||
// Everything else is gcode. Block if in alarm or jog mode.
|
||||
if (sys.state & (STATE_ALARM | STATE_JOG))
|
||||
if (sys.state & (STATE_ALARM | STATE_JOG)) {
|
||||
return STATUS_SYSTEM_GC_LOCK;
|
||||
}
|
||||
return gc_execute_line(line, client);
|
||||
}
|
||||
|
||||
@@ -186,8 +191,9 @@ void protocol_main_loop() {
|
||||
}
|
||||
// check to see if we should disable the stepper drivers ... esp32 work around for disable in main loop.
|
||||
if (stepper_idle) {
|
||||
if (esp_timer_get_time() > stepper_idle_counter)
|
||||
if (esp_timer_get_time() > stepper_idle_counter) {
|
||||
motors_set_disable(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
return; /* Never reached */
|
||||
@@ -200,8 +206,9 @@ void protocol_buffer_synchronize() {
|
||||
protocol_auto_cycle_start();
|
||||
do {
|
||||
protocol_execute_realtime(); // Check and execute run-time commands
|
||||
if (sys.abort)
|
||||
if (sys.abort) {
|
||||
return; // Check for system abort
|
||||
}
|
||||
} while (plan_get_current_block() || (sys.state == STATE_CYCLE));
|
||||
}
|
||||
|
||||
@@ -230,8 +237,9 @@ void protocol_auto_cycle_start() {
|
||||
// limit switches, or the main program.
|
||||
void protocol_execute_realtime() {
|
||||
protocol_exec_rt_system();
|
||||
if (sys.suspend)
|
||||
if (sys.suspend) {
|
||||
protocol_exec_rt_suspend();
|
||||
}
|
||||
}
|
||||
|
||||
// Executes run-time commands, when required. This function primarily operates as Grbl's state
|
||||
@@ -283,28 +291,32 @@ void protocol_exec_rt_system() {
|
||||
st_update_plan_block_parameters(); // Notify stepper module to recompute for hold deceleration.
|
||||
sys.step_control = STEP_CONTROL_EXECUTE_HOLD; // Initiate suspend state with active flag.
|
||||
if (sys.state == STATE_JOG) { // Jog cancelled upon any hold event, except for sleeping.
|
||||
if (!(rt_exec & EXEC_SLEEP))
|
||||
if (!(rt_exec & EXEC_SLEEP)) {
|
||||
sys.suspend |= SUSPEND_JOG_CANCEL;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// If IDLE, Grbl is not in motion. Simply indicate suspend state and hold is complete.
|
||||
if (sys.state == STATE_IDLE)
|
||||
if (sys.state == STATE_IDLE) {
|
||||
sys.suspend = SUSPEND_HOLD_COMPLETE;
|
||||
}
|
||||
// Execute and flag a motion cancel with deceleration and return to idle. Used primarily by probing cycle
|
||||
// to halt and cancel the remainder of the motion.
|
||||
if (rt_exec & EXEC_MOTION_CANCEL) {
|
||||
// MOTION_CANCEL only occurs during a CYCLE, but a HOLD and SAFETY_DOOR may been initiated beforehand
|
||||
// to hold the CYCLE. Motion cancel is valid for a single planner block motion only, while jog cancel
|
||||
// will handle and clear multiple planner block motions.
|
||||
if (!(sys.state & STATE_JOG))
|
||||
if (!(sys.state & STATE_JOG)) {
|
||||
sys.suspend |= SUSPEND_MOTION_CANCEL; // NOTE: State is STATE_CYCLE.
|
||||
}
|
||||
}
|
||||
// Execute a feed hold with deceleration, if required. Then, suspend system.
|
||||
if (rt_exec & EXEC_FEED_HOLD) {
|
||||
// Block SAFETY_DOOR, JOG, and SLEEP states from changing to HOLD state.
|
||||
if (!(sys.state & (STATE_SAFETY_DOOR | STATE_JOG | STATE_SLEEP)))
|
||||
if (!(sys.state & (STATE_SAFETY_DOOR | STATE_JOG | STATE_SLEEP))) {
|
||||
sys.state = STATE_HOLD;
|
||||
}
|
||||
}
|
||||
// Execute a safety door stop with a feed hold and disable spindle/coolant.
|
||||
// NOTE: Safety door differs from feed holds by stopping everything no matter state, disables powered
|
||||
@@ -329,8 +341,9 @@ void protocol_exec_rt_system() {
|
||||
sys.suspend |= SUSPEND_RESTART_RETRACT;
|
||||
}
|
||||
}
|
||||
if (sys.state != STATE_SLEEP)
|
||||
if (sys.state != STATE_SLEEP) {
|
||||
sys.state = STATE_SAFETY_DOOR;
|
||||
}
|
||||
}
|
||||
// NOTE: This flag doesn't change when the door closes, unlike sys.state. Ensures any parking motions
|
||||
// are executed if the door switch closes and the state returns to HOLD.
|
||||
@@ -338,8 +351,9 @@ void protocol_exec_rt_system() {
|
||||
}
|
||||
}
|
||||
if (rt_exec & EXEC_SLEEP) {
|
||||
if (sys.state == STATE_ALARM)
|
||||
if (sys.state == STATE_ALARM) {
|
||||
sys.suspend |= (SUSPEND_RETRACT_COMPLETE | SUSPEND_HOLD_COMPLETE);
|
||||
}
|
||||
sys.state = STATE_SLEEP;
|
||||
}
|
||||
system_clear_exec_state_flag((EXEC_MOTION_CANCEL | EXEC_FEED_HOLD | EXEC_SAFETY_DOOR | EXEC_SLEEP));
|
||||
@@ -393,8 +407,9 @@ void protocol_exec_rt_system() {
|
||||
// Hold complete. Set to indicate ready to resume. Remain in HOLD or DOOR states until user
|
||||
// has issued a resume command or reset.
|
||||
plan_cycle_reinitialize();
|
||||
if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD)
|
||||
if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) {
|
||||
sys.suspend |= SUSPEND_HOLD_COMPLETE;
|
||||
}
|
||||
bit_false(sys.step_control, (STEP_CONTROL_EXECUTE_HOLD | STEP_CONTROL_EXECUTE_SYS_MOTION));
|
||||
} else {
|
||||
// Motion complete. Includes CYCLE/JOG/HOMING states and jog cancel/motion cancel/soft limit events.
|
||||
@@ -423,25 +438,33 @@ void protocol_exec_rt_system() {
|
||||
if (rt_exec) {
|
||||
system_clear_exec_motion_overrides(); // Clear all motion override flags.
|
||||
uint8_t new_f_override = sys.f_override;
|
||||
if (rt_exec & EXEC_FEED_OVR_RESET)
|
||||
if (rt_exec & EXEC_FEED_OVR_RESET) {
|
||||
new_f_override = DEFAULT_FEED_OVERRIDE;
|
||||
if (rt_exec & EXEC_FEED_OVR_COARSE_PLUS)
|
||||
}
|
||||
if (rt_exec & EXEC_FEED_OVR_COARSE_PLUS) {
|
||||
new_f_override += FEED_OVERRIDE_COARSE_INCREMENT;
|
||||
if (rt_exec & EXEC_FEED_OVR_COARSE_MINUS)
|
||||
}
|
||||
if (rt_exec & EXEC_FEED_OVR_COARSE_MINUS) {
|
||||
new_f_override -= FEED_OVERRIDE_COARSE_INCREMENT;
|
||||
if (rt_exec & EXEC_FEED_OVR_FINE_PLUS)
|
||||
}
|
||||
if (rt_exec & EXEC_FEED_OVR_FINE_PLUS) {
|
||||
new_f_override += FEED_OVERRIDE_FINE_INCREMENT;
|
||||
if (rt_exec & EXEC_FEED_OVR_FINE_MINUS)
|
||||
}
|
||||
if (rt_exec & EXEC_FEED_OVR_FINE_MINUS) {
|
||||
new_f_override -= FEED_OVERRIDE_FINE_INCREMENT;
|
||||
}
|
||||
new_f_override = MIN(new_f_override, MAX_FEED_RATE_OVERRIDE);
|
||||
new_f_override = MAX(new_f_override, MIN_FEED_RATE_OVERRIDE);
|
||||
uint8_t new_r_override = sys.r_override;
|
||||
if (rt_exec & EXEC_RAPID_OVR_RESET)
|
||||
if (rt_exec & EXEC_RAPID_OVR_RESET) {
|
||||
new_r_override = DEFAULT_RAPID_OVERRIDE;
|
||||
if (rt_exec & EXEC_RAPID_OVR_MEDIUM)
|
||||
}
|
||||
if (rt_exec & EXEC_RAPID_OVR_MEDIUM) {
|
||||
new_r_override = RAPID_OVERRIDE_MEDIUM;
|
||||
if (rt_exec & EXEC_RAPID_OVR_LOW)
|
||||
}
|
||||
if (rt_exec & EXEC_RAPID_OVR_LOW) {
|
||||
new_r_override = RAPID_OVERRIDE_LOW;
|
||||
}
|
||||
if ((new_f_override != sys.f_override) || (new_r_override != sys.r_override)) {
|
||||
sys.f_override = new_f_override;
|
||||
sys.r_override = new_r_override;
|
||||
@@ -455,16 +478,21 @@ void protocol_exec_rt_system() {
|
||||
system_clear_exec_accessory_overrides(); // Clear all accessory override flags.
|
||||
// NOTE: Unlike motion overrides, spindle overrides do not require a planner reinitialization.
|
||||
uint8_t last_s_override = sys.spindle_speed_ovr;
|
||||
if (rt_exec & EXEC_SPINDLE_OVR_RESET)
|
||||
if (rt_exec & EXEC_SPINDLE_OVR_RESET) {
|
||||
last_s_override = DEFAULT_SPINDLE_SPEED_OVERRIDE;
|
||||
if (rt_exec & EXEC_SPINDLE_OVR_COARSE_PLUS)
|
||||
}
|
||||
if (rt_exec & EXEC_SPINDLE_OVR_COARSE_PLUS) {
|
||||
last_s_override += SPINDLE_OVERRIDE_COARSE_INCREMENT;
|
||||
if (rt_exec & EXEC_SPINDLE_OVR_COARSE_MINUS)
|
||||
}
|
||||
if (rt_exec & EXEC_SPINDLE_OVR_COARSE_MINUS) {
|
||||
last_s_override -= SPINDLE_OVERRIDE_COARSE_INCREMENT;
|
||||
if (rt_exec & EXEC_SPINDLE_OVR_FINE_PLUS)
|
||||
}
|
||||
if (rt_exec & EXEC_SPINDLE_OVR_FINE_PLUS) {
|
||||
last_s_override += SPINDLE_OVERRIDE_FINE_INCREMENT;
|
||||
if (rt_exec & EXEC_SPINDLE_OVR_FINE_MINUS)
|
||||
}
|
||||
if (rt_exec & EXEC_SPINDLE_OVR_FINE_MINUS) {
|
||||
last_s_override -= SPINDLE_OVERRIDE_FINE_INCREMENT;
|
||||
}
|
||||
last_s_override = MIN(last_s_override, MAX_SPINDLE_SPEED_OVERRIDE);
|
||||
last_s_override = MAX(last_s_override, MIN_SPINDLE_SPEED_OVERRIDE);
|
||||
if (last_s_override != sys.spindle_speed_ovr) {
|
||||
@@ -472,17 +500,19 @@ void protocol_exec_rt_system() {
|
||||
sys.spindle_speed_ovr = last_s_override;
|
||||
sys.report_ovr_counter = 0; // Set to report change immediately
|
||||
// If spinlde is on, tell it the rpm has been overridden
|
||||
if (gc_state.modal.spindle != SpindleState::Disable)
|
||||
if (gc_state.modal.spindle != SpindleState::Disable) {
|
||||
spindle->set_rpm(gc_state.spindle_speed);
|
||||
}
|
||||
}
|
||||
if (rt_exec & EXEC_SPINDLE_OVR_STOP) {
|
||||
// Spindle stop override allowed only while in HOLD state.
|
||||
// NOTE: Report counters are set in spindle_set_state() when spindle stop is executed.
|
||||
if (sys.state == STATE_HOLD) {
|
||||
if (!(sys.spindle_stop_ovr))
|
||||
if (!(sys.spindle_stop_ovr)) {
|
||||
sys.spindle_stop_ovr = SPINDLE_STOP_OVR_INITIATE;
|
||||
else if (sys.spindle_stop_ovr & SPINDLE_STOP_OVR_ENABLED)
|
||||
} else if (sys.spindle_stop_ovr & SPINDLE_STOP_OVR_ENABLED) {
|
||||
sys.spindle_stop_ovr |= SPINDLE_STOP_OVR_RESTORE;
|
||||
}
|
||||
}
|
||||
}
|
||||
// NOTE: Since coolant state always performs a planner sync whenever it changes, the current
|
||||
@@ -492,18 +522,20 @@ void protocol_exec_rt_system() {
|
||||
CoolantMode coolant_state = gc_state.modal.coolant;
|
||||
#ifdef COOLANT_FLOOD_PIN
|
||||
if (rt_exec & EXEC_COOLANT_FLOOD_OVR_TOGGLE) {
|
||||
if (coolant_state & CoolantMode::Flood)
|
||||
bit_false(coolant_state, CoolantMode::Flood);
|
||||
else
|
||||
coolant_state |= CoolantMode::Flood;
|
||||
if (coolant_state.Flood) {
|
||||
coolant_state.Flood = 0;
|
||||
} else {
|
||||
coolant_state.Flood = 1;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#ifdef COOLANT_MIST_PIN
|
||||
if (rt_exec & EXEC_COOLANT_MIST_OVR_TOGGLE) {
|
||||
if (coolant_state & CoolantMode::Mist)
|
||||
bit_false(coolant_state, CoolantMode::Mist);
|
||||
else
|
||||
coolant_state |= CoolantMode::Mist;
|
||||
if (coolant_state.Mist) {
|
||||
coolant_state.Mist = 0;
|
||||
} else {
|
||||
coolant_state.Mist = 1;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
coolant_set_state(coolant_state); // Report counter set in coolant_set_state().
|
||||
@@ -518,8 +550,9 @@ void protocol_exec_rt_system() {
|
||||
}
|
||||
#endif
|
||||
// Reload step segment buffer
|
||||
if (sys.state & (STATE_CYCLE | STATE_HOLD | STATE_SAFETY_DOOR | STATE_HOMING | STATE_SLEEP | STATE_JOG))
|
||||
if (sys.state & (STATE_CYCLE | STATE_HOLD | STATE_SAFETY_DOOR | STATE_HOMING | STATE_SLEEP | STATE_JOG)) {
|
||||
st_prep_buffer();
|
||||
}
|
||||
}
|
||||
|
||||
// Handles Grbl system suspend procedures, such as feed hold, safety door, and parking motion.
|
||||
@@ -555,13 +588,15 @@ static void protocol_exec_rt_suspend() {
|
||||
restore_spindle_speed = block->spindle_speed;
|
||||
}
|
||||
#ifdef DISABLE_LASER_DURING_HOLD
|
||||
if (laser_mode->get())
|
||||
if (laser_mode->get()) {
|
||||
system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_STOP);
|
||||
}
|
||||
#endif
|
||||
|
||||
while (sys.suspend) {
|
||||
if (sys.abort)
|
||||
if (sys.abort) {
|
||||
return;
|
||||
}
|
||||
// Block until initial hold is complete and the machine has stopped motion.
|
||||
if (sys.suspend & SUSPEND_HOLD_COMPLETE) {
|
||||
// Parking manager. Handles de/re-energizing, switch state checks, and parking motions for
|
||||
@@ -573,7 +608,7 @@ static void protocol_exec_rt_suspend() {
|
||||
sys.spindle_stop_ovr = SPINDLE_STOP_OVR_DISABLED;
|
||||
#ifndef PARKING_ENABLE
|
||||
spindle->set_state(SpindleState::Disable, 0); // De-energize
|
||||
coolant_set_state(CoolantMode::Disable); // De-energize
|
||||
coolant_set_state(CoolantMode()); // De-energize
|
||||
#else
|
||||
// Get current position and store restore location and spindle retract waypoint.
|
||||
system_convert_array_steps_to_mpos(parking_target, sys_position);
|
||||
@@ -598,11 +633,11 @@ static void protocol_exec_rt_suspend() {
|
||||
}
|
||||
// NOTE: Clear accessory state after retract and after an aborted restore motion.
|
||||
pl_data->spindle = SpindleState::Disable;
|
||||
pl_data->coolant = CoolantMode::Disable;
|
||||
pl_data->coolant = CoolantMode();
|
||||
pl_data->motion = (PL_MOTION_SYSTEM_MOTION | PL_MOTION_NO_FEED_OVERRIDE);
|
||||
pl_data->spindle_speed = 0.0;
|
||||
spindle->set_state(SpindleState::Disable, 0); // De-energize
|
||||
coolant_set_state(CoolantMode::Disable); // De-energize
|
||||
coolant_set_state(CoolantMode()); // De-energize
|
||||
// Execute fast parking retract motion to parking target location.
|
||||
if (parking_target[PARKING_AXIS] < PARKING_TARGET) {
|
||||
parking_target[PARKING_AXIS] = PARKING_TARGET;
|
||||
@@ -613,7 +648,7 @@ static void protocol_exec_rt_suspend() {
|
||||
// Parking motion not possible. Just disable the spindle and coolant.
|
||||
// NOTE: Laser mode does not start a parking motion to ensure the laser stops immediately.
|
||||
spindle->set_state(SpindleState::Disable, 0); // De-energize
|
||||
coolant_set_state(CoolantMode::Disable); // De-energize
|
||||
coolant_set_state(CoolantMode()); // De-energize
|
||||
}
|
||||
#endif
|
||||
sys.suspend &= ~(SUSPEND_RESTART_RETRACT);
|
||||
@@ -623,11 +658,12 @@ static void protocol_exec_rt_suspend() {
|
||||
report_feedback_message(MESSAGE_SLEEP_MODE);
|
||||
// Spindle and coolant should already be stopped, but do it again just to be sure.
|
||||
spindle->set_state(SpindleState::Disable, 0); // De-energize
|
||||
coolant_set_state(CoolantMode::Disable); // De-energize
|
||||
coolant_set_state(CoolantMode()); // De-energize
|
||||
st_go_idle(); // Disable steppers
|
||||
while (!(sys.abort))
|
||||
while (!(sys.abort)) {
|
||||
protocol_exec_rt_system(); // Do nothing until reset.
|
||||
return; // Abort received. Return to re-initialize.
|
||||
}
|
||||
return; // Abort received. Return to re-initialize.
|
||||
}
|
||||
// Allows resuming from parking/safety door. Actively checks if safety door is closed and ready to resume.
|
||||
if (sys.state == STATE_SAFETY_DOOR) {
|
||||
@@ -662,7 +698,7 @@ static void protocol_exec_rt_suspend() {
|
||||
}
|
||||
}
|
||||
}
|
||||
if (gc_state.modal.coolant != CoolantMode::Disable) {
|
||||
if (!gc_state.modal.coolant.IsDisabled()) {
|
||||
// Block if safety door re-opened during prior restore actions.
|
||||
if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) {
|
||||
// NOTE: Laser mode will honor this delay. An exhaust system is often controlled by this pin.
|
||||
|
@@ -54,8 +54,9 @@ EspClass esp;
|
||||
|
||||
// this is a generic send function that everything should use, so interfaces could be added (Bluetooth, etc)
|
||||
void grbl_send(uint8_t client, const char* text) {
|
||||
if (client == CLIENT_INPUT)
|
||||
if (client == CLIENT_INPUT) {
|
||||
return;
|
||||
}
|
||||
#ifdef ENABLE_BLUETOOTH
|
||||
if (WebUI::SerialBT.hasClient() && (client == CLIENT_BT || client == CLIENT_ALL)) {
|
||||
WebUI::SerialBT.print(text);
|
||||
@@ -63,12 +64,14 @@ void grbl_send(uint8_t client, const char* text) {
|
||||
}
|
||||
#endif
|
||||
#if defined(ENABLE_WIFI) && defined(ENABLE_HTTP) && defined(ENABLE_SERIAL2SOCKET_OUT)
|
||||
if (client == CLIENT_WEBUI || client == CLIENT_ALL)
|
||||
if (client == CLIENT_WEBUI || client == CLIENT_ALL) {
|
||||
WebUI::Serial2Socket.write((const uint8_t*)text, strlen(text));
|
||||
}
|
||||
#endif
|
||||
#if defined(ENABLE_WIFI) && defined(ENABLE_TELNET)
|
||||
if (client == CLIENT_TELNET || client == CLIENT_ALL)
|
||||
if (client == CLIENT_TELNET || client == CLIENT_ALL) {
|
||||
WebUI::telnet_server.write((const uint8_t*)text, strlen(text));
|
||||
}
|
||||
#endif
|
||||
if (client == CLIENT_SERIAL || client == CLIENT_ALL) {
|
||||
Serial.print(text);
|
||||
@@ -97,8 +100,9 @@ void grbl_sendf(uint8_t client, const char* format, ...) {
|
||||
len = vsnprintf(temp, len + 1, format, arg);
|
||||
grbl_send(client, temp);
|
||||
va_end(arg);
|
||||
if (len > 64)
|
||||
if (len > 64) {
|
||||
delete[] temp;
|
||||
}
|
||||
}
|
||||
// Use to send [MSG:xxxx] Type messages. The level allows messages to be easily suppressed
|
||||
void grbl_msg_sendf(uint8_t client, uint8_t level, const char* format, ...) {
|
||||
@@ -118,8 +122,9 @@ void grbl_msg_sendf(uint8_t client, uint8_t level, const char* format, ...) {
|
||||
va_end(copy);
|
||||
if (len >= sizeof(loc_buf)) {
|
||||
temp = new char[len + 1];
|
||||
if (temp == NULL)
|
||||
if (temp == NULL) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
len = vsnprintf(temp, len + 1, format, arg);
|
||||
grbl_sendf(client, "[MSG:%s]\r\n", temp);
|
||||
@@ -165,8 +170,9 @@ static void report_util_axis_values(float* axis_value, char* rpt) {
|
||||
char axisVal[20];
|
||||
float unit_conv = 1.0; // unit conversion multiplier..default is mm
|
||||
rpt[0] = '\0';
|
||||
if (report_inches->get())
|
||||
if (report_inches->get()) {
|
||||
unit_conv = 1.0 / MM_PER_INCH;
|
||||
}
|
||||
for (idx = 0; idx < N_AXIS; idx++) {
|
||||
if (report_inches->get()) {
|
||||
sprintf(axisVal, "%4.4f", axis_value[idx] * unit_conv); // Report inches to 4 decimals
|
||||
@@ -326,10 +332,11 @@ void report_ngc_parameters(uint8_t client) {
|
||||
strcat(ngc_rpt, temp);
|
||||
strcat(ngc_rpt, "]\r\n");
|
||||
strcat(ngc_rpt, "[TLO:"); // Print tool length offset value
|
||||
if (report_inches->get())
|
||||
if (report_inches->get()) {
|
||||
sprintf(temp, "%4.3f]\r\n", gc_state.tool_length_offset * INCH_PER_MM);
|
||||
else
|
||||
} else {
|
||||
sprintf(temp, "%4.3f]\r\n", gc_state.tool_length_offset);
|
||||
}
|
||||
strcat(ngc_rpt, temp);
|
||||
grbl_send(client, ngc_rpt);
|
||||
report_probe_parameters(client);
|
||||
@@ -410,23 +417,24 @@ void report_gcode_modes(uint8_t client) {
|
||||
strcat(modes_rpt, mode);
|
||||
|
||||
//report_util_gcode_modes_M(); // optional M7 and M8 should have been dealt with by here
|
||||
if (gc_state.modal.coolant == CoolantMode::Disable) {
|
||||
if (gc_state.modal.coolant.IsDisabled()) {
|
||||
mode = " M9";
|
||||
} else {
|
||||
uint8_t coolant = static_cast<uint8_t>(gc_state.modal.coolant);
|
||||
auto coolant = gc_state.modal.coolant;
|
||||
// Note: Multiple coolant states may be active at the same time.
|
||||
if (coolant & static_cast<uint8_t>(CoolantMode::Mist)) {
|
||||
if (coolant.Mist) {
|
||||
mode = " M7";
|
||||
}
|
||||
if (coolant & static_cast<uint8_t>(CoolantMode::Flood)) {
|
||||
if (coolant.Flood) {
|
||||
mode = " M8";
|
||||
}
|
||||
}
|
||||
strcat(modes_rpt, mode);
|
||||
|
||||
#ifdef ENABLE_PARKING_OVERRIDE_CONTROL
|
||||
if (sys.override_ctrl == OVERRIDE_PARKING_MOTION)
|
||||
if (sys.override_ctrl == OVERRIDE_PARKING_MOTION) {
|
||||
strcat(modes_rpt, " M56");
|
||||
}
|
||||
#endif
|
||||
|
||||
sprintf(temp, " T%d", gc_state.tool);
|
||||
@@ -548,10 +556,11 @@ void report_realtime_status(uint8_t client) {
|
||||
case STATE_HOLD:
|
||||
if (!(sys.suspend & SUSPEND_JOG_CANCEL)) {
|
||||
strcat(status, "Hold:");
|
||||
if (sys.suspend & SUSPEND_HOLD_COMPLETE)
|
||||
if (sys.suspend & SUSPEND_HOLD_COMPLETE) {
|
||||
strcat(status, "0"); // Ready to resume
|
||||
else
|
||||
} else {
|
||||
strcat(status, "1"); // Actively holding
|
||||
}
|
||||
break;
|
||||
} // Continues to print jog state during jog cancel.
|
||||
case STATE_JOG: strcat(status, "Jog"); break;
|
||||
@@ -566,8 +575,9 @@ void report_realtime_status(uint8_t client) {
|
||||
if (sys.suspend & SUSPEND_RETRACT_COMPLETE) {
|
||||
if (sys.suspend & SUSPEND_SAFETY_DOOR_AJAR) {
|
||||
strcat(status, "1"); // Door ajar
|
||||
} else
|
||||
} else {
|
||||
strcat(status, "0");
|
||||
}
|
||||
// Door closed and ready to resume
|
||||
} else {
|
||||
strcat(status, "2"); // Retracting
|
||||
@@ -581,16 +591,18 @@ void report_realtime_status(uint8_t client) {
|
||||
for (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)
|
||||
if (idx == TOOL_LENGTH_OFFSET_AXIS) {
|
||||
wco[idx] += gc_state.tool_length_offset;
|
||||
if (bit_isfalse(status_mask->get(), BITFLAG_RT_STATUS_POSITION_TYPE))
|
||||
}
|
||||
if (bit_isfalse(status_mask->get(), BITFLAG_RT_STATUS_POSITION_TYPE)) {
|
||||
print_position[idx] -= wco[idx];
|
||||
}
|
||||
}
|
||||
}
|
||||
// Report machine position
|
||||
if (bit_istrue(status_mask->get(), BITFLAG_RT_STATUS_POSITION_TYPE))
|
||||
if (bit_istrue(status_mask->get(), BITFLAG_RT_STATUS_POSITION_TYPE)) {
|
||||
strcat(status, "|MPos:");
|
||||
else {
|
||||
} else {
|
||||
#ifdef USE_FWD_KINEMATIC
|
||||
forward_kinematics(print_position);
|
||||
#endif
|
||||
@@ -603,8 +615,9 @@ void report_realtime_status(uint8_t client) {
|
||||
if (bit_istrue(status_mask->get(), BITFLAG_RT_STATUS_BUFFER_STATE)) {
|
||||
int bufsize = DEFAULTBUFFERSIZE;
|
||||
# if defined(ENABLE_WIFI) && defined(ENABLE_TELNET)
|
||||
if (client == CLIENT_TELNET)
|
||||
if (client == CLIENT_TELNET) {
|
||||
bufsize = WebUI::telnet_server.get_rx_buffer_available();
|
||||
}
|
||||
# endif //ENABLE_WIFI && ENABLE_TELNET
|
||||
# if defined(ENABLE_BLUETOOTH)
|
||||
if (client == CLIENT_BT) {
|
||||
@@ -612,8 +625,9 @@ void report_realtime_status(uint8_t client) {
|
||||
bufsize = 512 - WebUI::SerialBT.available();
|
||||
}
|
||||
# endif //ENABLE_BLUETOOTH
|
||||
if (client == CLIENT_SERIAL)
|
||||
if (client == CLIENT_SERIAL) {
|
||||
bufsize = serial_get_rx_buffer_available(CLIENT_SERIAL);
|
||||
}
|
||||
sprintf(temp, "|Bf:%d,%d", plan_get_block_buffer_available(), bufsize);
|
||||
strcat(status, temp);
|
||||
}
|
||||
@@ -633,10 +647,11 @@ void report_realtime_status(uint8_t client) {
|
||||
#endif
|
||||
// Report realtime feed speed
|
||||
#ifdef REPORT_FIELD_CURRENT_FEED_SPEED
|
||||
if (report_inches->get())
|
||||
if (report_inches->get()) {
|
||||
sprintf(temp, "|FS:%.1f,%d", st_get_realtime_rate() / MM_PER_INCH, sys.spindle_speed);
|
||||
else
|
||||
} else {
|
||||
sprintf(temp, "|FS:%.0f,%d", st_get_realtime_rate(), sys.spindle_speed);
|
||||
}
|
||||
strcat(status, temp);
|
||||
#endif
|
||||
#ifdef REPORT_FIELD_PIN_STATE
|
||||
@@ -645,70 +660,84 @@ void report_realtime_status(uint8_t client) {
|
||||
uint8_t prb_pin_state = probe_get_state();
|
||||
if (lim_pin_state | ctrl_pin_state | prb_pin_state) {
|
||||
strcat(status, "|Pn:");
|
||||
if (prb_pin_state)
|
||||
if (prb_pin_state) {
|
||||
strcat(status, "P");
|
||||
}
|
||||
if (lim_pin_state) {
|
||||
if (bit_istrue(lim_pin_state, bit(X_AXIS)))
|
||||
if (bit_istrue(lim_pin_state, bit(X_AXIS))) {
|
||||
strcat(status, "X");
|
||||
if (bit_istrue(lim_pin_state, bit(Y_AXIS)))
|
||||
}
|
||||
if (bit_istrue(lim_pin_state, bit(Y_AXIS))) {
|
||||
strcat(status, "Y");
|
||||
if (bit_istrue(lim_pin_state, bit(Z_AXIS)))
|
||||
}
|
||||
if (bit_istrue(lim_pin_state, bit(Z_AXIS))) {
|
||||
strcat(status, "Z");
|
||||
}
|
||||
# if (N_AXIS > A_AXIS)
|
||||
if (bit_istrue(lim_pin_state, bit(A_AXIS)))
|
||||
if (bit_istrue(lim_pin_state, bit(A_AXIS))) {
|
||||
strcat(status, "A");
|
||||
}
|
||||
# endif
|
||||
# if (N_AXIS > B_AXIS)
|
||||
if (bit_istrue(lim_pin_state, bit(B_AXIS)))
|
||||
if (bit_istrue(lim_pin_state, bit(B_AXIS))) {
|
||||
strcat(status, "B");
|
||||
}
|
||||
# endif
|
||||
# if (N_AXIS > C_AXIS)
|
||||
if (bit_istrue(lim_pin_state, bit(C_AXIS)))
|
||||
if (bit_istrue(lim_pin_state, bit(C_AXIS))) {
|
||||
strcat(status, "C");
|
||||
}
|
||||
# endif
|
||||
}
|
||||
if (ctrl_pin_state) {
|
||||
# ifdef ENABLE_SAFETY_DOOR_INPUT_PIN
|
||||
if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_SAFETY_DOOR))
|
||||
if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_SAFETY_DOOR)) {
|
||||
strcat(status, "D");
|
||||
}
|
||||
# endif
|
||||
if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_RESET))
|
||||
if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_RESET)) {
|
||||
strcat(status, "R");
|
||||
if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_FEED_HOLD))
|
||||
}
|
||||
if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_FEED_HOLD)) {
|
||||
strcat(status, "H");
|
||||
if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_CYCLE_START))
|
||||
}
|
||||
if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_CYCLE_START)) {
|
||||
strcat(status, "S");
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#ifdef REPORT_FIELD_WORK_COORD_OFFSET
|
||||
if (sys.report_wco_counter > 0)
|
||||
if (sys.report_wco_counter > 0) {
|
||||
sys.report_wco_counter--;
|
||||
else {
|
||||
} else {
|
||||
if (sys.state & (STATE_HOMING | STATE_CYCLE | STATE_HOLD | STATE_JOG | STATE_SAFETY_DOOR)) {
|
||||
sys.report_wco_counter = (REPORT_WCO_REFRESH_BUSY_COUNT - 1); // Reset counter for slow refresh
|
||||
} else
|
||||
} else {
|
||||
sys.report_wco_counter = (REPORT_WCO_REFRESH_IDLE_COUNT - 1);
|
||||
if (sys.report_ovr_counter == 0)
|
||||
}
|
||||
if (sys.report_ovr_counter == 0) {
|
||||
sys.report_ovr_counter = 1; // Set override on next report.
|
||||
}
|
||||
strcat(status, "|WCO:");
|
||||
report_util_axis_values(wco, temp);
|
||||
strcat(status, temp);
|
||||
}
|
||||
#endif
|
||||
#ifdef REPORT_FIELD_OVERRIDES
|
||||
if (sys.report_ovr_counter > 0)
|
||||
if (sys.report_ovr_counter > 0) {
|
||||
sys.report_ovr_counter--;
|
||||
else {
|
||||
} else {
|
||||
if (sys.state & (STATE_HOMING | STATE_CYCLE | STATE_HOLD | STATE_JOG | STATE_SAFETY_DOOR)) {
|
||||
sys.report_ovr_counter = (REPORT_OVR_REFRESH_BUSY_COUNT - 1); // Reset counter for slow refresh
|
||||
} else
|
||||
} else {
|
||||
sys.report_ovr_counter = (REPORT_OVR_REFRESH_IDLE_COUNT - 1);
|
||||
}
|
||||
sprintf(temp, "|Ov:%d,%d,%d", sys.f_override, sys.r_override, sys.spindle_speed_ovr);
|
||||
strcat(status, temp);
|
||||
SpindleState sp_state = spindle->get_state();
|
||||
CoolantMode coolant_state = coolant_get_state();
|
||||
if (sp_state != SpindleState::Disable || coolant_state != CoolantMode::Disable) {
|
||||
if (sp_state != SpindleState::Disable || !coolant_state.IsDisabled()) {
|
||||
strcat(status, "|A:");
|
||||
switch (sp_state) {
|
||||
case SpindleState::Disable: break;
|
||||
@@ -716,12 +745,14 @@ void report_realtime_status(uint8_t client) {
|
||||
case SpindleState::Ccw: strcat(status, "C"); break;
|
||||
}
|
||||
|
||||
uint8_t coolant = static_cast<uint8_t>(coolant_state);
|
||||
if (coolant & static_cast<uint8_t>(CoolantMode::Flood))
|
||||
auto coolant = coolant_state;
|
||||
if (coolant.Flood) {
|
||||
strcat(status, "F");
|
||||
}
|
||||
# ifdef COOLANT_MIST_PIN // TODO Deal with M8 - Flood
|
||||
if (coolant & static_cast<uint8_t>(CoolantMode::Mist))
|
||||
if (coolant.Mist) {
|
||||
strcat(status, "M");
|
||||
}
|
||||
# endif
|
||||
}
|
||||
}
|
||||
|
@@ -50,10 +50,12 @@ void listDir(fs::FS& fs, const char* dirname, uint8_t levels, uint8_t client) {
|
||||
File file = root.openNextFile();
|
||||
while (file) {
|
||||
if (file.isDirectory()) {
|
||||
if (levels)
|
||||
if (levels) {
|
||||
listDir(fs, file.name(), levels - 1, client);
|
||||
} else
|
||||
}
|
||||
} else {
|
||||
grbl_sendf(CLIENT_ALL, "[FILE:%s|SIZE:%d]\r\n", file.name(), file.size());
|
||||
}
|
||||
file = root.openNextFile();
|
||||
}
|
||||
}
|
||||
@@ -71,8 +73,9 @@ boolean openFile(fs::FS& fs, const char* path) {
|
||||
}
|
||||
|
||||
boolean closeFile() {
|
||||
if (!myFile)
|
||||
if (!myFile) {
|
||||
return false;
|
||||
}
|
||||
set_sd_state(SDCARD_IDLE);
|
||||
SD_ready_next = false;
|
||||
sd_current_line_number = 0;
|
||||
@@ -110,9 +113,10 @@ boolean readFileLine(char* line, int maxlen) {
|
||||
|
||||
// return a percentage complete 50.5 = 50.5%
|
||||
float sd_report_perc_complete() {
|
||||
if (!myFile)
|
||||
if (!myFile) {
|
||||
return 0.0;
|
||||
return ((float)myFile.position() / (float)myFile.size() * 100.0);
|
||||
}
|
||||
return (float)myFile.position() / (float)myFile.size() * 100.0f;
|
||||
}
|
||||
|
||||
uint32_t sd_get_current_line_number() {
|
||||
@@ -130,19 +134,22 @@ uint8_t get_sd_state(bool refresh) {
|
||||
}
|
||||
#endif
|
||||
//if busy doing something return state
|
||||
if (!((sd_state == SDCARD_NOT_PRESENT) || (sd_state == SDCARD_IDLE)))
|
||||
if (!((sd_state == SDCARD_NOT_PRESENT) || (sd_state == SDCARD_IDLE))) {
|
||||
return sd_state;
|
||||
}
|
||||
if (!refresh) {
|
||||
return sd_state; //to avoid refresh=true + busy to reset SD and waste time
|
||||
}
|
||||
|
||||
//SD is idle or not detected, let see if still the case
|
||||
SD.end();
|
||||
sd_state = SDCARD_NOT_PRESENT;
|
||||
//using default value for speed ? should be parameter
|
||||
//refresh content if card was removed
|
||||
if (SD.begin((GRBL_SPI_SS == -1) ? SS : GRBL_SPI_SS, SPI, GRBL_SPI_FREQ)) {
|
||||
if (SD.cardSize() > 0)
|
||||
if (SD.cardSize() > 0) {
|
||||
sd_state = SDCARD_IDLE;
|
||||
}
|
||||
}
|
||||
return sd_state;
|
||||
}
|
||||
@@ -153,8 +160,9 @@ uint8_t set_sd_state(uint8_t flag) {
|
||||
}
|
||||
|
||||
void sd_get_current_filename(char* name) {
|
||||
if (myFile)
|
||||
if (myFile) {
|
||||
strcpy(name, myFile.name());
|
||||
else
|
||||
} else {
|
||||
name[0] = 0;
|
||||
}
|
||||
}
|
||||
|
@@ -128,9 +128,9 @@ void serialCheckTask(void* pvParameters) {
|
||||
}
|
||||
// Pick off realtime command characters directly from the serial stream. These characters are
|
||||
// not passed into the main buffer, but these set system state flag bits for realtime execution.
|
||||
if (is_realtime_command(data))
|
||||
if (is_realtime_command(data)) {
|
||||
execute_realtime_command(data, client);
|
||||
else {
|
||||
} else {
|
||||
vTaskEnterCritical(&myMutex);
|
||||
client_buffer[client].write(data);
|
||||
vTaskExitCritical(&myMutex);
|
||||
@@ -194,7 +194,7 @@ bool any_client_has_data() {
|
||||
|
||||
// checks to see if a character is a realtime character
|
||||
bool is_realtime_command(uint8_t data) {
|
||||
return (data == CMD_RESET || data == CMD_STATUS_REPORT || data == CMD_CYCLE_START || data == CMD_FEED_HOLD || data > 0x7F);
|
||||
return data == CMD_RESET || data == CMD_STATUS_REPORT || data == CMD_CYCLE_START || data == CMD_FEED_HOLD || data > 0x7F;
|
||||
}
|
||||
|
||||
// Act upon a realtime character
|
||||
@@ -214,8 +214,9 @@ void execute_realtime_command(uint8_t command, uint8_t client) {
|
||||
break;
|
||||
case CMD_SAFETY_DOOR: system_set_exec_state_flag(EXEC_SAFETY_DOOR); break; // Set as true
|
||||
case CMD_JOG_CANCEL:
|
||||
if (sys.state & STATE_JOG) // Block all other states from invoking motion cancel.
|
||||
if (sys.state & STATE_JOG) { // Block all other states from invoking motion cancel.
|
||||
system_set_exec_state_flag(EXEC_MOTION_CANCEL);
|
||||
}
|
||||
break;
|
||||
#ifdef DEBUG
|
||||
case CMD_DEBUG_REPORT: {
|
||||
|
@@ -375,8 +375,9 @@ const char* StringSetting::getStringValue() {
|
||||
#endif
|
||||
_checker == (bool (*)(char*))WebUI::COMMANDS::isLocalPasswordValid)) {
|
||||
return "******";
|
||||
} else {
|
||||
return _currentValue.c_str();
|
||||
}
|
||||
return _currentValue.c_str();
|
||||
}
|
||||
|
||||
void StringSetting::addWebui(WebUI::JSONencoder* j) {
|
||||
|
@@ -91,8 +91,9 @@ public:
|
||||
|
||||
static err_t report_nvs_stats(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) {
|
||||
nvs_stats_t stats;
|
||||
if (err_t err = nvs_get_stats(NULL, &stats))
|
||||
if (err_t err = nvs_get_stats(NULL, &stats)) {
|
||||
return err;
|
||||
}
|
||||
grbl_sendf(out->client(), "[MSG: NVS Used: %d Free: %d Total: %d]\r\n", stats.used_entries, stats.free_entries, stats.total_entries);
|
||||
#if 0 // The SDK we use does not have this yet
|
||||
nvs_iterator_t it = nvs_entry_find(NULL, NULL, NVS_TYPE_ANY);
|
||||
|
@@ -154,8 +154,9 @@ static const char* makename(const char* axisName, const char* tail) {
|
||||
}
|
||||
|
||||
static bool checkStartupLine(char* value) {
|
||||
if (sys.state != STATE_IDLE)
|
||||
if (sys.state != STATE_IDLE) {
|
||||
return STATUS_IDLE_ERROR;
|
||||
}
|
||||
return gc_execute_line(value, CLIENT_SERIAL) == 0;
|
||||
}
|
||||
|
||||
@@ -293,10 +294,10 @@ void make_settings() {
|
||||
rpm_min = new FloatSetting(GRBL, WG, "31", "GCode/MinS", DEFAULT_SPINDLE_RPM_MIN, 0, 100000);
|
||||
rpm_max = new FloatSetting(GRBL, WG, "30", "GCode/MaxS", DEFAULT_SPINDLE_RPM_MAX, 0, 100000);
|
||||
|
||||
homing_pulloff = new FloatSetting(GRBL, WG, "27", "Homing/Pulloff", DEFAULT_HOMING_PULLOFF, 0, 1000);
|
||||
homing_debounce = new FloatSetting(GRBL, WG, "26", "Homing/Debounce", DEFAULT_HOMING_DEBOUNCE_DELAY, 0, 10000);
|
||||
homing_seek_rate = new FloatSetting(GRBL, WG, "25", "Homing/Seek", DEFAULT_HOMING_SEEK_RATE, 0, 10000);
|
||||
homing_feed_rate = new FloatSetting(GRBL, WG, "24", "Homing/Feed", DEFAULT_HOMING_FEED_RATE, 0, 10000);
|
||||
homing_pulloff = new FloatSetting(GRBL, WG, "27", "Homing/Pulloff", DEFAULT_HOMING_PULLOFF, 0, 1000);
|
||||
homing_debounce = new FloatSetting(GRBL, WG, "26", "Homing/Debounce", DEFAULT_HOMING_DEBOUNCE_DELAY, 0, 10000);
|
||||
homing_seek_rate = new FloatSetting(GRBL, WG, "25", "Homing/Seek", DEFAULT_HOMING_SEEK_RATE, 0, 10000);
|
||||
homing_feed_rate = new FloatSetting(GRBL, WG, "24", "Homing/Feed", DEFAULT_HOMING_FEED_RATE, 0, 10000);
|
||||
homing_squared_axes = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Squared", DEFAULT_HOMING_SQUARED_AXES);
|
||||
|
||||
// TODO Settings - need to call st_generate_step_invert_masks()
|
||||
|
@@ -31,9 +31,9 @@ uint8_t settings_read_coord_data(uint8_t coord_select, float* coord_data) {
|
||||
// Reset with default zero vector
|
||||
clear_vector_float(coord_data);
|
||||
settings_write_coord_data(coord_select, coord_data);
|
||||
return (false);
|
||||
return false;
|
||||
}
|
||||
return (true);
|
||||
return true;
|
||||
}
|
||||
|
||||
// Method to store coord data parameters into EEPROM
|
||||
@@ -58,9 +58,9 @@ uint8_t settings_read_build_info(char* line) {
|
||||
// Reset line with default value
|
||||
line[0] = 0; // Empty line
|
||||
settings_store_build_info(line);
|
||||
return (false);
|
||||
return false;
|
||||
}
|
||||
return (true);
|
||||
return true;
|
||||
}
|
||||
|
||||
// Returns step pin mask according to Grbl internal axis indexing.
|
||||
|
@@ -81,32 +81,37 @@ namespace Spindles {
|
||||
uint32_t _10v::set_rpm(uint32_t rpm) {
|
||||
uint32_t pwm_value;
|
||||
|
||||
if (_output_pin == UNDEFINED_PIN)
|
||||
if (_output_pin == UNDEFINED_PIN) {
|
||||
return rpm;
|
||||
}
|
||||
|
||||
// apply speed overrides
|
||||
rpm = rpm * sys.spindle_speed_ovr / 100; // Scale by spindle speed override value (percent)
|
||||
|
||||
// apply limits limits
|
||||
if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm))
|
||||
if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) {
|
||||
rpm = _max_rpm;
|
||||
else if (rpm != 0 && rpm <= _min_rpm)
|
||||
} else if (rpm != 0 && rpm <= _min_rpm) {
|
||||
rpm = _min_rpm;
|
||||
}
|
||||
sys.spindle_speed = rpm;
|
||||
|
||||
// determine the pwm value
|
||||
if (rpm == 0)
|
||||
if (rpm == 0) {
|
||||
pwm_value = _pwm_off_value;
|
||||
else
|
||||
} else {
|
||||
pwm_value = map_uint32_t(rpm, _min_rpm, _max_rpm, _pwm_min_value, _pwm_max_value);
|
||||
}
|
||||
|
||||
set_output(pwm_value);
|
||||
return rpm;
|
||||
}
|
||||
|
||||
/*
|
||||
void _10v::set_state(SpindleState state, uint32_t rpm) {
|
||||
if (sys.abort)
|
||||
return; // Block during abort.
|
||||
if (sys.abort) {
|
||||
return; // Block during abort.
|
||||
}
|
||||
|
||||
if (state == SpindleState::Disable) { // Halt or set spindle direction and rpm.
|
||||
sys.spindle_speed = 0;
|
||||
@@ -120,15 +125,16 @@ namespace Spindles {
|
||||
|
||||
sys.report_ovr_counter = 0; // Set to report change immediately
|
||||
}
|
||||
|
||||
*/
|
||||
|
||||
SpindleState _10v::get_state() {
|
||||
if (_current_pwm_duty == 0 || _output_pin == UNDEFINED_PIN)
|
||||
return (SpindleState::Disable);
|
||||
if (_direction_pin != UNDEFINED_PIN)
|
||||
if (_current_pwm_duty == 0 || _output_pin == UNDEFINED_PIN) {
|
||||
return SpindleState::Disable;
|
||||
}
|
||||
if (_direction_pin != UNDEFINED_PIN) {
|
||||
return digitalRead(_direction_pin) ? SpindleState::Cw : SpindleState::Ccw;
|
||||
return (SpindleState::Cw);
|
||||
}
|
||||
return SpindleState::Cw;
|
||||
}
|
||||
|
||||
void _10v::stop() {
|
||||
@@ -139,11 +145,13 @@ namespace Spindles {
|
||||
|
||||
void _10v::set_enable_pin(bool enable) {
|
||||
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Spindle::_10v::set_enable_pin");
|
||||
if (_off_with_zero_speed && sys.spindle_speed == 0)
|
||||
if (_off_with_zero_speed && sys.spindle_speed == 0) {
|
||||
enable = false;
|
||||
}
|
||||
|
||||
if (spindle_enable_invert->get())
|
||||
if (spindle_enable_invert->get()) {
|
||||
enable = !enable;
|
||||
}
|
||||
|
||||
digitalWrite(_enable_pin, enable);
|
||||
|
||||
|
@@ -96,17 +96,19 @@ namespace Spindles {
|
||||
uint32_t BESC::set_rpm(uint32_t rpm) {
|
||||
uint32_t pwm_value;
|
||||
|
||||
if (_output_pin == UNDEFINED_PIN)
|
||||
if (_output_pin == UNDEFINED_PIN) {
|
||||
return rpm;
|
||||
}
|
||||
|
||||
// apply speed overrides
|
||||
rpm = rpm * sys.spindle_speed_ovr / 100; // Scale by spindle speed override value (percent)
|
||||
|
||||
// apply limits limits
|
||||
if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm))
|
||||
if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) {
|
||||
rpm = _max_rpm;
|
||||
else if (rpm != 0 && rpm <= _min_rpm)
|
||||
} else if (rpm != 0 && rpm <= _min_rpm) {
|
||||
rpm = _min_rpm;
|
||||
}
|
||||
sys.spindle_speed = rpm;
|
||||
|
||||
// determine the pwm value
|
||||
|
@@ -28,8 +28,9 @@ namespace Spindles {
|
||||
void Dac::init() {
|
||||
get_pins_and_settings();
|
||||
|
||||
if (_output_pin == UNDEFINED_PIN)
|
||||
if (_output_pin == UNDEFINED_PIN) {
|
||||
return;
|
||||
}
|
||||
|
||||
_min_rpm = rpm_min->get();
|
||||
_max_rpm = rpm_max->get();
|
||||
@@ -62,8 +63,9 @@ namespace Spindles {
|
||||
}
|
||||
|
||||
uint32_t Dac::set_rpm(uint32_t rpm) {
|
||||
if (_output_pin == UNDEFINED_PIN)
|
||||
if (_output_pin == UNDEFINED_PIN) {
|
||||
return rpm;
|
||||
}
|
||||
|
||||
uint32_t pwm_value;
|
||||
|
||||
|
@@ -86,8 +86,9 @@ namespace Spindles {
|
||||
_pwm_precision = calc_pwm_precision(_pwm_freq); // detewrmine the best precision
|
||||
_pwm_period = (1 << _pwm_precision);
|
||||
|
||||
if (spindle_pwm_min_value->get() > spindle_pwm_min_value->get())
|
||||
if (spindle_pwm_min_value->get() > spindle_pwm_min_value->get()) {
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Warning: Spindle min pwm is greater than max. Check $35 and $36");
|
||||
}
|
||||
|
||||
// pre-caculate some PWM count values
|
||||
_pwm_off_value = (_pwm_period * spindle_pwm_off_value->get() / 100.0);
|
||||
@@ -112,8 +113,9 @@ namespace Spindles {
|
||||
uint32_t PWM::set_rpm(uint32_t rpm) {
|
||||
uint32_t pwm_value;
|
||||
|
||||
if (_output_pin == UNDEFINED_PIN)
|
||||
if (_output_pin == UNDEFINED_PIN) {
|
||||
return rpm;
|
||||
}
|
||||
|
||||
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "set_rpm(%d)", rpm);
|
||||
|
||||
@@ -121,10 +123,11 @@ namespace Spindles {
|
||||
rpm = rpm * sys.spindle_speed_ovr / 100; // Scale by spindle speed override value (uint8_t percent)
|
||||
|
||||
// apply limits
|
||||
if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm))
|
||||
if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) {
|
||||
rpm = _max_rpm;
|
||||
else if (rpm != 0 && rpm <= _min_rpm)
|
||||
} else if (rpm != 0 && rpm <= _min_rpm) {
|
||||
rpm = _min_rpm;
|
||||
}
|
||||
|
||||
sys.spindle_speed = rpm;
|
||||
|
||||
@@ -134,10 +137,11 @@ namespace Spindles {
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Warning: Linear fit not implemented yet.");
|
||||
|
||||
} else {
|
||||
if (rpm == 0)
|
||||
if (rpm == 0) {
|
||||
pwm_value = _pwm_off_value;
|
||||
else
|
||||
} else {
|
||||
pwm_value = map_uint32_t(rpm, _min_rpm, _max_rpm, _pwm_min_value, _pwm_max_value);
|
||||
}
|
||||
}
|
||||
|
||||
set_enable_pin(_current_state != SpindleState::Disable);
|
||||
@@ -147,8 +151,9 @@ namespace Spindles {
|
||||
}
|
||||
|
||||
void PWM::set_state(SpindleState state, uint32_t rpm) {
|
||||
if (sys.abort)
|
||||
if (sys.abort) {
|
||||
return; // Block during abort.
|
||||
}
|
||||
|
||||
_current_state = state;
|
||||
|
||||
@@ -177,11 +182,13 @@ namespace Spindles {
|
||||
}
|
||||
|
||||
SpindleState PWM::get_state() {
|
||||
if (_current_pwm_duty == 0 || _output_pin == UNDEFINED_PIN)
|
||||
return (SpindleState::Disable);
|
||||
if (_direction_pin != UNDEFINED_PIN)
|
||||
if (_current_pwm_duty == 0 || _output_pin == UNDEFINED_PIN) {
|
||||
return SpindleState::Disable;
|
||||
}
|
||||
if (_direction_pin != UNDEFINED_PIN) {
|
||||
return digitalRead(_direction_pin) ? SpindleState::Cw : SpindleState::Ccw;
|
||||
return (SpindleState::Cw);
|
||||
}
|
||||
return SpindleState::Cw;
|
||||
}
|
||||
|
||||
void PWM::stop() {
|
||||
@@ -203,17 +210,20 @@ namespace Spindles {
|
||||
}
|
||||
|
||||
void PWM::set_output(uint32_t duty) {
|
||||
if (_output_pin == UNDEFINED_PIN)
|
||||
if (_output_pin == UNDEFINED_PIN) {
|
||||
return;
|
||||
}
|
||||
|
||||
// to prevent excessive calls to ledcWrite, make sure duty hass changed
|
||||
if (duty == _current_pwm_duty)
|
||||
if (duty == _current_pwm_duty) {
|
||||
return;
|
||||
}
|
||||
|
||||
_current_pwm_duty = duty;
|
||||
|
||||
if (_invert_pwm)
|
||||
if (_invert_pwm) {
|
||||
duty = (1 << _pwm_precision) - duty;
|
||||
}
|
||||
|
||||
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "set_output(%d)", duty);
|
||||
|
||||
@@ -221,14 +231,17 @@ namespace Spindles {
|
||||
}
|
||||
|
||||
void PWM::set_enable_pin(bool enable) {
|
||||
if (_enable_pin == UNDEFINED_PIN)
|
||||
if (_enable_pin == UNDEFINED_PIN) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (_off_with_zero_speed && sys.spindle_speed == 0)
|
||||
if (_off_with_zero_speed && sys.spindle_speed == 0) {
|
||||
enable = false;
|
||||
}
|
||||
|
||||
if (spindle_enable_invert->get())
|
||||
if (spindle_enable_invert->get()) {
|
||||
enable = !enable;
|
||||
}
|
||||
|
||||
digitalWrite(_enable_pin, enable);
|
||||
}
|
||||
@@ -245,8 +258,9 @@ namespace Spindles {
|
||||
uint8_t precision = 0;
|
||||
|
||||
// increase the precision (bits) until it exceeds allow by frequency the max or is 16
|
||||
while ((1 << precision) < (uint32_t)(80000000 / freq) && precision <= 16)
|
||||
while ((1 << precision) < (uint32_t)(80000000 / freq) && precision <= 16) {
|
||||
precision++;
|
||||
}
|
||||
|
||||
return precision - 1;
|
||||
}
|
||||
|
@@ -30,8 +30,9 @@ namespace Spindles {
|
||||
void Relay::init() {
|
||||
get_pins_and_settings();
|
||||
|
||||
if (_output_pin == UNDEFINED_PIN)
|
||||
if (_output_pin == UNDEFINED_PIN) {
|
||||
return;
|
||||
}
|
||||
|
||||
pinMode(_output_pin, OUTPUT);
|
||||
pinMode(_enable_pin, OUTPUT);
|
||||
@@ -54,8 +55,9 @@ namespace Spindles {
|
||||
}
|
||||
|
||||
uint32_t Relay::set_rpm(uint32_t rpm) {
|
||||
if (_output_pin == UNDEFINED_PIN)
|
||||
if (_output_pin == UNDEFINED_PIN) {
|
||||
return rpm;
|
||||
}
|
||||
|
||||
sys.spindle_speed = rpm;
|
||||
set_output(rpm != 0);
|
||||
|
@@ -301,8 +301,9 @@ static void stepper_pulse_func() {
|
||||
st_go_idle();
|
||||
if (!(sys.state & STATE_JOG)) { // added to prevent ... jog after probing crash
|
||||
// Ensure pwm is set properly upon completion of rate-controlled motion.
|
||||
if (st.exec_block != NULL && st.exec_block->is_pwm_rate_adjusted)
|
||||
if (st.exec_block != NULL && st.exec_block->is_pwm_rate_adjusted) {
|
||||
spindle->set_rpm(0);
|
||||
}
|
||||
}
|
||||
|
||||
system_set_exec_state_flag(EXEC_CYCLE_STOP); // Flag main program for cycle end
|
||||
@@ -310,8 +311,9 @@ static void stepper_pulse_func() {
|
||||
}
|
||||
}
|
||||
// Check probing state.
|
||||
if (sys_probe_state == PROBE_ACTIVE)
|
||||
if (sys_probe_state == PROBE_ACTIVE) {
|
||||
probe_state_monitor();
|
||||
}
|
||||
// Reset step out bits.
|
||||
st.step_outbits = 0;
|
||||
// Execute step displacement profile by Bresenham line algorithm
|
||||
@@ -323,10 +325,11 @@ static void stepper_pulse_func() {
|
||||
if (st.counter_x > st.exec_block->step_event_count) {
|
||||
st.step_outbits |= bit(X_AXIS);
|
||||
st.counter_x -= st.exec_block->step_event_count;
|
||||
if (st.exec_block->direction_bits & bit(X_AXIS))
|
||||
if (st.exec_block->direction_bits & bit(X_AXIS)) {
|
||||
sys_position[X_AXIS]--;
|
||||
else
|
||||
} else {
|
||||
sys_position[X_AXIS]++;
|
||||
}
|
||||
}
|
||||
#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
|
||||
st.counter_y += st.steps[Y_AXIS];
|
||||
@@ -336,10 +339,11 @@ static void stepper_pulse_func() {
|
||||
if (st.counter_y > st.exec_block->step_event_count) {
|
||||
st.step_outbits |= bit(Y_AXIS);
|
||||
st.counter_y -= st.exec_block->step_event_count;
|
||||
if (st.exec_block->direction_bits & bit(Y_AXIS))
|
||||
if (st.exec_block->direction_bits & bit(Y_AXIS)) {
|
||||
sys_position[Y_AXIS]--;
|
||||
else
|
||||
} else {
|
||||
sys_position[Y_AXIS]++;
|
||||
}
|
||||
}
|
||||
#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
|
||||
st.counter_z += st.steps[Z_AXIS];
|
||||
@@ -349,10 +353,11 @@ static void stepper_pulse_func() {
|
||||
if (st.counter_z > st.exec_block->step_event_count) {
|
||||
st.step_outbits |= bit(Z_AXIS);
|
||||
st.counter_z -= st.exec_block->step_event_count;
|
||||
if (st.exec_block->direction_bits & bit(Z_AXIS))
|
||||
if (st.exec_block->direction_bits & bit(Z_AXIS)) {
|
||||
sys_position[Z_AXIS]--;
|
||||
else
|
||||
} else {
|
||||
sys_position[Z_AXIS]++;
|
||||
}
|
||||
}
|
||||
#if (N_AXIS > A_AXIS)
|
||||
# ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
|
||||
@@ -363,10 +368,11 @@ static void stepper_pulse_func() {
|
||||
if (st.counter_a > st.exec_block->step_event_count) {
|
||||
st.step_outbits |= bit(A_AXIS);
|
||||
st.counter_a -= st.exec_block->step_event_count;
|
||||
if (st.exec_block->direction_bits & bit(A_AXIS))
|
||||
if (st.exec_block->direction_bits & bit(A_AXIS)) {
|
||||
sys_position[A_AXIS]--;
|
||||
else
|
||||
} else {
|
||||
sys_position[A_AXIS]++;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#if (N_AXIS > B_AXIS)
|
||||
@@ -378,10 +384,11 @@ static void stepper_pulse_func() {
|
||||
if (st.counter_b > st.exec_block->step_event_count) {
|
||||
st.step_outbits |= bit(B_AXIS);
|
||||
st.counter_b -= st.exec_block->step_event_count;
|
||||
if (st.exec_block->direction_bits & bit(B_AXIS))
|
||||
if (st.exec_block->direction_bits & bit(B_AXIS)) {
|
||||
sys_position[B_AXIS]--;
|
||||
else
|
||||
} else {
|
||||
sys_position[B_AXIS]++;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#if (N_AXIS > C_AXIS)
|
||||
@@ -393,21 +400,24 @@ static void stepper_pulse_func() {
|
||||
if (st.counter_c > st.exec_block->step_event_count) {
|
||||
st.step_outbits |= bit(C_AXIS);
|
||||
st.counter_c -= st.exec_block->step_event_count;
|
||||
if (st.exec_block->direction_bits & bit(C_AXIS))
|
||||
if (st.exec_block->direction_bits & bit(C_AXIS)) {
|
||||
sys_position[C_AXIS]--;
|
||||
else
|
||||
} else {
|
||||
sys_position[C_AXIS]++;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
// During a homing cycle, lock out and prevent desired axes from moving.
|
||||
if (sys.state == STATE_HOMING)
|
||||
if (sys.state == STATE_HOMING) {
|
||||
st.step_outbits &= sys.homing_axis_lock;
|
||||
}
|
||||
st.step_count--; // Decrement step events count
|
||||
if (st.step_count == 0) {
|
||||
// Segment is complete. Discard current segment and advance segment indexing.
|
||||
st.exec_segment = NULL;
|
||||
if (++segment_buffer_tail == SEGMENT_BUFFER_SIZE)
|
||||
if (++segment_buffer_tail == SEGMENT_BUFFER_SIZE) {
|
||||
segment_buffer_tail = 0;
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef USE_I2S_STEPS
|
||||
@@ -513,20 +523,24 @@ void set_stepper_pins_on(uint8_t onMask) {
|
||||
# ifndef X2_STEP_PIN // if not a ganged axis
|
||||
digitalWrite(X_STEP_PIN, (onMask & bit(X_AXIS)));
|
||||
# else // is a ganged axis
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A))
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
|
||||
digitalWrite(X_STEP_PIN, (onMask & bit(X_AXIS)));
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B))
|
||||
}
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
|
||||
digitalWrite(X2_STEP_PIN, (onMask & bit(X_AXIS)));
|
||||
}
|
||||
# endif
|
||||
#endif
|
||||
#ifdef Y_STEP_PIN
|
||||
# ifndef Y2_STEP_PIN // if not a ganged axis
|
||||
digitalWrite(Y_STEP_PIN, (onMask & bit(Y_AXIS)));
|
||||
# else // is a ganged axis
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A))
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
|
||||
digitalWrite(Y_STEP_PIN, (onMask & bit(Y_AXIS)));
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B))
|
||||
}
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
|
||||
digitalWrite(Y2_STEP_PIN, (onMask & bit(Y_AXIS)));
|
||||
}
|
||||
# endif
|
||||
#endif
|
||||
|
||||
@@ -534,10 +548,12 @@ void set_stepper_pins_on(uint8_t onMask) {
|
||||
# ifndef Z2_STEP_PIN // if not a ganged axis
|
||||
digitalWrite(Z_STEP_PIN, (onMask & bit(Z_AXIS)));
|
||||
# else // is a ganged axis
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A))
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
|
||||
digitalWrite(Z_STEP_PIN, (onMask & bit(Z_AXIS)));
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B))
|
||||
}
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
|
||||
digitalWrite(Z2_STEP_PIN, (onMask & bit(Z_AXIS)));
|
||||
}
|
||||
# endif
|
||||
#endif
|
||||
|
||||
@@ -545,10 +561,12 @@ void set_stepper_pins_on(uint8_t onMask) {
|
||||
# ifndef A2_STEP_PIN // if not a ganged axis
|
||||
digitalWrite(A_STEP_PIN, (onMask & bit(A_AXIS)));
|
||||
# else // is a ganged axis
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A))
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
|
||||
digitalWrite(A_STEP_PIN, (onMask & bit(A_AXIS)));
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B))
|
||||
}
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
|
||||
digitalWrite(A2_STEP_PIN, (onMask & bit(A_AXIS)));
|
||||
}
|
||||
# endif
|
||||
#endif
|
||||
|
||||
@@ -556,10 +574,12 @@ void set_stepper_pins_on(uint8_t onMask) {
|
||||
# ifndef B2_STEP_PIN // if not a ganged axis
|
||||
digitalWrite(B_STEP_PIN, (onMask & bit(B_AXIS)));
|
||||
# else // is a ganged axis
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A))
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
|
||||
digitalWrite(B_STEP_PIN, (onMask & bit(B_AXIS)));
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B))
|
||||
}
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
|
||||
digitalWrite(B2_STEP_PIN, (onMask & bit(B_AXIS)));
|
||||
}
|
||||
# endif
|
||||
#endif
|
||||
|
||||
@@ -567,10 +587,12 @@ void set_stepper_pins_on(uint8_t onMask) {
|
||||
# ifndef C2_STEP_PIN // if not a ganged axis
|
||||
digitalWrite(C_STEP_PIN, (onMask & bit(C_AXIS)));
|
||||
# else // is a ganged axis
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A))
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
|
||||
digitalWrite(C_STEP_PIN, (onMask & bit(C_AXIS)));
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B))
|
||||
}
|
||||
if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
|
||||
digitalWrite(C2_STEP_PIN, (onMask & bit(C_AXIS)));
|
||||
}
|
||||
# endif
|
||||
#endif
|
||||
}
|
||||
@@ -692,6 +714,7 @@ void st_go_idle() {
|
||||
// Disable Stepper Driver Interrupt. Allow Stepper Port Reset Interrupt to finish, if active.
|
||||
Stepper_Timer_Stop();
|
||||
busy = false;
|
||||
|
||||
// Set stepper driver idle state, disabled or enabled, depending on settings and circumstances.
|
||||
if (((stepper_idle_lock_time->get() != 0xff) || sys_rt_exec_alarm || sys.state == STATE_SLEEP) && sys.state != STATE_HOMING) {
|
||||
// Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete
|
||||
@@ -704,8 +727,9 @@ void st_go_idle() {
|
||||
stepper_idle_counter = esp_timer_get_time() + (stepper_idle_lock_time->get() * 1000); // * 1000 because the time is in uSecs
|
||||
// after idle countdown will be disabled in protocol loop
|
||||
}
|
||||
} else
|
||||
} else {
|
||||
motors_set_disable(false);
|
||||
}
|
||||
|
||||
set_stepper_pins_on(0);
|
||||
st.step_outbits = 0;
|
||||
@@ -747,8 +771,10 @@ void st_parking_restore_buffer() {
|
||||
prep.step_per_mm = prep.last_step_per_mm;
|
||||
prep.recalculate_flag = (PREP_FLAG_HOLD_PARTIAL_BLOCK | PREP_FLAG_RECALCULATE);
|
||||
prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR / prep.step_per_mm; // Recompute this value.
|
||||
} else
|
||||
} else {
|
||||
prep.recalculate_flag = false;
|
||||
}
|
||||
|
||||
pl_block = NULL; // Set to reload next block.
|
||||
}
|
||||
#endif
|
||||
@@ -756,9 +782,11 @@ void st_parking_restore_buffer() {
|
||||
// Increments the step segment buffer block data ring buffer.
|
||||
static uint8_t st_next_block_index(uint8_t block_index) {
|
||||
block_index++;
|
||||
if (block_index == (SEGMENT_BUFFER_SIZE - 1))
|
||||
return (0);
|
||||
return (block_index);
|
||||
if (block_index == (SEGMENT_BUFFER_SIZE - 1)) {
|
||||
return 0;
|
||||
} else {
|
||||
return block_index;
|
||||
}
|
||||
}
|
||||
|
||||
/* Prepares step segment buffer. Continuously called from main program.
|
||||
@@ -776,26 +804,32 @@ static uint8_t st_next_block_index(uint8_t block_index) {
|
||||
*/
|
||||
void st_prep_buffer() {
|
||||
// Block step prep buffer, while in a suspend state and there is no suspend motion to execute.
|
||||
if (bit_istrue(sys.step_control, STEP_CONTROL_END_MOTION))
|
||||
if (bit_istrue(sys.step_control, STEP_CONTROL_END_MOTION)) {
|
||||
return;
|
||||
}
|
||||
|
||||
while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer.
|
||||
// Determine if we need to load a new planner block or if the block needs to be recomputed.
|
||||
if (pl_block == NULL) {
|
||||
// Query planner for a queued block
|
||||
if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION)
|
||||
if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) {
|
||||
pl_block = plan_get_system_motion_block();
|
||||
else
|
||||
} else {
|
||||
pl_block = plan_get_current_block();
|
||||
}
|
||||
|
||||
if (pl_block == NULL) {
|
||||
return; // No planner blocks. Exit.
|
||||
}
|
||||
|
||||
// Check if we need to only recompute the velocity profile or load a new block.
|
||||
if (prep.recalculate_flag & PREP_FLAG_RECALCULATE) {
|
||||
#ifdef PARKING_ENABLE
|
||||
if (prep.recalculate_flag & PREP_FLAG_PARKING)
|
||||
if (prep.recalculate_flag & PREP_FLAG_PARKING) {
|
||||
prep.recalculate_flag &= ~(PREP_FLAG_RECALCULATE);
|
||||
else
|
||||
} else {
|
||||
prep.recalculate_flag = false;
|
||||
}
|
||||
#else
|
||||
prep.recalculate_flag = false;
|
||||
#endif
|
||||
@@ -809,15 +843,17 @@ void st_prep_buffer() {
|
||||
st_prep_block->direction_bits = pl_block->direction_bits;
|
||||
uint8_t idx;
|
||||
#ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
|
||||
for (idx = 0; idx < N_AXIS; idx++)
|
||||
for (idx = 0; idx < N_AXIS; idx++) {
|
||||
st_prep_block->steps[idx] = pl_block->steps[idx];
|
||||
}
|
||||
st_prep_block->step_event_count = pl_block->step_event_count;
|
||||
#else
|
||||
// With AMASS enabled, simply bit-shift multiply all Bresenham data by the max AMASS
|
||||
// level, such that we never divide beyond the original data anywhere in the algorithm.
|
||||
// If the original data is divided, we can lose a step from integer roundoff.
|
||||
for (idx = 0; idx < N_AXIS; idx++)
|
||||
for (idx = 0; idx < N_AXIS; idx++) {
|
||||
st_prep_block->steps[idx] = pl_block->steps[idx] << MAX_AMASS_LEVEL;
|
||||
}
|
||||
st_prep_block->step_event_count = pl_block->step_event_count << MAX_AMASS_LEVEL;
|
||||
#endif
|
||||
// Initialize segment buffer data for generating the segments.
|
||||
@@ -830,8 +866,9 @@ void st_prep_buffer() {
|
||||
prep.current_speed = prep.exit_speed;
|
||||
pl_block->entry_speed_sqr = prep.exit_speed * prep.exit_speed;
|
||||
prep.recalculate_flag &= ~(PREP_FLAG_DECEL_OVERRIDE);
|
||||
} else
|
||||
} else {
|
||||
prep.current_speed = sqrt(pl_block->entry_speed_sqr);
|
||||
}
|
||||
|
||||
if (spindle->isRateAdjusted()) { // laser_mode->get() {
|
||||
if (pl_block->spindle == SpindleState::Ccw) {
|
||||
@@ -874,6 +911,7 @@ void st_prep_buffer() {
|
||||
exit_speed_sqr = plan_get_exec_block_exit_speed_sqr();
|
||||
prep.exit_speed = sqrt(exit_speed_sqr);
|
||||
}
|
||||
|
||||
nominal_speed = plan_compute_profile_nominal_speed(pl_block);
|
||||
float nominal_speed_sqr = nominal_speed * nominal_speed;
|
||||
float intersect_distance = 0.5 * (pl_block->millimeters + inv_2_accel * (pl_block->entry_speed_sqr - exit_speed_sqr));
|
||||
@@ -927,10 +965,13 @@ void st_prep_buffer() {
|
||||
|
||||
bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM); // Force update whenever updating block.
|
||||
}
|
||||
|
||||
// Initialize new segment
|
||||
segment_t* prep_segment = &segment_buffer[segment_buffer_head];
|
||||
|
||||
// Set new segment to point to the current segment data block.
|
||||
prep_segment->st_block_index = prep.st_block_index;
|
||||
|
||||
/*------------------------------------------------------------------------------------
|
||||
Compute the average velocity of this new segment by determining the total distance
|
||||
traveled over the segment time DT_SEGMENT. The following code first attempts to create
|
||||
@@ -952,8 +993,11 @@ void st_prep_buffer() {
|
||||
float speed_var; // Speed worker variable
|
||||
float mm_remaining = pl_block->millimeters; // New segment distance from end of block.
|
||||
float minimum_mm = mm_remaining - prep.req_mm_increment; // Guarantee at least one step.
|
||||
if (minimum_mm < 0.0)
|
||||
|
||||
if (minimum_mm < 0.0) {
|
||||
minimum_mm = 0.0;
|
||||
}
|
||||
|
||||
do {
|
||||
switch (prep.ramp_type) {
|
||||
case RAMP_DECEL_OVERRIDE:
|
||||
@@ -966,8 +1010,9 @@ void st_prep_buffer() {
|
||||
time_var = 2.0 * (pl_block->millimeters - mm_remaining) / (prep.current_speed + prep.maximum_speed);
|
||||
prep.ramp_type = RAMP_CRUISE;
|
||||
prep.current_speed = prep.maximum_speed;
|
||||
} else // Mid-deceleration override ramp.
|
||||
} else { // Mid-deceleration override ramp.
|
||||
prep.current_speed -= speed_var;
|
||||
}
|
||||
break;
|
||||
case RAMP_ACCEL:
|
||||
// NOTE: Acceleration ramp only computes during first do-while loop.
|
||||
@@ -977,13 +1022,15 @@ void st_prep_buffer() {
|
||||
// Acceleration-cruise, acceleration-deceleration ramp junction, or end of block.
|
||||
mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB
|
||||
time_var = 2.0 * (pl_block->millimeters - mm_remaining) / (prep.current_speed + prep.maximum_speed);
|
||||
if (mm_remaining == prep.decelerate_after)
|
||||
if (mm_remaining == prep.decelerate_after) {
|
||||
prep.ramp_type = RAMP_DECEL;
|
||||
else
|
||||
} else {
|
||||
prep.ramp_type = RAMP_CRUISE;
|
||||
}
|
||||
prep.current_speed = prep.maximum_speed;
|
||||
} else // Acceleration only.
|
||||
} else { // Acceleration only.
|
||||
prep.current_speed += speed_var;
|
||||
}
|
||||
break;
|
||||
case RAMP_CRUISE:
|
||||
// NOTE: mm_var used to retain the last mm_remaining for incomplete segment time_var calculations.
|
||||
@@ -995,8 +1042,9 @@ void st_prep_buffer() {
|
||||
time_var = (mm_remaining - prep.decelerate_after) / prep.maximum_speed;
|
||||
mm_remaining = prep.decelerate_after; // NOTE: 0.0 at EOB
|
||||
prep.ramp_type = RAMP_DECEL;
|
||||
} else // Cruising only.
|
||||
} else { // Cruising only.
|
||||
mm_remaining = mm_var;
|
||||
}
|
||||
break;
|
||||
default: // case RAMP_DECEL:
|
||||
// NOTE: mm_var used as a misc worker variable to prevent errors when near zero speed.
|
||||
@@ -1015,6 +1063,7 @@ void st_prep_buffer() {
|
||||
mm_remaining = prep.mm_complete;
|
||||
prep.current_speed = prep.exit_speed;
|
||||
}
|
||||
|
||||
dt += time_var; // Add computed ramp time to total segment time.
|
||||
if (dt < dt_max) {
|
||||
time_var = dt_max - dt; // **Incomplete** At ramp junction.
|
||||
@@ -1068,6 +1117,7 @@ void st_prep_buffer() {
|
||||
float n_steps_remaining = ceil(step_dist_remaining); // Round-up current steps remaining
|
||||
float last_n_steps_remaining = ceil(prep.steps_remaining); // Round-up last steps remaining
|
||||
prep_segment->n_step = last_n_steps_remaining - n_steps_remaining; // Compute number of steps to execute.
|
||||
|
||||
// Bail if we are at the end of a feed hold and don't have a step to execute.
|
||||
if (prep_segment->n_step == 0) {
|
||||
if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) {
|
||||
@@ -1075,12 +1125,14 @@ void st_prep_buffer() {
|
||||
// requires full steps to execute. So, just bail.
|
||||
bit_true(sys.step_control, STEP_CONTROL_END_MOTION);
|
||||
#ifdef PARKING_ENABLE
|
||||
if (!(prep.recalculate_flag & PREP_FLAG_PARKING))
|
||||
if (!(prep.recalculate_flag & PREP_FLAG_PARKING)) {
|
||||
prep.recalculate_flag |= PREP_FLAG_HOLD_PARTIAL_BLOCK;
|
||||
}
|
||||
#endif
|
||||
return; // Segment not generated, but current step data still retained.
|
||||
}
|
||||
}
|
||||
|
||||
// Compute segment step rate. Since steps are integers and mm distances traveled are not,
|
||||
// the end of every segment can have a partial step of varying magnitudes that are not
|
||||
// executed, because the stepper ISR requires whole steps due to the AMASS algorithm. To
|
||||
@@ -1089,22 +1141,26 @@ void st_prep_buffer() {
|
||||
// adjusts the whole segment rate to keep step output exact. These rate adjustments are
|
||||
// typically very small and do not adversely effect performance, but ensures that Grbl
|
||||
// outputs the exact acceleration and velocity profiles as computed by the planner.
|
||||
|
||||
dt += prep.dt_remainder; // Apply previous segment partial step execute time
|
||||
float inv_rate = dt / (last_n_steps_remaining - step_dist_remaining); // Compute adjusted step rate inverse
|
||||
|
||||
// Compute CPU cycles per step for the prepped segment.
|
||||
uint32_t cycles = ceil((TICKS_PER_MICROSECOND * 1000000 * 60) * inv_rate); // (cycles/step)
|
||||
|
||||
#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
|
||||
// Compute step timing and multi-axis smoothing level.
|
||||
// NOTE: AMASS overdrives the timer with each level, so only one prescalar is required.
|
||||
if (cycles < AMASS_LEVEL1)
|
||||
if (cycles < AMASS_LEVEL1) {
|
||||
prep_segment->amass_level = 0;
|
||||
else {
|
||||
if (cycles < AMASS_LEVEL2)
|
||||
} else {
|
||||
if (cycles < AMASS_LEVEL2) {
|
||||
prep_segment->amass_level = 1;
|
||||
else if (cycles < AMASS_LEVEL3)
|
||||
} else if (cycles < AMASS_LEVEL3) {
|
||||
prep_segment->amass_level = 2;
|
||||
else
|
||||
} else {
|
||||
prep_segment->amass_level = 3;
|
||||
}
|
||||
cycles >>= prep_segment->amass_level;
|
||||
prep_segment->n_step <<= prep_segment->amass_level;
|
||||
}
|
||||
@@ -1123,16 +1179,18 @@ void st_prep_buffer() {
|
||||
prep_segment->cycles_per_tick = cycles >> 3;
|
||||
} else {
|
||||
prep_segment->prescaler = 3; // prescaler: 64
|
||||
if (cycles < (1UL << 22)) // < 4194304 (262ms@16MHz)
|
||||
if (cycles < (1UL << 22)) { // < 4194304 (262ms@16MHz)
|
||||
prep_segment->cycles_per_tick = cycles >> 6;
|
||||
else // Just set the slowest speed possible. (Around 4 step/sec.)
|
||||
} else { // Just set the slowest speed possible. (Around 4 step/sec.)
|
||||
prep_segment->cycles_per_tick = 0xffff;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
// Segment complete! Increment segment buffer indices, so stepper ISR can immediately execute it.
|
||||
segment_buffer_head = segment_next_head;
|
||||
if (++segment_next_head == SEGMENT_BUFFER_SIZE)
|
||||
if (++segment_next_head == SEGMENT_BUFFER_SIZE) {
|
||||
segment_next_head = 0;
|
||||
}
|
||||
// Update the appropriate planner and segment data.
|
||||
pl_block->millimeters = mm_remaining;
|
||||
prep.steps_remaining = n_steps_remaining;
|
||||
@@ -1146,8 +1204,9 @@ void st_prep_buffer() {
|
||||
// cycle stop flag from the ISR. Prep_segment is blocked until then.
|
||||
bit_true(sys.step_control, STEP_CONTROL_END_MOTION);
|
||||
#ifdef PARKING_ENABLE
|
||||
if (!(prep.recalculate_flag & PREP_FLAG_PARKING))
|
||||
if (!(prep.recalculate_flag & PREP_FLAG_PARKING)) {
|
||||
prep.recalculate_flag |= PREP_FLAG_HOLD_PARTIAL_BLOCK;
|
||||
}
|
||||
#endif
|
||||
return; // Bail!
|
||||
} else { // End of planner block
|
||||
@@ -1168,9 +1227,11 @@ void st_prep_buffer() {
|
||||
// in the segment buffer. It will always be behind by up to the number of segment blocks (-1)
|
||||
// divided by the ACCELERATION TICKS PER SECOND in seconds.
|
||||
float st_get_realtime_rate() {
|
||||
if (sys.state & (STATE_CYCLE | STATE_HOMING | STATE_HOLD | STATE_JOG | STATE_SAFETY_DOOR))
|
||||
if (sys.state & (STATE_CYCLE | STATE_HOMING | STATE_HOLD | STATE_JOG | STATE_SAFETY_DOOR)) {
|
||||
return prep.current_speed;
|
||||
return 0.0f;
|
||||
} else {
|
||||
return 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
void IRAM_ATTR Stepper_Timer_WritePeriod(uint64_t alarm_val) {
|
||||
|
@@ -118,8 +118,9 @@ void controlCheckTask(void* pvParameters) {
|
||||
xQueueReceive(control_sw_queue, &evt, portMAX_DELAY); // block until receive queue
|
||||
vTaskDelay(CONTROL_SW_DEBOUNCE_PERIOD); // delay a while
|
||||
uint8_t pin = system_control_get_state();
|
||||
if (pin)
|
||||
if (pin) {
|
||||
system_exec_control_pin(pin);
|
||||
}
|
||||
debouncing = false;
|
||||
}
|
||||
}
|
||||
@@ -142,9 +143,9 @@ void IRAM_ATTR isr_control_inputs() {
|
||||
// Returns if safety door is ajar(T) or closed(F), based on pin state.
|
||||
uint8_t system_check_safety_door_ajar() {
|
||||
#ifdef ENABLE_SAFETY_DOOR_INPUT_PIN
|
||||
return (system_control_get_state() & CONTROL_PIN_INDEX_SAFETY_DOOR);
|
||||
return system_control_get_state() & CONTROL_PIN_INDEX_SAFETY_DOOR;
|
||||
#else
|
||||
return (false); // Input pin not enabled, so just return that it's closed.
|
||||
return false; // Input pin not enabled, so just return that it's closed.
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -219,22 +220,24 @@ 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();
|
||||
#ifdef COREXY
|
||||
if (idx == X_AXIS)
|
||||
if (idx == X_AXIS) {
|
||||
pos = (float)system_convert_corexy_to_x_axis_steps(steps) / steps_per_mm;
|
||||
else if (idx == Y_AXIS)
|
||||
} else if (idx == Y_AXIS) {
|
||||
pos = (float)system_convert_corexy_to_y_axis_steps(steps) / steps_per_mm;
|
||||
else
|
||||
} else {
|
||||
pos = steps[idx] / steps_per_mm;
|
||||
}
|
||||
#else
|
||||
pos = steps[idx] / steps_per_mm;
|
||||
#endif
|
||||
return (pos);
|
||||
return pos;
|
||||
}
|
||||
|
||||
void system_convert_array_steps_to_mpos(float* position, int32_t* steps) {
|
||||
uint8_t idx;
|
||||
for (idx = 0; idx < N_AXIS; idx++)
|
||||
for (idx = 0; idx < N_AXIS; idx++) {
|
||||
position[idx] = system_convert_axis_steps_to_mpos(steps, idx);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -247,23 +250,27 @@ uint8_t system_check_travel_limits(float* target) {
|
||||
uint8_t mask = homing_dir_mask->get();
|
||||
// When homing forced set origin is enabled, soft limits checks need to account for directionality.
|
||||
if (bit_istrue(mask, bit(idx))) {
|
||||
if (target[idx] < 0 || target[idx] > travel)
|
||||
return (true);
|
||||
if (target[idx] < 0 || target[idx] > travel) {
|
||||
return true;
|
||||
}
|
||||
} else {
|
||||
if (target[idx] > 0 || target[idx] < -travel)
|
||||
return (true);
|
||||
if (target[idx] > 0 || target[idx] < -travel) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
#else
|
||||
# ifdef HOMING_FORCE_POSITIVE_SPACE
|
||||
if (target[idx] < 0 || target[idx] > travel)
|
||||
return (true);
|
||||
if (target[idx] < 0 || target[idx] > travel) {
|
||||
return true;
|
||||
}
|
||||
# else
|
||||
if (target[idx] > 0 || target[idx] < -travel)
|
||||
return (true);
|
||||
if (target[idx] > 0 || target[idx] < -travel) {
|
||||
return true;
|
||||
}
|
||||
# endif
|
||||
#endif
|
||||
}
|
||||
return (false);
|
||||
return false;
|
||||
}
|
||||
|
||||
// Returns control pin state as a uint8 bitfield. Each bit indicates the input pin state, where
|
||||
@@ -275,48 +282,57 @@ uint8_t system_control_get_state() {
|
||||
|
||||
#ifdef CONTROL_SAFETY_DOOR_PIN
|
||||
defined_pin_mask |= CONTROL_PIN_INDEX_SAFETY_DOOR;
|
||||
if (digitalRead(CONTROL_SAFETY_DOOR_PIN))
|
||||
if (digitalRead(CONTROL_SAFETY_DOOR_PIN)) {
|
||||
control_state |= CONTROL_PIN_INDEX_SAFETY_DOOR;
|
||||
}
|
||||
#endif
|
||||
#ifdef CONTROL_RESET_PIN
|
||||
defined_pin_mask |= CONTROL_PIN_INDEX_RESET;
|
||||
if (digitalRead(CONTROL_RESET_PIN))
|
||||
if (digitalRead(CONTROL_RESET_PIN)) {
|
||||
control_state |= CONTROL_PIN_INDEX_RESET;
|
||||
}
|
||||
#endif
|
||||
#ifdef CONTROL_FEED_HOLD_PIN
|
||||
defined_pin_mask |= CONTROL_PIN_INDEX_FEED_HOLD;
|
||||
if (digitalRead(CONTROL_FEED_HOLD_PIN))
|
||||
if (digitalRead(CONTROL_FEED_HOLD_PIN)) {
|
||||
control_state |= CONTROL_PIN_INDEX_FEED_HOLD;
|
||||
}
|
||||
#endif
|
||||
#ifdef CONTROL_CYCLE_START_PIN
|
||||
defined_pin_mask |= CONTROL_PIN_INDEX_CYCLE_START;
|
||||
if (digitalRead(CONTROL_CYCLE_START_PIN))
|
||||
if (digitalRead(CONTROL_CYCLE_START_PIN)) {
|
||||
control_state |= CONTROL_PIN_INDEX_CYCLE_START;
|
||||
}
|
||||
#endif
|
||||
#ifdef MACRO_BUTTON_0_PIN
|
||||
defined_pin_mask |= CONTROL_PIN_INDEX_MACRO_0;
|
||||
if (digitalRead(MACRO_BUTTON_0_PIN))
|
||||
if (digitalRead(MACRO_BUTTON_0_PIN)) {
|
||||
control_state |= CONTROL_PIN_INDEX_MACRO_0;
|
||||
}
|
||||
#endif
|
||||
#ifdef MACRO_BUTTON_1_PIN
|
||||
defined_pin_mask |= CONTROL_PIN_INDEX_MACRO_1;
|
||||
if (digitalRead(MACRO_BUTTON_1_PIN))
|
||||
if (digitalRead(MACRO_BUTTON_1_PIN)) {
|
||||
control_state |= CONTROL_PIN_INDEX_MACRO_1;
|
||||
}
|
||||
#endif
|
||||
#ifdef MACRO_BUTTON_2_PIN
|
||||
defined_pin_mask |= CONTROL_PIN_INDEX_MACRO_2;
|
||||
if (digitalRead(MACRO_BUTTON_2_PIN))
|
||||
if (digitalRead(MACRO_BUTTON_2_PIN)) {
|
||||
control_state |= CONTROL_PIN_INDEX_MACRO_2;
|
||||
}
|
||||
#endif
|
||||
#ifdef MACRO_BUTTON_3_PIN
|
||||
defined_pin_mask |= CONTROL_PIN_INDEX_MACRO_3;
|
||||
if (digitalRead(MACRO_BUTTON_3_PIN))
|
||||
if (digitalRead(MACRO_BUTTON_3_PIN)) {
|
||||
control_state |= CONTROL_PIN_INDEX_MACRO_3;
|
||||
}
|
||||
#endif
|
||||
#ifdef INVERT_CONTROL_PIN_MASK
|
||||
control_state ^= (INVERT_CONTROL_PIN_MASK & defined_pin_mask);
|
||||
#endif
|
||||
return (control_state);
|
||||
|
||||
return control_state;
|
||||
}
|
||||
|
||||
// execute the function of the control pin
|
||||
@@ -324,12 +340,13 @@ void system_exec_control_pin(uint8_t pin) {
|
||||
if (bit_istrue(pin, CONTROL_PIN_INDEX_RESET)) {
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Reset via control pin");
|
||||
mc_reset();
|
||||
} else if (bit_istrue(pin, CONTROL_PIN_INDEX_CYCLE_START))
|
||||
} else if (bit_istrue(pin, CONTROL_PIN_INDEX_CYCLE_START)) {
|
||||
bit_true(sys_rt_exec_state, EXEC_CYCLE_START);
|
||||
else if (bit_istrue(pin, CONTROL_PIN_INDEX_FEED_HOLD))
|
||||
} else if (bit_istrue(pin, CONTROL_PIN_INDEX_FEED_HOLD)) {
|
||||
bit_true(sys_rt_exec_state, EXEC_FEED_HOLD);
|
||||
else if (bit_istrue(pin, CONTROL_PIN_INDEX_SAFETY_DOOR))
|
||||
} else if (bit_istrue(pin, CONTROL_PIN_INDEX_SAFETY_DOOR)) {
|
||||
bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR);
|
||||
}
|
||||
#ifdef MACRO_BUTTON_0_PIN
|
||||
else if (bit_istrue(pin, CONTROL_PIN_INDEX_MACRO_0)) {
|
||||
user_defined_macro(CONTROL_PIN_INDEX_MACRO_0); // function must be implemented by user
|
||||
@@ -354,10 +371,11 @@ void system_exec_control_pin(uint8_t pin) {
|
||||
|
||||
// CoreXY calculation only. Returns x or y-axis "steps" based on CoreXY motor steps.
|
||||
int32_t system_convert_corexy_to_x_axis_steps(int32_t* steps) {
|
||||
return ((steps[A_MOTOR] + steps[B_MOTOR]) / 2);
|
||||
return (steps[A_MOTOR] + steps[B_MOTOR]) / 2;
|
||||
}
|
||||
|
||||
int32_t system_convert_corexy_to_y_axis_steps(int32_t* steps) {
|
||||
return ((steps[A_MOTOR] - steps[B_MOTOR]) / 2);
|
||||
return (steps[A_MOTOR] - steps[B_MOTOR]) / 2;
|
||||
}
|
||||
|
||||
// io_num is the virtual pin# and has nothing to do with the actual esp32 GPIO_NUM_xx
|
||||
@@ -401,9 +419,9 @@ void fast_sys_io_control(uint8_t io_num_mask, bool turnOn) {
|
||||
// returns -1 for error
|
||||
int8_t sys_get_next_RMT_chan_num() {
|
||||
static uint8_t next_RMT_chan_num = 0; // channels 0-7 are valid
|
||||
if (next_RMT_chan_num < 8) // 7 is the max PWM channel number
|
||||
if (next_RMT_chan_num < 8) { // 7 is the max PWM channel number
|
||||
return next_RMT_chan_num++;
|
||||
else {
|
||||
} else {
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_ERROR, "Error: out of RMT channels");
|
||||
return -1;
|
||||
}
|
||||
@@ -420,9 +438,9 @@ int8_t sys_get_next_RMT_chan_num() {
|
||||
*/
|
||||
int8_t sys_get_next_PWM_chan_num() {
|
||||
static uint8_t next_PWM_chan_num = 2; // start at 2 to avoid spindle
|
||||
if (next_PWM_chan_num < 8) // 7 is the max PWM channel number
|
||||
if (next_PWM_chan_num < 8) { // 7 is the max PWM channel number
|
||||
return next_PWM_chan_num++;
|
||||
else {
|
||||
} else {
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_ERROR, "Error: out of PWM channels");
|
||||
return -1;
|
||||
}
|
||||
|
@@ -68,12 +68,14 @@ namespace WebUI {
|
||||
result += "(";
|
||||
result += device_address();
|
||||
result += "):Status=";
|
||||
if (SerialBT.hasClient())
|
||||
if (SerialBT.hasClient()) {
|
||||
result += "Connected with " + _btclient;
|
||||
else
|
||||
} else {
|
||||
result += "Not connected";
|
||||
} else
|
||||
}
|
||||
} else {
|
||||
result += "No BT";
|
||||
}
|
||||
result += "]\r\n";
|
||||
return result.c_str();
|
||||
}
|
||||
@@ -88,8 +90,9 @@ namespace WebUI {
|
||||
//only letter and digit
|
||||
for (int i = 0; i < strlen(hostname); i++) {
|
||||
c = hostname[i];
|
||||
if (!(isdigit(c) || isalpha(c) || c == '_'))
|
||||
if (!(isdigit(c) || isalpha(c) || c == '_')) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
@@ -111,14 +114,15 @@ namespace WebUI {
|
||||
end();
|
||||
_btname = bt_name->get();
|
||||
if (wifi_radio_mode->get() == ESP_BT) {
|
||||
if (!SerialBT.begin(_btname))
|
||||
if (!SerialBT.begin(_btname)) {
|
||||
report_status_message(STATUS_BT_FAIL_BEGIN, CLIENT_ALL);
|
||||
else {
|
||||
} else {
|
||||
SerialBT.register_callback(&my_spp_cb);
|
||||
grbl_sendf(CLIENT_ALL, "[MSG:BT Started with %s]\r\n", _btname.c_str());
|
||||
}
|
||||
} else
|
||||
} else {
|
||||
end();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
|
@@ -37,20 +37,24 @@ namespace WebUI {
|
||||
uint32_t timeout = millis();
|
||||
esp_task_wdt_reset(); //for a wait 0;
|
||||
//wait feeding WDT
|
||||
while ((millis() - timeout) < milliseconds)
|
||||
while ((millis() - timeout) < milliseconds) {
|
||||
esp_task_wdt_reset();
|
||||
}
|
||||
}
|
||||
|
||||
bool COMMANDS::isLocalPasswordValid(char* password) {
|
||||
char c;
|
||||
//limited size
|
||||
if ((strlen(password) > MAX_LOCAL_PASSWORD_LENGTH) || (strlen(password) < MIN_LOCAL_PASSWORD_LENGTH))
|
||||
if ((strlen(password) > MAX_LOCAL_PASSWORD_LENGTH) || (strlen(password) < MIN_LOCAL_PASSWORD_LENGTH)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
//no space allowed
|
||||
for (int i = 0; i < strlen(password); i++) {
|
||||
c = password[i];
|
||||
if (c == ' ')
|
||||
if (c == ' ') {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
@@ -68,8 +72,7 @@ namespace WebUI {
|
||||
//in case of restart requested
|
||||
if (restart_ESP_module) {
|
||||
ESP.restart();
|
||||
while (1)
|
||||
;
|
||||
while (1) {}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@@ -52,27 +52,30 @@ namespace WebUI {
|
||||
|
||||
void ESPResponseStream::println(const char* data) {
|
||||
print(data);
|
||||
if (_client == CLIENT_TELNET)
|
||||
if (_client == CLIENT_TELNET) {
|
||||
print("\r\n");
|
||||
else
|
||||
} else {
|
||||
print("\n");
|
||||
}
|
||||
}
|
||||
|
||||
//helper to format size to readable string
|
||||
String ESPResponseStream::formatBytes(uint64_t bytes) {
|
||||
if (bytes < 1024)
|
||||
if (bytes < 1024) {
|
||||
return String((uint16_t)bytes) + " B";
|
||||
else if (bytes < (1024 * 1024))
|
||||
} else if (bytes < (1024 * 1024)) {
|
||||
return String((float)(bytes / 1024.0), 2) + " KB";
|
||||
else if (bytes < (1024 * 1024 * 1024))
|
||||
} else if (bytes < (1024 * 1024 * 1024)) {
|
||||
return String((float)(bytes / 1024.0 / 1024.0), 2) + " MB";
|
||||
else
|
||||
} else {
|
||||
return String((float)(bytes / 1024.0 / 1024.0 / 1024.0), 2) + " GB";
|
||||
}
|
||||
}
|
||||
|
||||
void ESPResponseStream::print(const char* data) {
|
||||
if (_client == CLIENT_INPUT)
|
||||
if (_client == CLIENT_INPUT) {
|
||||
return;
|
||||
}
|
||||
#if defined(ENABLE_HTTP) && defined(ENABLE_WIFI)
|
||||
if (_webserver) {
|
||||
if (!_header_sent) {
|
||||
@@ -82,6 +85,7 @@ namespace WebUI {
|
||||
_webserver->send(200);
|
||||
_header_sent = true;
|
||||
}
|
||||
|
||||
_buffer += data;
|
||||
if (_buffer.length() > 1200) {
|
||||
//send data
|
||||
@@ -92,8 +96,9 @@ namespace WebUI {
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
if (_client == CLIENT_WEBUI)
|
||||
if (_client == CLIENT_WEBUI) {
|
||||
return; //this is sanity check
|
||||
}
|
||||
grbl_send(_client, data);
|
||||
}
|
||||
|
||||
@@ -102,8 +107,10 @@ namespace WebUI {
|
||||
if (_webserver) {
|
||||
if (_header_sent) {
|
||||
//send data
|
||||
if (_buffer.length() > 0)
|
||||
if (_buffer.length() > 0) {
|
||||
_webserver->sendContent(_buffer);
|
||||
}
|
||||
|
||||
//close connection
|
||||
_webserver->sendContent("");
|
||||
}
|
||||
|
@@ -43,15 +43,17 @@ namespace WebUI {
|
||||
|
||||
int InputBuffer::available() { return _RXbufferSize; }
|
||||
|
||||
int InputBuffer::availableforwrite() { return (RXBUFFERSIZE - _RXbufferSize); }
|
||||
int InputBuffer::availableforwrite() { return RXBUFFERSIZE - _RXbufferSize; }
|
||||
|
||||
size_t InputBuffer::write(uint8_t c) {
|
||||
if ((1 + _RXbufferSize) <= RXBUFFERSIZE) {
|
||||
int current = _RXbufferpos + _RXbufferSize;
|
||||
if (current > RXBUFFERSIZE)
|
||||
if (current > RXBUFFERSIZE) {
|
||||
current = current - RXBUFFERSIZE;
|
||||
if (current > (RXBUFFERSIZE - 1))
|
||||
}
|
||||
if (current > (RXBUFFERSIZE - 1)) {
|
||||
current = 0;
|
||||
}
|
||||
_RXbuffer[current] = c;
|
||||
current++;
|
||||
_RXbufferSize += 1;
|
||||
@@ -67,21 +69,24 @@ namespace WebUI {
|
||||
}
|
||||
|
||||
int InputBuffer::peek(void) {
|
||||
if (_RXbufferSize > 0)
|
||||
if (_RXbufferSize > 0) {
|
||||
return _RXbuffer[_RXbufferpos];
|
||||
else
|
||||
} else {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
bool InputBuffer::push(const char* data) {
|
||||
int data_size = strlen(data);
|
||||
if ((data_size + _RXbufferSize) <= RXBUFFERSIZE) {
|
||||
int current = _RXbufferpos + _RXbufferSize;
|
||||
if (current > RXBUFFERSIZE)
|
||||
if (current > RXBUFFERSIZE) {
|
||||
current = current - RXBUFFERSIZE;
|
||||
}
|
||||
for (int i = 0; i < data_size; i++) {
|
||||
if (current > (RXBUFFERSIZE - 1))
|
||||
if (current > (RXBUFFERSIZE - 1)) {
|
||||
current = 0;
|
||||
}
|
||||
_RXbuffer[current] = data[i];
|
||||
current++;
|
||||
}
|
||||
@@ -95,12 +100,14 @@ namespace WebUI {
|
||||
if (_RXbufferSize > 0) {
|
||||
int v = _RXbuffer[_RXbufferpos];
|
||||
_RXbufferpos++;
|
||||
if (_RXbufferpos > (RXBUFFERSIZE - 1))
|
||||
if (_RXbufferpos > (RXBUFFERSIZE - 1)) {
|
||||
_RXbufferpos = 0;
|
||||
}
|
||||
_RXbufferSize--;
|
||||
return v;
|
||||
} else
|
||||
} else {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
void InputBuffer::flush(void) {
|
||||
|
@@ -63,8 +63,9 @@ namespace WebUI {
|
||||
while (client.connected() && ((millis() - starttimeout) < timeout)) {
|
||||
answer = client.readStringUntil('\n');
|
||||
log_d("Answer: %s", answer.c_str());
|
||||
if ((answer.indexOf(linetrigger) != -1) || (strlen(linetrigger) == 0))
|
||||
if ((answer.indexOf(linetrigger) != -1) || (strlen(linetrigger) == 0)) {
|
||||
break;
|
||||
}
|
||||
COMMANDS::wait(10);
|
||||
}
|
||||
if (strlen(expected_answer) == 0) {
|
||||
@@ -95,8 +96,9 @@ namespace WebUI {
|
||||
}
|
||||
|
||||
bool NotificationsService::sendMSG(const char* title, const char* message) {
|
||||
if (!_started)
|
||||
if (!_started) {
|
||||
return false;
|
||||
}
|
||||
if (!((strlen(title) == 0) && (strlen(message) == 0))) {
|
||||
switch (_notificationType) {
|
||||
case ESP_PUSHOVER_NOTIFICATION: return sendPushoverMSG(title, message); break;
|
||||
@@ -262,8 +264,10 @@ namespace WebUI {
|
||||
bool NotificationsService::getPortFromSettings() {
|
||||
String tmp = notification_ts->get();
|
||||
int pos = tmp.lastIndexOf(':');
|
||||
if (pos == -1)
|
||||
if (pos == -1) {
|
||||
return false;
|
||||
}
|
||||
|
||||
_port = tmp.substring(pos + 1).toInt();
|
||||
log_d("port : %d", _port);
|
||||
return _port > 0;
|
||||
@@ -273,8 +277,10 @@ namespace WebUI {
|
||||
String tmp = notification_ts->get();
|
||||
int pos1 = tmp.indexOf('#');
|
||||
int pos2 = tmp.lastIndexOf(':');
|
||||
if ((pos1 == -1) || (pos2 == -1))
|
||||
if ((pos1 == -1) || (pos2 == -1)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
//TODO add a check for valid email ?
|
||||
_serveraddress = tmp.substring(pos1 + 1, pos2);
|
||||
log_d("server : %s", _serveraddress.c_str());
|
||||
@@ -284,8 +290,9 @@ namespace WebUI {
|
||||
bool NotificationsService::getEmailFromSettings() {
|
||||
String tmp = notification_ts->get();
|
||||
int pos = tmp.indexOf('#');
|
||||
if (pos == -1)
|
||||
if (pos == -1) {
|
||||
return false;
|
||||
}
|
||||
_settings = tmp.substring(0, pos);
|
||||
log_d("email : %s", _settings.c_str());
|
||||
//TODO add a check for valid email ?
|
||||
@@ -319,16 +326,21 @@ namespace WebUI {
|
||||
default: return false; break;
|
||||
}
|
||||
bool res = true;
|
||||
if (WiFi.getMode() != WIFI_STA)
|
||||
if (WiFi.getMode() != WIFI_STA) {
|
||||
res = false;
|
||||
if (!res)
|
||||
}
|
||||
if (!res) {
|
||||
end();
|
||||
}
|
||||
_started = res;
|
||||
return _started;
|
||||
}
|
||||
|
||||
void NotificationsService::end() {
|
||||
if (!_started)
|
||||
if (!_started) {
|
||||
return;
|
||||
}
|
||||
|
||||
_started = false;
|
||||
_notificationType = 0;
|
||||
_token1 = "";
|
||||
|
@@ -252,6 +252,7 @@ namespace WebUI {
|
||||
if (SPIFFS.exists(pathWithGz)) {
|
||||
path = pathWithGz;
|
||||
}
|
||||
|
||||
File file = SPIFFS.open(path, FILE_READ);
|
||||
_webserver->streamFile(file, contentType);
|
||||
file.close();
|
||||
@@ -302,6 +303,7 @@ namespace WebUI {
|
||||
_webserver->client().write(buf, 1024);
|
||||
i += v;
|
||||
}
|
||||
|
||||
if (i >= totalFileSize) {
|
||||
done = true;
|
||||
}
|
||||
@@ -593,7 +595,7 @@ namespace WebUI {
|
||||
String suserPassword = user_password->get();
|
||||
|
||||
if (!(sUser == DEFAULT_ADMIN_LOGIN && sPassword == sadminPassword) ||
|
||||
(sUser == DEFAULT_USER_LOGIN && sPassword == suserPassword)) {
|
||||
(sUser == DEFAULT_USER_LOGIN && sPassword == suserPassword)) {
|
||||
msg_alert_error = true;
|
||||
smsg = "Error: Incorrect password";
|
||||
code = 401;
|
||||
@@ -607,7 +609,6 @@ namespace WebUI {
|
||||
//change password
|
||||
if (_webserver->hasArg("PASSWORD") && _webserver->hasArg("USER") && _webserver->hasArg("NEWPASSWORD") &&
|
||||
(msg_alert_error == false)) {
|
||||
|
||||
String newpassword = _webserver->arg("NEWPASSWORD");
|
||||
|
||||
char pwdbuf[MAX_LOCAL_PASSWORD_LENGTH + 1];
|
||||
|
@@ -394,8 +394,9 @@ namespace WebUI {
|
||||
size_t flashsize = 0;
|
||||
if (esp_ota_get_running_partition()) {
|
||||
const esp_partition_t* partition = esp_ota_get_next_update_partition(NULL);
|
||||
if (partition)
|
||||
if (partition) {
|
||||
flashsize = partition->size;
|
||||
}
|
||||
}
|
||||
webPrintln("Available Size for update: ", ESPResponseStream::formatBytes(flashsize));
|
||||
webPrintln("Available Size for SPIFFS: ", ESPResponseStream::formatBytes(SPIFFS.totalBytes()));
|
||||
@@ -513,10 +514,12 @@ namespace WebUI {
|
||||
webPrint("Status: ");
|
||||
if (SerialBT.hasClient()) {
|
||||
webPrintln("Connected with ", bt_config._btclient);
|
||||
} else
|
||||
} else {
|
||||
webPrintln("Not connected");
|
||||
} else
|
||||
}
|
||||
} else {
|
||||
webPrintln("Off");
|
||||
}
|
||||
#endif
|
||||
#ifdef ENABLE_NOTIFICATIONS
|
||||
webPrint("Notifications: ");
|
||||
@@ -776,12 +779,14 @@ namespace WebUI {
|
||||
// Display the radio state
|
||||
bool on = false;
|
||||
#if defined(ENABLE_WIFI)
|
||||
if (WiFi.getMode() != WIFI_MODE_NULL)
|
||||
if (WiFi.getMode() != WIFI_MODE_NULL) {
|
||||
on = true;
|
||||
}
|
||||
#endif
|
||||
#if defined(ENABLE_BLUETOOTH)
|
||||
if (bt_config.Is_BT_on())
|
||||
if (bt_config.Is_BT_on()) {
|
||||
on = true;
|
||||
}
|
||||
#endif
|
||||
webPrintln(on ? "ON" : "OFF");
|
||||
return STATUS_OK;
|
||||
@@ -796,6 +801,7 @@ namespace WebUI {
|
||||
webPrintln("only ON or OFF mode supported!");
|
||||
return STATUS_INVALID_VALUE;
|
||||
}
|
||||
|
||||
//Stop everything
|
||||
#if defined(ENABLE_WIFI)
|
||||
if (WiFi.getMode() != WIFI_MODE_NULL) {
|
||||
|
@@ -216,7 +216,7 @@ namespace WebUI {
|
||||
if (RSSI >= -50) {
|
||||
return 100;
|
||||
}
|
||||
return (2 * (RSSI + 100));
|
||||
return 2 * (RSSI + 100);
|
||||
}
|
||||
|
||||
/*
|
||||
@@ -248,7 +248,7 @@ namespace WebUI {
|
||||
count++;
|
||||
status = WiFi.status();
|
||||
}
|
||||
return (status == WL_CONNECTED);
|
||||
return status == WL_CONNECTED;
|
||||
}
|
||||
|
||||
/*
|
||||
@@ -387,8 +387,9 @@ namespace WebUI {
|
||||
}
|
||||
//start services
|
||||
wifi_services.begin();
|
||||
} else
|
||||
} else {
|
||||
WiFi.mode(WIFI_OFF);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
|
@@ -95,8 +95,9 @@ namespace WebUI {
|
||||
if (!MDNS.begin(h.c_str())) {
|
||||
grbl_send(CLIENT_ALL, "[MSG:Cannot start mDNS]\r\n");
|
||||
no_error = false;
|
||||
} else
|
||||
} else {
|
||||
grbl_sendf(CLIENT_ALL, "[MSG:Start mDNS with hostname:http://%s.local/]\r\n", h.c_str());
|
||||
}
|
||||
}
|
||||
# endif
|
||||
# ifdef ENABLE_HTTP
|
||||
|
Reference in New Issue
Block a user