From b5a478cfc9df27bafe6e7691430b1d3e75818be6 Mon Sep 17 00:00:00 2001 From: bdring Date: Mon, 27 Jul 2020 16:18:27 -0500 Subject: [PATCH] Spindle Updates - protocol.cpp when spindle speed is overridden, spindlee object gets notified - protocol.cpp Reformat - HuanyanySpindle.cpp - Prevent it from running if the $Spindle/Type is changed to Haunyang, but the pins were not defined. - Increase command queue size from 5 to 10. Queue is cleared if M5 is received. - Added spindle override to Huanyang. --- Grbl_Esp32/Spindles/HuanyangSpindle.cpp | 136 ++++++++++++++++++------ Grbl_Esp32/Spindles/SpindleClass.h | 4 + Grbl_Esp32/grbl.h | 2 +- Grbl_Esp32/protocol.cpp | 93 ++++++++-------- 4 files changed, 152 insertions(+), 83 deletions(-) diff --git a/Grbl_Esp32/Spindles/HuanyangSpindle.cpp b/Grbl_Esp32/Spindles/HuanyangSpindle.cpp index c4330a35..fdd9e81f 100644 --- a/Grbl_Esp32/Spindles/HuanyangSpindle.cpp +++ b/Grbl_Esp32/Spindles/HuanyangSpindle.cpp @@ -2,6 +2,8 @@ HuanyangSpindle.cpp This is for a Huanyang VFD based spindle via RS485 Modbus. + Sorry for the lengthy comments, but finding the details on this + VFD was a PITA. I am just trying to help the next person. Part of Grbl_ESP32 2020 - Bart Dring @@ -21,6 +23,21 @@ VFDs are very dangerous. They have high voltages and are very powerful Remove power before changing bits. + ============================================================================== + + If a user changes state or RPM level, the command to do that is sent. If + the command is not responded to a message is sent to serial that there was + a timeout. If the Grbl is in a critical state, an alarm will be generated and + the machine stopped. + + If there are no commands to execute, various status items will be polled. If there + is no response, it will behave as described above. It will stop any running jobs with + an alarm. + + =============================================================================== + + Protocol Details + VFD frequencies are in Hz. Multiply by 60 for RPM before using spindle, VFD must be setup for RS485 and match your spindle @@ -47,26 +64,49 @@ Spindle Talker 2 https://github.com/GilchristT/SpindleTalker2/releases Python https://github.com/RobertOlechowski/Huanyang_VFD + ========================================================================= + Commands ADDR CMD LEN DATA CRC 0x01 0x03 0x01 0x01 0x31 0x88 Start spindle clockwise 0x01 0x03 0x01 0x08 0xF1 0x8E Stop spindle - 0x01 0x03 0x01 0x11 0x30 0x44 Start spindle counter-clockwise + 0x01 0x03 0x01 0x11 0x30 0x44 Start spindle counter-clockwise + + Return values are + 0 = run + 1 = jog + 2 = r/f + 3 = running + 4 = jogging + 5 = r/f + 6 = Braking + 7 = Track start + + ========================================================================== Setting RPM ADDR CMD LEN DATA CRC 0x01 0x05 0x02 0x09 0xC4 0xBF 0x0F Write Frequency (0x9C4 = 2500 = 25.00HZ) + Response is same as data sent + + ========================================================================== + Status registers - Addr Read Len Reg Data Data CRC CRC - 0x01 0x04 0x03 0x00 0x00 0x00 // Set Frequency - 0x01 0x04 0x03 0x01 // Ouput Frequency - 0x01 0x04 0x03 0x02 // Ouput Amps - 0x01 0x04 0x03 0x03 0x00 0x00 0xF0 0x4E // Read RPM - 0x01 0x04 0x03 0x04 // DC voltage - 0x01 0x04 0x03 0x05 // AC voltage - 0x01 0x04 0x03 0x06 // Cont - 0x01 0x04 0x03 0x07 // VFD Temp + Addr Read Len Reg DataH DataL CRC CRC + 0x01 0x04 0x03 0x00 0x00 0x00 CRC CRC // Set Frequency * 100 (25Hz = 2500) + 0x01 0x04 0x03 0x01 0x00 0x00 CRC CRC // Ouput Frequency * 100 + 0x01 0x04 0x03 0x02 0x00 0x00 CRC CRC // Ouput Amps * 10 + 0x01 0x04 0x03 0x03 0x00 0x00 0xF0 0x4E // Read RPM (example CRC shown) + 0x01 0x04 0x03 0x0 0x00 0x00 CRC CRC // DC voltage + 0x01 0x04 0x03 0x05 0x00 0x00 CRC CRC // AC voltage + 0x01 0x04 0x03 0x06 0x00 0x00 CRC CRC // Cont + 0x01 0x04 0x03 0x07 0x00 0x00 CRC CRC // VFD Temp + Message is returned with requested value = (DataH * 16) + DataL (see decimal offset above) + + TODO: + Move CRC Calc to task to free up main task + */ #include "SpindleClass.h" @@ -76,6 +116,7 @@ #define HUANYANG_UART_PORT UART_NUM_2 // hard coded for this port right now #define ECHO_TEST_CTS UART_PIN_NO_CHANGE // CTS pin is not used #define HUANYANG_BUF_SIZE 127 +#define HUANYANG_QUEUE_SIZE 10 // numv\ber of commands that can be queued up. #define RESPONSE_WAIT_TICKS 50 // how long to wait for a response #define HUANYANG_MAX_MSG_SIZE 16 // more than enough for a modbus message #define HUANYANG_POLL_RATE 200 // in milliseconds betwwen commands @@ -113,6 +154,8 @@ QueueHandle_t hy_cmd_queue; static TaskHandle_t vfd_cmdTaskHandle = 0; +bool hy_spindle_ok = true; + // The communications task void vfd_cmd_task(void* pvParameters) { static bool unresponsive = false; // to pop off a message once each time it becomes unresponsive @@ -163,9 +206,16 @@ void vfd_cmd_task(void* pvParameters) { // ================== Class methods ================================== void HuanyangSpindle :: init() { + hy_spindle_ok = true; // initialize + + // fail if required items are not defined + if (!get_pins_and_settings()) { + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Huanyang spindle errors"); + return; + } if (! _task_running) { // init can happen many times, we only want to start one task - hy_cmd_queue = xQueueCreate(5, sizeof(hy_command_t)); + hy_cmd_queue = xQueueCreate(HUANYANG_QUEUE_SIZE, sizeof(hy_command_t)); xTaskCreatePinnedToCore(vfd_cmd_task, // task "vfd_cmdTaskHandle", // name for task 2048, // size of task stack @@ -175,13 +225,7 @@ void HuanyangSpindle :: init() { 0 // core ); _task_running = true; - } - - // fail if required items are not defined - if (!get_pins_and_settings()) { - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Huanyang spindle errors"); - return; - } + } // this allows us to init() again later. // If you change certain settings, init() gets called agian @@ -227,30 +271,38 @@ void HuanyangSpindle :: init() { // It returns a message for each missing pin // Returns true if all pins are defined. bool HuanyangSpindle :: get_pins_and_settings() { - bool pins_ok = true; + #ifdef HUANYANG_TXD_PIN _txd_pin = HUANYANG_TXD_PIN; #else - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Missing HUANYANG_TXD_PIN"); - pins_ok = false; + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Undefined HUANYANG_TXD_PIN"); + hy_spindle_ok = false; #endif #ifdef HUANYANG_RXD_PIN _rxd_pin = HUANYANG_RXD_PIN; #else - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "No HUANYANG_RXD_PIN"); - pins_ok = false; + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Undefined HUANYANG_RXD_PIN"); + hy_spindle_ok = false; #endif #ifdef HUANYANG_RTS_PIN _rts_pin = HUANYANG_RTS_PIN; #else - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "No HUANYANG_RTS_PIN"); - pins_ok = false; + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Undefined HUANYANG_RTS_PIN"); + hy_spindle_ok = false; #endif - return pins_ok; + if (laser_mode->get()) { + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Huanyang spindle disabled in laser mode. Set $GCode/LaserMode=Off and restart"); + hy_spindle_ok = false; + } + + _min_rpm = rpm_min->get(); + _max_rpm = rpm_max->get(); + + return hy_spindle_ok; } void HuanyangSpindle :: config_message() { @@ -260,7 +312,6 @@ void HuanyangSpindle :: config_message() { pinName(_txd_pin).c_str(), pinName(_rxd_pin).c_str(), pinName(_rts_pin).c_str()); - } @@ -294,6 +345,9 @@ void HuanyangSpindle :: set_state(uint8_t state, uint32_t rpm) { } bool HuanyangSpindle :: set_mode(uint8_t mode, bool critical) { + + if (!hy_spindle_ok) return false; + hy_command_t mode_cmd; mode_cmd.tx_length = 6; @@ -303,12 +357,17 @@ bool HuanyangSpindle :: set_mode(uint8_t mode, bool critical) { mode_cmd.msg[1] = 0x03; mode_cmd.msg[2] = 0x01; - if (mode == SPINDLE_ENABLE_CW) + if (mode == SPINDLE_ENABLE_CW) mode_cmd.msg[3] = 0x01; else if (mode == SPINDLE_ENABLE_CCW) mode_cmd.msg[3] = 0x11; - else //SPINDLE_DISABLE - mode_cmd.msg[3] = 0x08; + else { //SPINDLE_DISABLE + mode_cmd.msg[3] = 0x08; + + if (! xQueueReset(hy_cmd_queue)) { + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "VFD spindle off, queue could not be reset"); + } + } add_ModRTU_CRC(mode_cmd.msg, mode_cmd.rx_length); @@ -322,10 +381,23 @@ bool HuanyangSpindle :: set_mode(uint8_t mode, bool critical) { uint32_t HuanyangSpindle :: set_rpm(uint32_t rpm) { + if (!hy_spindle_ok) return 0; + hy_command_t rpm_cmd; + // apply override + 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)) + rpm = _max_rpm; + else if (rpm != 0 && rpm <= _min_rpm) + rpm = _min_rpm; + + sys.spindle_speed = rpm; + if (rpm == _current_rpm) // prevent setting same RPM twice - return rpm; + return rpm; _current_rpm = rpm; @@ -380,7 +452,7 @@ void HuanyangSpindle :: read_value(uint8_t reg) { grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "VFD Queue Full"); } -void HuanyangSpindle ::stop() { +void HuanyangSpindle ::stop() { set_mode(SPINDLE_DISABLE, false); } diff --git a/Grbl_Esp32/Spindles/SpindleClass.h b/Grbl_Esp32/Spindles/SpindleClass.h index 6258b80d..28a7fdd9 100644 --- a/Grbl_Esp32/Spindles/SpindleClass.h +++ b/Grbl_Esp32/Spindles/SpindleClass.h @@ -175,6 +175,10 @@ class HuanyangSpindle : public Spindle { void stop(); static void read_value(uint8_t reg); static void add_ModRTU_CRC(char* buf, int full_msg_len); + + protected: + uint32_t _min_rpm; + uint32_t _max_rpm; diff --git a/Grbl_Esp32/grbl.h b/Grbl_Esp32/grbl.h index 8676f771..5741f337 100644 --- a/Grbl_Esp32/grbl.h +++ b/Grbl_Esp32/grbl.h @@ -22,7 +22,7 @@ // Grbl versioning system #define GRBL_VERSION "1.3a" -#define GRBL_VERSION_BUILD "20200726" +#define GRBL_VERSION_BUILD "20200727" //#include diff --git a/Grbl_Esp32/protocol.cpp b/Grbl_Esp32/protocol.cpp index 9464675b..eb236a19 100644 --- a/Grbl_Esp32/protocol.cpp +++ b/Grbl_Esp32/protocol.cpp @@ -39,21 +39,17 @@ typedef struct { } client_line_t; client_line_t client_lines[CLIENT_COUNT]; -static void empty_line(uint8_t client) -{ +static void empty_line(uint8_t client) { client_line_t* cl = &client_lines[client]; cl->len = 0; 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) -{ +err_t add_char_to_line(char c, uint8_t client) { client_line_t* cl = &client_lines[client]; // Simple editing for interactive input if (c == '\b') { @@ -64,9 +60,8 @@ 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++; @@ -77,21 +72,17 @@ err_t add_char_to_line(char c, uint8_t client) return STATUS_OK; } -err_t execute_line(char* line, uint8_t client, auth_t auth_level) -{ +err_t execute_line(char* line, uint8_t client, auth_t 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); } @@ -156,25 +147,25 @@ void protocol_main_loop() { while ((c = serial_read(client)) != SERIAL_NO_DATA) { err_t res = add_char_to_line(c, client); switch (res) { - case STATUS_OK: - break; - case STATUS_EOL: - protocol_execute_realtime(); // Runtime command check point. - if (sys.abort) { - return; // Bail to calling function upon system abort - } - line = client_lines[client].buffer; + case STATUS_OK: + break; + case STATUS_EOL: + protocol_execute_realtime(); // Runtime command check point. + if (sys.abort) { + return; // Bail to calling function upon system abort + } + line = client_lines[client].buffer; #ifdef REPORT_ECHO_RAW_LINE_RECEIVED - report_echo_line_received(line, client); + report_echo_line_received(line, client); #endif - // auth_level can be upgraded by supplying a password on the command line - report_status_message(execute_line(line, client, LEVEL_GUEST), client); - empty_line(client); - break; - case STATUS_OVERFLOW: - report_status_message(STATUS_OVERFLOW, client); - empty_line(client); - break; + // auth_level can be upgraded by supplying a password on the command line + report_status_message(execute_line(line, client, LEVEL_GUEST), client); + empty_line(client); + break; + case STATUS_OVERFLOW: + report_status_message(STATUS_OVERFLOW, client); + empty_line(client); + break; } } // while serial read } // for clients @@ -188,9 +179,8 @@ 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 */ @@ -456,6 +446,9 @@ void protocol_exec_rt_system() { bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM); 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 != SPINDLE_DISABLE) + spindle->set_rpm(gc_state.spindle_speed); } if (rt_exec & EXEC_SPINDLE_OVR_STOP) { // Spindle stop override allowed only while in HOLD state. @@ -564,11 +557,11 @@ static void protocol_exec_rt_suspend() { // NOTE: State is will remain DOOR, until the de-energizing and retract is complete. #ifdef ENABLE_PARKING_OVERRIDE_CONTROL if (homing_enable->get() && - (parking_target[PARKING_AXIS] < PARKING_TARGET) && - laser_mode->get() && - (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { + (parking_target[PARKING_AXIS] < PARKING_TARGET) && + laser_mode->get() && + (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { #else - if (homing_enable->get() && + if (homing_enable->get() && (parking_target[PARKING_AXIS] < PARKING_TARGET) && laser_mode->get()) { #endif @@ -585,8 +578,8 @@ static void protocol_exec_rt_suspend() { pl_data->condition = (PL_COND_FLAG_SYSTEM_MOTION | PL_COND_FLAG_NO_FEED_OVERRIDE); pl_data->spindle_speed = 0.0; spindle->set_state((SPINDLE_DISABLE, 0); // De-energize - coolant_set_state(COOLANT_DISABLE); // De-energize - // Execute fast parking retract motion to parking target location. + coolant_set_state(COOLANT_DISABLE); // De-energize + // Execute fast parking retract motion to parking target location. if (parking_target[PARKING_AXIS] < PARKING_TARGET) { parking_target[PARKING_AXIS] = PARKING_TARGET; pl_data->feed_rate = PARKING_RATE; @@ -624,7 +617,7 @@ static void protocol_exec_rt_suspend() { // NOTE: State is will remain DOOR, until the de-energizing and retract is complete. #ifdef ENABLE_PARKING_OVERRIDE_CONTROL if (homing_enable->get() && !laser_mode->get()) && - (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { + (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { #else if (homing_enable->get() && !laser_mode->get()) { #endif @@ -638,8 +631,8 @@ static void protocol_exec_rt_suspend() { #endif // Delayed Tasks: Restart spindle and coolant, delay to power-up, then resume cycle. if (gc_state.modal.spindle != SPINDLE_DISABLE) { - // Block if safety door re-opened during prior restore actions. - if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { + // Block if safety door re-opened during prior restore actions. + if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { if (laser_mode->get()) { // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts. bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM); @@ -650,8 +643,8 @@ static void protocol_exec_rt_suspend() { } } if (gc_state.modal.coolant != COOLANT_DISABLE) { - // Block if safety door re-opened during prior restore actions. - if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { + // 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. coolant_set_state((restore_condition & (PL_COND_FLAG_COOLANT_FLOOD | PL_COND_FLAG_COOLANT_FLOOD))); delay_sec(SAFETY_DOOR_COOLANT_DELAY, DELAY_MODE_SYS_SUSPEND); @@ -661,7 +654,7 @@ static void protocol_exec_rt_suspend() { // Execute slow plunge motion from pull-out position to resume position. #ifdef ENABLE_PARKING_OVERRIDE_CONTROL if (homing_enable->get() && !laser_mode->get()) && - (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { + (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { #else if (homing_enable->get() && !laser_mode->get()) { #endif @@ -678,8 +671,8 @@ static void protocol_exec_rt_suspend() { } #endif if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { - sys.suspend |= SUSPEND_RESTORE_COMPLETE; - system_set_exec_state_flag(EXEC_CYCLE_START); // Set to resume program. + sys.suspend |= SUSPEND_RESTORE_COMPLETE; + system_set_exec_state_flag(EXEC_CYCLE_START); // Set to resume program. } } }