From 212caccd0ea7e894a0dd82eecf407bbb43addcb8 Mon Sep 17 00:00:00 2001 From: Jim Morris Date: Sun, 11 Oct 2015 22:28:08 -0700 Subject: [PATCH] use push/pop state to save current state on suspend/resume fix speed issues in extruder after suspend resume when left in relative mode --- src/modules/robot/Robot.cpp | 37 +++++++++++++------- src/modules/robot/Robot.h | 4 ++- src/modules/tools/extruder/Extruder.cpp | 6 ++-- src/modules/utils/player/Player.cpp | 45 +++++++++++++++---------- src/modules/utils/player/Player.h | 5 --- 5 files changed, 58 insertions(+), 39 deletions(-) diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp index 78ada0a7..aaf986e9 100644 --- a/src/modules/robot/Robot.cpp +++ b/src/modules/robot/Robot.cpp @@ -251,6 +251,26 @@ void Robot::on_config_reload(void *argument) //this->clearToolOffset(); } +void Robot::push_state() +{ + bool am= this->absolute_mode; + bool im= this->inch_mode; + saved_state_t s(this->feed_rate, this->seek_rate, am, im); + state_stack.push(s); +} + +void Robot::pop_state() +{ + if(!state_stack.empty()) { + auto s= state_stack.top(); + state_stack.pop(); + this->feed_rate= std::get<0>(s); + this->seek_rate= std::get<1>(s); + this->absolute_mode= std::get<2>(s); + this->inch_mode= std::get<3>(s); + } +} + // this does a sanity check that actuator speeds do not exceed steps rate capability // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency void Robot::check_max_actuator_speeds() @@ -361,21 +381,12 @@ void Robot::on_gcode_received(void *argument) } return; - case 120: { // push state - bool b= this->absolute_mode; - saved_state_t s(this->feed_rate, this->seek_rate, b); - state_stack.push(s); - } - break; + case 120: // push state + push_state(); + break; case 121: // pop state - if(!state_stack.empty()) { - auto s= state_stack.top(); - state_stack.pop(); - this->feed_rate= std::get<0>(s); - this->seek_rate= std::get<1>(s); - this->absolute_mode= std::get<2>(s); - } + pop_state(); break; case 203: // M203 Set maximum feedrates in mm/sec diff --git a/src/modules/robot/Robot.h b/src/modules/robot/Robot.h index 3294ecf3..90f48743 100644 --- a/src/modules/robot/Robot.h +++ b/src/modules/robot/Robot.h @@ -37,6 +37,8 @@ class Robot : public Module { float get_z_maxfeedrate() const { return this->max_speeds[2]; } void setToolOffset(const float offset[3]); float get_feed_rate() const { return feed_rate; } + void push_state(); + void pop_state(); BaseSolution* arm_solution; // Selected Arm solution ( millimeters to step calculation ) @@ -63,7 +65,7 @@ class Robot : public Module { void clearToolOffset(); void check_max_actuator_speeds(); - typedef std::tuple saved_state_t; // save current feedrate and absolute mode + typedef std::tuple saved_state_t; // save current feedrate and absolute mode, inch mode std::stack state_stack; // saves state from M120 float last_milestone[3]; // Last position, in millimeters float transformed_last_milestone[3]; // Last transformed position diff --git a/src/modules/tools/extruder/Extruder.cpp b/src/modules/tools/extruder/Extruder.cpp index 2e31947c..212f5b79 100644 --- a/src/modules/tools/extruder/Extruder.cpp +++ b/src/modules/tools/extruder/Extruder.cpp @@ -265,7 +265,6 @@ void Extruder::on_set_public_data(void *argument) if(!pdr->starts_with(extruder_checksum)) return; // handle extrude rates request from robot - // TODO if not in volumetric mode then limit speed based on max_speed if(pdr->second_element_is(target_checksum)) { // disabled extruders do not reply NOTE only one enabled extruder supported if(!this->enabled) return; @@ -287,8 +286,9 @@ void Extruder::on_set_public_data(void *argument) this->saved_absolute_mode = this->absolute_mode; pdr->set_taken(); } else if(pdr->second_element_is(restore_state_checksum)) { - this->current_position = this->saved_current_position; - this->absolute_mode = this->saved_absolute_mode; + // NOTE this only gets called when the queue is empty so the milestones will be the same + this->milestone_last_position= this->current_position = this->saved_current_position; + this->milestone_absolute_mode= this->absolute_mode = this->saved_absolute_mode; pdr->set_taken(); } } diff --git a/src/modules/utils/player/Player.cpp b/src/modules/utils/player/Player.cpp index 0ca13d37..3eb1a329 100644 --- a/src/modules/utils/player/Player.cpp +++ b/src/modules/utils/player/Player.cpp @@ -502,16 +502,17 @@ void Player::suspend_command(string parameters, StreamOutput *stream ) // we need to allow main loop to cycle a few times to clear any buffered commands in the serial streams etc suspend_loops= 10; - suspend_stream= stream; } // this completes the suspend void Player::suspend_part2() { + // need to use streams here as the original stream may have changed + THEKERNEL->streams->printf("// Waiting for queue to empty (Host must stop sending)...\n"); // wait for queue to empty THEKERNEL->conveyor->wait_for_empty_queue(); - suspend_stream->printf("// Saving current state...\n"); + THEKERNEL->streams->printf("// Saving current state...\n"); // save current XYZ position THEKERNEL->robot->get_axis_position(this->saved_position); @@ -519,10 +520,8 @@ void Player::suspend_part2() // save current extruder state PublicData::set_value( extruder_checksum, save_state_checksum, nullptr ); - // save state - this->saved_inch_mode= THEKERNEL->robot->inch_mode; - this->saved_absolute_mode= THEKERNEL->robot->absolute_mode; - this->saved_feed_rate= THEKERNEL->robot->get_feed_rate() * 60; // save in mm/min + // save state use M120 + THEKERNEL->robot->push_state(); // TODO retract by optional amount... @@ -556,7 +555,7 @@ void Player::suspend_part2() THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message ); } - suspend_stream->printf("// Print Suspended, enter resume to continue printing\n"); + THEKERNEL->streams->printf("// Print Suspended, enter resume to continue printing\n"); } /** @@ -575,14 +574,13 @@ void Player::resume_command(string parameters, StreamOutput *stream ) stream->printf("resuming print...\n"); - // set heaters to saved temps - for(auto& h : this->saved_temperatures) { - float t= h.second; - PublicData::set_value( temperature_control_checksum, h.first, &t ); - } - // wait for them to reach temp if(!this->saved_temperatures.empty()) { + // set heaters to saved temps + for(auto& h : this->saved_temperatures) { + float t= h.second; + PublicData::set_value( temperature_control_checksum, h.first, &t ); + } stream->printf("Waiting for heaters...\n"); bool wait= true; uint32_t tus= us_ticker_read(); // mbed call @@ -607,6 +605,15 @@ void Player::resume_command(string parameters, StreamOutput *stream ) if(wait) THEKERNEL->call_event(ON_IDLE, this); + + if(THEKERNEL->is_halted()) { + // abort temp wait and rest of resume + THEKERNEL->streams->printf("Resume aborted by kill\n"); + THEKERNEL->robot->pop_state(); + this->saved_temperatures.clear(); + suspended= false; + return; + } } } @@ -621,15 +628,18 @@ void Player::resume_command(string parameters, StreamOutput *stream ) // Restore position stream->printf("Restoring saved XYZ positions and state...\n"); - THEKERNEL->robot->inch_mode= saved_inch_mode; - THEKERNEL->robot->absolute_mode= saved_absolute_mode; - char buf[128]; + THEKERNEL->robot->pop_state(); + bool abs_mode= THEKERNEL->robot->absolute_mode; // what mode we were in + // force absolute mode for restoring position, then set to the saved relative/absolute mode + THEKERNEL->robot->absolute_mode= true; { - int n = snprintf(buf, sizeof(buf), "G1 X%f Y%f Z%f F%f", saved_position[0], saved_position[1], saved_position[2], saved_feed_rate); + char buf[128]; + int n = snprintf(buf, sizeof(buf), "G1 X%f Y%f Z%f", saved_position[0], saved_position[1], saved_position[2]); string g(buf, n); Gcode gcode(g, &(StreamOutput::NullStream)); THEKERNEL->call_event(ON_GCODE_RECEIVED, &gcode ); } + THEKERNEL->robot->absolute_mode= abs_mode; // restore extruder state PublicData::set_value( extruder_checksum, restore_state_checksum, nullptr ); @@ -638,6 +648,7 @@ void Player::resume_command(string parameters, StreamOutput *stream ) if(this->was_playing_file) { this->playing_file = true; + this->was_playing_file= false; }else{ // Send resume to host THEKERNEL->streams->printf("// action:resume\r\n"); diff --git a/src/modules/utils/player/Player.h b/src/modules/utils/player/Player.h index c651d90c..0f5b4fcb 100644 --- a/src/modules/utils/player/Player.h +++ b/src/modules/utils/player/Player.h @@ -46,26 +46,21 @@ class Player : public Module { string on_boot_gcode; StreamOutput* current_stream; StreamOutput* reply_stream; - StreamOutput* suspend_stream; FILE* current_file_handler; long file_size; unsigned long played_cnt; unsigned long elapsed_secs; float saved_position[3]; - float saved_feed_rate; std::map saved_temperatures; struct { bool on_boot_gcode_enable:1; bool booted:1; bool playing_file:1; bool suspended:1; - bool saved_inch_mode:1; - bool saved_absolute_mode:1; bool was_playing_file:1; bool leave_heaters_on:1; uint8_t suspend_loops:4; - }; }; -- 2.20.1