// Compute extrusion speed based on parameters and gcode distance of travel void Extruder::on_gcode_execute(void* argument) { Gcode* gcode = static_cast<Gcode*>(argument); // Absolute/relative mode if( gcode->has_m ) { if( gcode->m == 82 ) { this->absolute_mode = true; } if( gcode->m == 83 ) { this->absolute_mode = false; } if( gcode->m == 84 ) { this->en_pin->set(1); } } // The mode is OFF by default, and SOLO or FOLLOW only if we need to extrude this->mode = OFF; if( gcode->has_g ) { // G92: Reset extruder position if( gcode->g == 92 ) { if( gcode->has_letter('E') ) { this->current_position = gcode->get_value('E'); this->target_position = this->current_position; this->current_steps = int(floor(this->steps_per_millimeter * this->current_position)); } else if( gcode->get_num_args() == 0) { this->current_position = 0.0; this->target_position = this->current_position; this->current_steps = 0; } } else { // Extrusion length from 'G' Gcode if( gcode->has_letter('E' )) { // Get relative extrusion distance depending on mode ( in absolute mode we must substract target_position ) double relative_extrusion_distance = gcode->get_value('E'); if( this->absolute_mode == true ) { relative_extrusion_distance = relative_extrusion_distance - this->target_position; } // If the robot is moving, we follow it's movement, otherwise, we move alone if( fabs(gcode->millimeters_of_travel) < 0.0001 ) { // With floating numbers, we can have 0 != 0 ... beeeh. For more info see : http://upload.wikimedia.org/wikipedia/commons/0/0a/Cain_Henri_Vidal_Tuileries.jpg this->mode = SOLO; this->travel_distance = relative_extrusion_distance; if( gcode->has_letter('F') ) { this->feed_rate = gcode->get_value('F'); } } else { // We move proportionally to the robot's movement this->mode = FOLLOW; this->travel_ratio = relative_extrusion_distance / gcode->millimeters_of_travel; } this->en_pin->set(0); } } } }
// Process and respond to eeprom gcodes (M50x) void Configurator::on_gcode_execute(void* argument){ Gcode* gcode = static_cast<Gcode*>(argument); if( gcode->has_letter('G') ){ int code = gcode->get_value('G'); switch( code ){ } } else if( gcode->has_letter('M') ){ int code = gcode->get_value('M'); switch( code ){ } } }
void TemperatureControl::on_gcode_execute(void* argument){ Gcode* gcode = static_cast<Gcode*>(argument); // Set temperature if( gcode->has_letter('M') && gcode->get_value('M') == 104 && gcode->has_letter('S') ){ this->set_desired_temperature(gcode->get_value('S')); } // Get temperature if( gcode->has_letter('M') && gcode->get_value('M') == 105 ){ this->kernel->serial->printf("get temperature: %f current:%f target:%f \r\n", this->get_temperature(), this->new_thermistor_reading(), this->desired_adc_value ); } }
void SpindleControl::on_gcode_received(void *argument) { Gcode *gcode = static_cast<Gcode *>(argument); if (gcode->has_m) { if (gcode->m == 957) { // M957: report spindle speed report_speed(); } else if (gcode->m == 958) { THECONVEYOR->wait_for_idle(); // M958: set spindle PID parameters if (gcode->has_letter('P')) set_p_term( gcode->get_value('P') ); if (gcode->has_letter('I')) set_p_term( gcode->get_value('I') ); if (gcode->has_letter('D')) set_p_term( gcode->get_value('D') ); // report PID settings get_pid_settings(); } else if (gcode->m == 3) { THECONVEYOR->wait_for_idle(); // M3: Spindle on if(!spindle_on) { turn_on(); } // M3 with S value provided: set speed if (gcode->has_letter('S')) { set_speed(gcode->get_value('S')); } } else if (gcode->m == 5) { THECONVEYOR->wait_for_idle(); // M5: spindle off if(spindle_on) { turn_off(); } } } }
void Extruder::on_gcode_received(void *argument){ Gcode *gcode = static_cast<Gcode*>(argument); // Gcodes to execute immediately if (gcode->has_m){ if (gcode->m == 114){ gcode->stream->printf("E:%4.1f ", this->current_position); gcode->add_nl = true; gcode->mark_as_taken(); } if (gcode->m == 92 ){ double spm = this->steps_per_millimeter; if (gcode->has_letter('E')) spm = gcode->get_value('E'); gcode->stream->printf("E:%g ", spm); gcode->add_nl = true; gcode->mark_as_taken(); } } // Gcodes to pass along to on_gcode_execute if( ( gcode->has_m && (gcode->m == 17 || gcode->m == 18 || gcode->m == 82 || gcode->m == 83 || gcode->m == 84 || gcode->m == 92 ) ) || ( gcode->has_g && gcode->g == 92 && gcode->has_letter('E') ) || ( gcode->has_g && ( gcode->g == 90 || gcode->g == 91 ) ) ){ gcode->mark_as_taken(); if( this->kernel->conveyor->queue.size() == 0 ){ this->kernel->call_event(ON_GCODE_EXECUTE, gcode ); }else{ Block* block = this->kernel->conveyor->queue.get_ref( this->kernel->conveyor->queue.size() - 1 ); block->append_gcode(gcode); } } // Add to the queue for on_gcode_execute to process if( gcode->has_g && gcode->g < 4 && gcode->has_letter('E') ){ if( !gcode->has_letter('X') && !gcode->has_letter('Y') && !gcode->has_letter('Z') ){ // This is a solo move, we add an empty block to the queue //If the queue is empty, execute immediatly, otherwise attach to the last added block if( this->kernel->conveyor->queue.size() == 0 ){ this->kernel->call_event(ON_GCODE_EXECUTE, gcode ); this->append_empty_block(); }else{ Block* block = this->kernel->conveyor->queue.get_ref( this->kernel->conveyor->queue.size() - 1 ); block->append_gcode(gcode); this->append_empty_block(); } } }else{ // This is for follow move } }
// Turn pin on and off void Switch::on_gcode_execute(void* argument){ Gcode* gcode = static_cast<Gcode*>(argument); if( gcode->has_m){ int code = gcode->m; if( code == this->on_m_code ){ if (gcode->has_letter('S')) { int v = gcode->get_value('S') * output_pin.max_pwm() / 256.0; if (v) this->output_pin.pwm(v); else this->output_pin.set(0); } else { // Turn pin on this->output_pin.set(1); } } if( code == this->off_m_code ){ // Turn pin off this->output_pin.set(0); } } }
void ToolManager::on_gcode_received(void *argument) { Gcode *gcode = static_cast<Gcode*>(argument); if( gcode->has_letter('T') ) { int new_tool = gcode->get_value('T'); if(new_tool >= (int)this->tools.size() || new_tool < 0) { // invalid tool char buf[32]; // should be big enough for any status int n = snprintf(buf, sizeof(buf), "T%d invalid tool ", new_tool); gcode->txt_after_ok.append(buf, n); } else { if(new_tool != this->active_tool) { // We must wait for an empty queue before we can disable the current extruder THEKERNEL->conveyor->wait_for_empty_queue(); this->tools[active_tool]->disable(); this->active_tool = new_tool; this->current_tool_name = this->tools[active_tool]->get_name(); this->tools[active_tool]->enable(); //send new_tool_offsets to robot const float *new_tool_offset = tools[new_tool]->get_offset(); THEKERNEL->robot->setToolOffset(new_tool_offset); } } } }
// Turn pin on and off void Switch::on_gcode_execute(void *argument) { Gcode *gcode = static_cast<Gcode *>(argument); if(match_input_on_gcode(gcode)) { int v; this->switch_state = true; if (this->output_type == PWM) { // PWM output pin turn on if(gcode->has_letter('S')) { v = round(gcode->get_value('S') * output_pin.max_pwm() / 255.0); // scale by max_pwm so input of 255 and max_pwm of 128 would set value to 128 this->output_pin.pwm(v); } else { this->output_pin.pwm(this->switch_value); } } else { // logic pin turn on this->output_pin.set(true); } } else if(match_input_off_gcode(gcode)) { this->switch_state = false; if (this->output_type == PWM) { // PWM output pin this->output_pin.set(false); } else { // logic pin turn off this->output_pin.set(false); } } }
void CurrentControl::on_gcode_received(void *argument) { Gcode *gcode = static_cast<Gcode*>(argument); char alpha[8] = { 'X', 'Y', 'Z', 'E', 'A', 'B', 'C', 'D' }; if (gcode->has_m) { if (gcode->m == 907) { int i; for (i = 0; i < 8; i++) { if (gcode->has_letter(alpha[i])){ float c= gcode->get_value(alpha[i]); this->digipot->set_current(i, c); switch(i) { case 0: this->alpha_current= c; break; case 1: this->beta_current= c; break; case 2: this->gamma_current= c; break; case 3: this->delta_current= c; break; case 4: this->epsilon_current= c; break; case 5: this->zeta_current= c; break; case 6: this->eta_current= c; break; case 7: this->theta_current= c; break; } } gcode->stream->printf("%c:%3.1fA%c", alpha[i], this->digipot->get_current(i), (i == 7)?'\n':' '); } }else if(gcode->m == 500 || gcode->m == 503) { if(this->delta_current != this->original_delta_current) { // if not the same as loaded by config then save it gcode->stream->printf(";Extruder current:\nM907 E%1.5f\n", this->delta_current); } } } }
void TemperatureControl::on_gcode_execute(void* argument){ Gcode* gcode = static_cast<Gcode*>(argument); if( gcode->has_m){ if (((gcode->m == this->set_m_code) || (gcode->m == this->set_and_wait_m_code)) && gcode->has_letter('S')) { float v = gcode->get_value('S'); if (v == 0.0) { this->target_temperature = UNDEFINED; this->heater_pin.set((this->o=0)); } else { this->set_desired_temperature(v); if( gcode->m == this->set_and_wait_m_code) { THEKERNEL->pauser->take(); this->waiting = true; } } } } }
// Turn laser on/off depending on received GCodes void Laser::on_gcode_execute(void* argument){ Gcode* gcode = static_cast<Gcode*>(argument); this->laser_on = false; if( gcode->has_g){ int code = gcode->g; if( code == 0 ){ // G0 this->pwm_pin->write(this->pwm_inverting ? 1 - this->laser_minimum_power : this->laser_minimum_power); this->laser_on = false; }else if( code >= 1 && code <= 3 ){ // G1, G2, G3 this->laser_on = true; } } if ( gcode->has_letter('S' )){ float requested_power = gcode->get_value('S') / this->laser_maximum_s_value; // Ensure we can't exceed maximum power if (requested_power > 1) requested_power = 1; this->laser_power = requested_power; } if (this->ttl_used) this->ttl_pin->set(this->laser_on); }
void TemperatureControl::on_gcode_received(void* argument){ Gcode* gcode = static_cast<Gcode*>(argument); if (gcode->has_m) { // Get temperature if( gcode->m == this->get_m_code ){ char buf[32]; // should be big enough for any status int n= snprintf(buf, sizeof(buf), "%s:%3.1f /%3.1f @%d ", this->designator.c_str(), this->get_temperature(), ((target_temperature == UNDEFINED)?0.0:target_temperature), this->o); gcode->txt_after_ok.append(buf, n); gcode->mark_as_taken(); } else if (gcode->m == 301) { gcode->mark_as_taken(); if (gcode->has_letter('S') && (gcode->get_value('S') == this->pool_index)) { if (gcode->has_letter('P')) setPIDp( gcode->get_value('P') ); if (gcode->has_letter('I')) setPIDi( gcode->get_value('I') ); if (gcode->has_letter('D')) setPIDd( gcode->get_value('D') ); if (gcode->has_letter('X')) this->i_max = gcode->get_value('X'); } //gcode->stream->printf("%s(S%d): Pf:%g If:%g Df:%g X(I_max):%g Pv:%g Iv:%g Dv:%g O:%d\n", this->designator.c_str(), this->pool_index, this->p_factor, this->i_factor/this->PIDdt, this->d_factor*this->PIDdt, this->i_max, this->p, this->i, this->d, o); gcode->stream->printf("%s(S%d): Pf:%g If:%g Df:%g X(I_max):%g O:%d\n", this->designator.c_str(), this->pool_index, this->p_factor, this->i_factor/this->PIDdt, this->d_factor*this->PIDdt, this->i_max, o); } else if (gcode->m == 303) { if (gcode->has_letter('E') && (gcode->get_value('E') == this->pool_index)) { gcode->mark_as_taken(); float target = 150.0; if (gcode->has_letter('S')) { target = gcode->get_value('S'); gcode->stream->printf("Target: %5.1f\n", target); } int ncycles= 8; if (gcode->has_letter('C')) { ncycles= gcode->get_value('C'); } gcode->stream->printf("Start PID tune, command is %s\n", gcode->command.c_str()); this->pool->PIDtuner->begin(this, target, gcode->stream, ncycles); } } else if (gcode->m == 500 || gcode->m == 503){// M500 saves some volatile settings to config override file, M503 just prints the settings gcode->stream->printf(";PID settings:\nM301 S%d P%1.4f I%1.4f D%1.4f\n", this->pool_index, this->p_factor, this->i_factor/this->PIDdt, this->d_factor*this->PIDdt); gcode->mark_as_taken(); } else if( ( gcode->m == this->set_m_code || gcode->m == this->set_and_wait_m_code ) && gcode->has_letter('S') ) { // Attach gcodes to the last block for on_gcode_execute THEKERNEL->conveyor->append_gcode(gcode); // push an empty block if we have to wait, so the Planner can get things right, and we can prevent subsequent non-move gcodes from executing if (gcode->m == this->set_and_wait_m_code) // ensure that no subsequent gcodes get executed with our M109 or similar THEKERNEL->conveyor->queue_head_block(); } } }
void TemperatureSwitch::on_gcode_received(void *argument) { Gcode *gcode = static_cast<Gcode *>(argument); if(gcode->has_m && gcode->m == this->arm_mcode) { this->armed= (gcode->has_letter('S') && gcode->get_value('S') != 0); gcode->stream->printf("temperature switch %s\n", this->armed ? "armed" : "disarmed"); } }
void Spindle::on_gcode_received(void* argument) { Gcode *gcode = static_cast<Gcode *>(argument); if (gcode->has_m) { if (gcode->m == 957) { // M957: report spindle speed THEKERNEL->streams->printf("Current RPM: %5.0f Target RPM: %5.0f PWM value: %5.3f\n", current_rpm, target_rpm, current_pwm_value); } else if (gcode->m == 958) { // M958: set spindle PID parameters if (gcode->has_letter('P')) control_P_term = gcode->get_value('P'); if (gcode->has_letter('I')) control_I_term = gcode->get_value('I'); if (gcode->has_letter('D')) control_D_term = gcode->get_value('D'); THEKERNEL->streams->printf("P: %0.6f I: %0.6f D: %0.6f\n", control_P_term, control_I_term, control_D_term); } else if (gcode->m == 3) { THEKERNEL->conveyor->wait_for_empty_queue(); // M3: Spindle on spindle_on = true; if (gcode->has_letter('S')) { target_rpm = gcode->get_value('S'); } } else if (gcode->m == 5) { THEKERNEL->conveyor->wait_for_empty_queue(); spindle_on = false; } }else if(!gcode->has_m && !gcode->has_g && gcode->has_letter('S')) { // special case S on its own THEKERNEL->conveyor->wait_for_empty_queue(); target_rpm = gcode->get_value('S'); } }
// React to enable/disable gcodes void Stepper::on_gcode_execute(void *argument) { Gcode *gcode = static_cast<Gcode *>(argument); if( gcode->has_m) { if( gcode->m == 17 ) { this->turn_enable_pins_on(); } if( (gcode->m == 84 || gcode->m == 18) && !gcode->has_letter('E') ) { this->turn_enable_pins_off(); } } }
// Turn laser on/off depending on received GCodes void Laser::on_gcode_execute(void* argument){ Gcode* gcode = static_cast<Gcode*>(argument); this->laser_on = false; if( gcode->has_letter('G' )){ int code = gcode->get_value('G'); if( code == 0 ){ // G0 this->laser_pin = 0; this->laser_on = false; }else if( code >= 1 && code <= 3 ){ // G1, G2, G3 this->laser_on = true; } } }
void Stepper::on_gcode_received(void *argument) { Gcode *gcode = static_cast<Gcode *>(argument); if( gcode->has_m) { if( gcode->m == 17 ) { this->turn_enable_pins_on(); }else if( (gcode->m == 84 || gcode->m == 18) && !gcode->has_letter('E') ) { THEKERNEL->conveyor->wait_for_empty_queue(); this->turn_enable_pins_off(); } } }
// Turn laser on/off depending on received GCodes void Laser::on_gcode_execute(void* argument){ Gcode* gcode = static_cast<Gcode*>(argument); this->laser_on = false; if( gcode->has_g){ int code = gcode->g; if( code == 0 ){ // G0 this->laser_pin->write(this->laser_inverting ? 1 - this->laser_minimum_power : this->laser_minimum_power); this->laser_on = false; }else if( code >= 1 && code <= 3 ){ // G1, G2, G3 this->laser_on = true; } } if ( gcode->has_letter('S' )){ this->laser_power = gcode->get_value('S'); } }
void CurrentControl::on_gcode_received(void *argument) { Gcode *gcode = static_cast<Gcode*>(argument); char alpha[4] = { 'X', 'Y', 'Z', 'E' }; if (gcode->has_m) { if (gcode->m == 907) { int i; for (i = 0; i < 4; i++) { if (gcode->has_letter(alpha[i])) this->digipot->set_current(i, gcode->get_value(alpha[i])); gcode->stream->printf("%c:%3.1fA%c", alpha[i], this->digipot->get_current(i), (i == 3)?'\n':' '); } } } }
// Turn laser on/off depending on received GCodes void Laser::on_gcode_execute(void* argument){ Gcode* gcode = static_cast<Gcode*>(argument); this->laser_on = false; if( gcode->has_g){ int code = gcode->g; if( code == 0 ){ // G0 this->laser_pin->write(this->laser_tickle_power); this->laser_on = false; }else if( code >= 1 && code <= 3 ){ // G1, G2, G3 this->laser_on = true; } } if ( gcode->has_letter('S' )){ this->laser_max_power = gcode->get_value('S'); // this->kernel->streams->printf("Adjusted laser power to %d/100\r\n",(int)(this->laser_max_power*100.0+0.5)); } }
void Switch::on_gcode_received(void *argument) { Gcode *gcode = static_cast<Gcode *>(argument); // Add the gcode to the queue ourselves if we need it if (!(match_input_on_gcode(gcode) || match_input_off_gcode(gcode))) { return; } // drain queue THEKERNEL->conveyor->wait_for_empty_queue(); if(match_input_on_gcode(gcode)) { int v; if (this->output_type == PWM) { // PWM output pin turn on (or off if S0) if(gcode->has_letter('S')) { v = round(gcode->get_value('S') * output_pin.max_pwm() / 255.0); // scale by max_pwm so input of 255 and max_pwm of 128 would set value to 128 this->output_pin.pwm(v); this->switch_state= (v > 0); } else { this->output_pin.pwm(this->switch_value); this->switch_state= (this->switch_value > 0); } } else { // logic pin turn on this->output_pin.set(true); this->switch_state = true; } } else if(match_input_off_gcode(gcode)) { this->switch_state = false; if (this->output_type == PWM) { // PWM output pin this->output_pin.set(false); } else { // logic pin turn off this->output_pin.set(false); } } }
//A GCode has been received //See if the current Gcode line has some orders for us void Robot::on_gcode_received(void * argument){ Gcode* gcode = static_cast<Gcode*>(argument); //Temp variables, constant properties are stored in the object uint8_t next_action = NEXT_ACTION_DEFAULT; this->motion_mode = -1; //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly if( gcode->has_g){ switch( gcode->g ){ case 0: this->motion_mode = MOTION_MODE_SEEK; gcode->mark_as_taken(); break; case 1: this->motion_mode = MOTION_MODE_LINEAR; gcode->mark_as_taken(); break; case 2: this->motion_mode = MOTION_MODE_CW_ARC; gcode->mark_as_taken(); break; case 3: this->motion_mode = MOTION_MODE_CCW_ARC; gcode->mark_as_taken(); break; case 17: this->select_plane(X_AXIS, Y_AXIS, Z_AXIS); gcode->mark_as_taken(); break; case 18: this->select_plane(X_AXIS, Z_AXIS, Y_AXIS); gcode->mark_as_taken(); break; case 19: this->select_plane(Y_AXIS, Z_AXIS, X_AXIS); gcode->mark_as_taken(); break; case 20: this->inch_mode = true; gcode->mark_as_taken(); break; case 21: this->inch_mode = false; gcode->mark_as_taken(); break; case 90: this->absolute_mode = true; gcode->mark_as_taken(); break; case 91: this->absolute_mode = false; gcode->mark_as_taken(); break; case 92: { if(gcode->get_num_args() == 0){ clear_vector(this->last_milestone); }else{ for (char letter = 'X'; letter <= 'Z'; letter++){ if ( gcode->has_letter(letter) ) this->last_milestone[letter-'X'] = this->to_millimeters(gcode->get_value(letter)); } } memcpy(this->current_position, this->last_milestone, sizeof(double)*3); // current_position[] = last_milestone[]; this->arm_solution->millimeters_to_steps(this->current_position, this->kernel->planner->position); gcode->mark_as_taken(); return; // TODO: Wait until queue empty } } }else if( gcode->has_m){ switch( gcode->m ){ case 92: // M92 - set steps per mm double steps[3]; this->arm_solution->get_steps_per_millimeter(steps); if (gcode->has_letter('X')) steps[0] = this->to_millimeters(gcode->get_value('X')); if (gcode->has_letter('Y')) steps[1] = this->to_millimeters(gcode->get_value('Y')); if (gcode->has_letter('Z')) steps[2] = this->to_millimeters(gcode->get_value('Z')); if (gcode->has_letter('F')) seconds_per_minute = gcode->get_value('F'); this->arm_solution->set_steps_per_millimeter(steps); // update current position in steps this->arm_solution->millimeters_to_steps(this->current_position, this->kernel->planner->position); gcode->stream->printf("X:%g Y:%g Z:%g F:%g ", steps[0], steps[1], steps[2], seconds_per_minute); gcode->add_nl = true; gcode->mark_as_taken(); return; case 114: gcode->stream->printf("C: X:%1.3f Y:%1.3f Z:%1.3f ", from_millimeters(this->current_position[0]), from_millimeters(this->current_position[1]), from_millimeters(this->current_position[2])); gcode->add_nl = true; gcode->mark_as_taken(); return; // case 204: // M204 Snnn - set acceleration to nnn, NB only Snnn is currently supported // gcode->mark_as_taken(); // if (gcode->has_letter('S')) // { // double acc= gcode->get_value('S') * 60 * 60; // mm/min^2 // // enforce minimum // if (acc < 1.0) // acc = 1.0; // this->kernel->planner->acceleration= acc; // } // break; case 220: // M220 - speed override percentage gcode->mark_as_taken(); if (gcode->has_letter('S')) { double factor = gcode->get_value('S'); // enforce minimum 10% speed if (factor < 10.0) factor = 10.0; seconds_per_minute = factor * 0.6; } break; case 665: // M665 set optional arm solution variables based on arm solution gcode->mark_as_taken(); // the parameter args could be any letter so try each one for(char c='A';c<='Z';c++) { double v; bool supported= arm_solution->get_optional(c, &v); // retrieve current value if supported if(supported && gcode->has_letter(c)) { // set new value if supported v= gcode->get_value(c); arm_solution->set_optional(c, v); } if(supported) { // print all current values of supported options gcode->stream->printf("%c %8.3f ", c, v); gcode->add_nl = true; } } break; } } if( this->motion_mode < 0) return; //Get parameters double target[3], offset[3]; clear_vector(target); clear_vector(offset); memcpy(target, this->current_position, sizeof(target)); //default to last target for(char letter = 'I'; letter <= 'K'; letter++){ if( gcode->has_letter(letter) ){ offset[letter-'I'] = this->to_millimeters(gcode->get_value(letter)); } } for(char letter = 'X'; letter <= 'Z'; letter++){ if( gcode->has_letter(letter) ){ target[letter-'X'] = this->to_millimeters(gcode->get_value(letter)) + ( this->absolute_mode ? 0 : target[letter-'X']); } } if( gcode->has_letter('F') ) { if( this->motion_mode == MOTION_MODE_SEEK ) this->seek_rate = this->to_millimeters( gcode->get_value('F') ) / 60.0; else this->feed_rate = this->to_millimeters( gcode->get_value('F') ) / 60.0; } //Perform any physical actions switch( next_action ){ case NEXT_ACTION_DEFAULT: switch(this->motion_mode){ case MOTION_MODE_CANCEL: break; case MOTION_MODE_SEEK : this->append_line(gcode, target, this->seek_rate ); break; case MOTION_MODE_LINEAR: this->append_line(gcode, target, this->feed_rate ); break; case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: this->compute_arc(gcode, offset, target ); break; } break; } // As far as the parser is concerned, the position is now == target. In reality the // motion control system might still be processing the action and the real tool position // in any intermediate location. memcpy(this->current_position, target, sizeof(double)*3); // this->position[] = target[]; }
// Compute extrusion speed based on parameters and gcode distance of travel void Extruder::on_gcode_execute(void *argument) { Gcode *gcode = static_cast<Gcode *>(argument); // The mode is OFF by default, and SOLO or FOLLOW only if we need to extrude this->mode = OFF; // Absolute/relative mode, globally modal affect all extruders whether enabled or not if( gcode->has_m ) { switch(gcode->m) { case 17: this->en_pin.set(0); break; case 18: this->en_pin.set(1); break; case 82: this->absolute_mode = true; break; case 83: this->absolute_mode = false; break; case 84: this->en_pin.set(1); break; } return; } else if( gcode->has_g && (gcode->g == 90 || gcode->g == 91) ) { this->absolute_mode = (gcode->g == 90); return; } if( gcode->has_g && this->enabled ) { // G92: Reset extruder position if( gcode->g == 92 ) { if( gcode->has_letter('E') ) { this->current_position = gcode->get_value('E'); this->target_position = this->current_position; this->unstepped_distance = 0; } else if( gcode->get_num_args() == 0) { this->current_position = 0.0; this->target_position = this->current_position; this->unstepped_distance = 0; } } else if (gcode->g == 10) { // FW retract command feed_rate= retract_feedrate; // mm/sec this->mode = SOLO; this->travel_distance = -retract_length; this->target_position += this->travel_distance; this->en_pin.set(0); } else if (gcode->g == 11) { // un retract command feed_rate= retract_recover_feedrate; // mm/sec this->mode = SOLO; this->travel_distance = (retract_length + retract_recover_length); this->target_position += this->travel_distance; this->en_pin.set(0); } else if (gcode->g == 0 || gcode->g == 1) { // Extrusion length from 'G' Gcode if( gcode->has_letter('E' )) { // Get relative extrusion distance depending on mode ( in absolute mode we must substract target_position ) float extrusion_distance = gcode->get_value('E'); float relative_extrusion_distance = extrusion_distance; if (this->absolute_mode) { relative_extrusion_distance -= this->target_position; this->target_position = extrusion_distance; } else { this->target_position += relative_extrusion_distance; } // If the robot is moving, we follow it's movement, otherwise, we move alone if( fabs(gcode->millimeters_of_travel) < 0.0001F ) { // With floating numbers, we can have 0 != 0 ... beeeh. For more info see : http://upload.wikimedia.org/wikipedia/commons/0/0a/Cain_Henri_Vidal_Tuileries.jpg this->mode = SOLO; this->travel_distance = relative_extrusion_distance; } else { // We move proportionally to the robot's movement this->mode = FOLLOW; this->travel_ratio = (relative_extrusion_distance * this->volumetric_multiplier * this->extruder_multiplier) / gcode->millimeters_of_travel; // adjust for volumetric extrusion and extruder multiplier // TODO: check resulting flowrate, limit robot speed if it exceeds max_speed } this->en_pin.set(0); } if (gcode->has_letter('F')) { feed_rate = gcode->get_value('F') / THEKERNEL->robot->get_seconds_per_minute(); if (feed_rate > max_speed) feed_rate = max_speed; } } } }
void TemperatureControl::on_gcode_received(void *argument) { Gcode *gcode = static_cast<Gcode *>(argument); if (gcode->has_m) { if( gcode->m == this->get_m_code ) { char buf[32]; // should be big enough for any status int n = snprintf(buf, sizeof(buf), "%s:%3.1f /%3.1f @%d ", this->designator.c_str(), this->get_temperature(), ((target_temperature <= 0) ? 0.0 : target_temperature), this->o); gcode->txt_after_ok.append(buf, n); return; } if (gcode->m == 305) { // set or get sensor settings if (gcode->has_letter('S') && (gcode->get_value('S') == this->pool_index)) { TempSensor::sensor_options_t args= gcode->get_args(); args.erase('S'); // don't include the S if(args.size() > 0) { // set the new options if(sensor->set_optional(args)) { this->sensor_settings= true; }else{ gcode->stream->printf("Unable to properly set sensor settings, make sure you specify all required values\n"); } }else{ // don't override this->sensor_settings= false; } }else if(!gcode->has_letter('S')) { gcode->stream->printf("%s(S%d): using %s\n", this->designator.c_str(), this->pool_index, this->readonly?"Readonly" : this->use_bangbang?"Bangbang":"PID"); sensor->get_raw(); TempSensor::sensor_options_t options; if(sensor->get_optional(options)) { for(auto &i : options) { // foreach optional value gcode->stream->printf("%s(S%d): %c %1.18f\n", this->designator.c_str(), this->pool_index, i.first, i.second); } } } return; } // readonly sensors don't handle the rest if(this->readonly) return; if (gcode->m == 143) { if (gcode->has_letter('S') && (gcode->get_value('S') == this->pool_index)) { if(gcode->has_letter('P')) { max_temp= gcode->get_value('P'); } else { gcode->stream->printf("Nothing set NOTE Usage is M143 S0 P300 where <S> is the hotend index and <P> is the maximum temp to set\n"); } }else if(gcode->get_num_args() == 0) { gcode->stream->printf("Maximum temperature for %s(%d) is %f°C\n", this->designator.c_str(), this->pool_index, max_temp); } } else if (gcode->m == 301) { if (gcode->has_letter('S') && (gcode->get_value('S') == this->pool_index)) { if (gcode->has_letter('P')) setPIDp( gcode->get_value('P') ); if (gcode->has_letter('I')) setPIDi( gcode->get_value('I') ); if (gcode->has_letter('D')) setPIDd( gcode->get_value('D') ); if (gcode->has_letter('X')) this->i_max = gcode->get_value('X'); if (gcode->has_letter('Y')) this->heater_pin.max_pwm(gcode->get_value('Y')); }else if(!gcode->has_letter('S')) { gcode->stream->printf("%s(S%d): Pf:%g If:%g Df:%g X(I_max):%g max pwm: %d O:%d\n", this->designator.c_str(), this->pool_index, this->p_factor, this->i_factor / this->PIDdt, this->d_factor * this->PIDdt, this->i_max, this->heater_pin.max_pwm(), o); } } else if (gcode->m == 500 || gcode->m == 503) { // M500 saves some volatile settings to config override file, M503 just prints the settings gcode->stream->printf(";PID settings:\nM301 S%d P%1.4f I%1.4f D%1.4f X%1.4f Y%d\n", this->pool_index, this->p_factor, this->i_factor / this->PIDdt, this->d_factor * this->PIDdt, this->i_max, this->heater_pin.max_pwm()); gcode->stream->printf(";Max temperature setting:\nM143 S%d P%1.4f\n", this->pool_index, this->max_temp); if(this->sensor_settings) { // get or save any sensor specific optional values TempSensor::sensor_options_t options; if(sensor->get_optional(options) && !options.empty()) { gcode->stream->printf(";Optional temp sensor specific settings:\nM305 S%d", this->pool_index); for(auto &i : options) { gcode->stream->printf(" %c%1.18f", i.first, i.second); } gcode->stream->printf("\n"); } } } else if( ( gcode->m == this->set_m_code || gcode->m == this->set_and_wait_m_code ) && gcode->has_letter('S')) { // this only gets handled if it is not controlled by the tool manager or is active in the toolmanager this->active = true; // this is safe as old configs as well as single extruder configs the toolmanager will not be running so will return false // this will also ignore anything that the tool manager is not controlling and return false, otherwise it returns the active tool void *returned_data; bool ok = PublicData::get_value( tool_manager_checksum, is_active_tool_checksum, this->name_checksum, &returned_data ); if (ok) { uint16_t active_tool_name = *static_cast<uint16_t *>(returned_data); this->active = (active_tool_name == this->name_checksum); } if(this->active) { // required so temp change happens in order THEKERNEL->conveyor->wait_for_empty_queue(); float v = gcode->get_value('S'); if (v == 0.0) { this->target_temperature = UNDEFINED; this->heater_pin.set((this->o = 0)); } else { this->set_desired_temperature(v); // wait for temp to be reached, no more gcodes will be fetched until this is complete if( gcode->m == this->set_and_wait_m_code) { if(isinf(get_temperature()) && isinf(sensor->get_temperature())) { THEKERNEL->streams->printf("Temperature reading is unreliable on %s HALT asserted - reset or M999 required\n", designator.c_str()); THEKERNEL->call_event(ON_HALT, nullptr); return; } this->waiting = true; // on_second_tick will announce temps while ( get_temperature() < target_temperature ) { THEKERNEL->call_event(ON_IDLE, this); // check if ON_HALT was called (usually by kill button) if(THEKERNEL->is_halted() || this->target_temperature == UNDEFINED) { THEKERNEL->streams->printf("Wait on temperature aborted by kill\n"); break; } } this->waiting = false; } } } } } }
void Touchprobe::on_gcode_received(void* argument) { Gcode* gcode = static_cast<Gcode*>(argument); Robot* robot = THEKERNEL->robot; if( gcode->has_g) { if( gcode->g == 31 ) { float tmp[3], pos[3], mm[3]; int steps[3]; // first wait for an empty queue i.e. no moves left THEKERNEL->conveyor->wait_for_empty_queue(); robot->get_axis_position(pos); for(char c = 'X'; c <= 'Z'; c++){ if( gcode->has_letter(c) ){ tmp[c-'X'] = robot->to_millimeters(gcode->get_value(c)) - ( robot->absolute_mode ? pos[c-'X'] : 0 ); }else{ tmp[c-'X'] = 0; } } if( gcode->has_letter('F') ) { this->probe_rate = robot->to_millimeters( gcode->get_value('F') ) / robot->seconds_per_minute; } robot->arm_solution->cartesian_to_actuator(tmp, mm); for (int c = 0; c < 3; c++) steps[c] = mm[c] * robot->actuators[c]->steps_per_mm; if( ((abs(steps[0]) > 0 ? 1:0) + (abs(steps[1]) > 0 ? 1:0) + (abs(steps[2]) > 0 ? 1:0)) != 1 ){ return; //TODO coordinated movement not supported yet } // Enable the motors THEKERNEL->stepper->turn_enable_pins_on(); // move for(char c='X'; c<='Z'; c++){ tmp[c - 'X'] = robot->actuators[c - 'X']->steps_per_mm; if( steps[c-'X'] == 0 ){ continue; } bool dir = steps[c-'X'] < 0; // tmp is steps/mm, probe_rate in mm/s -> speed needs steps/s this->steppers[c-'X']->set_speed(this->probe_rate * robot->actuators[c]->steps_per_mm); this->steppers[c-'X']->move(dir,abs(steps[c-'X'])); } wait_for_touch(steps); // calculate new position for(char c='X'; c<='Z'; c++){ robot->reset_axis_position(pos[c-'X'] + mm[c-'X'], c-'X'); } if( this->should_log ){ robot->get_axis_position(pos); fprintf(logfile,"%1.3f %1.3f %1.3f\n", robot->from_millimeters(pos[0]), robot->from_millimeters(pos[1]), robot->from_millimeters(pos[2]) ); flush_log(); } } }else if(gcode->has_m) { // log rotation // for now this only writes a separator // TODO do a actual log rotation if( this->mcode != 0 && this->should_log && gcode->m == this->mcode){ string name; fputs("--\n",logfile); flush_log(); } } }
// Compute extrusion speed based on parameters and gcode distance of travel void Extruder::on_gcode_execute(void *argument) { Gcode *gcode = static_cast<Gcode *>(argument); // The mode is OFF by default, and SOLO or FOLLOW only if we need to extrude this->mode = OFF; // Absolute/relative mode, globably modal affect all extruders whether enabled or not if( gcode->has_m ) { switch(gcode->m) { case 17: this->en_pin.set(0); break; case 18: this->en_pin.set(1); break; case 82: this->absolute_mode = true; break; case 83: this->absolute_mode = false; break; case 84: this->en_pin.set(1); break; } return; } else if( gcode->has_g && (gcode->g == 90 || gcode->g == 91) ) { this->absolute_mode = (gcode->g == 90); return; } if( gcode->has_g && this->enabled ) { // G92: Reset extruder position if( gcode->g == 92 ) { if( gcode->has_letter('E') ) { this->current_position = gcode->get_value('E'); this->target_position = this->current_position; this->unstepped_distance = 0; } else if( gcode->get_num_args() == 0) { this->current_position = 0.0; this->target_position = this->current_position; this->unstepped_distance = 0; } } else if (gcode->g == 10) { // FW retract command feed_rate = retract_feedrate; // mm/sec this->mode = SOLO; this->travel_distance = -retract_length; this->target_position += this->travel_distance; this->en_pin.set(0); } else if (gcode->g == 11) { // un retract command feed_rate = retract_recover_feedrate; // mm/sec this->mode = SOLO; this->travel_distance = (retract_length + retract_recover_length); this->target_position += this->travel_distance; this->en_pin.set(0); } else if (gcode->g <= 3) { // Extrusion length from 'G' Gcode if( gcode->has_letter('E' )) { // Get relative extrusion distance depending on mode ( in absolute mode we must subtract target_position ) float extrusion_distance = gcode->get_value('E'); float relative_extrusion_distance = extrusion_distance; if (this->absolute_mode) { relative_extrusion_distance -= this->target_position; this->target_position = extrusion_distance; } else { this->target_position += relative_extrusion_distance; } // If the robot is moving, we follow it's movement, otherwise, we move alone if( fabsf(gcode->millimeters_of_travel) < 0.00001F ) { // With floating numbers, we can have 0 != 0, NOTE needs to be same as in Robot.cpp#745 this->mode = SOLO; this->travel_distance = relative_extrusion_distance; } else { // We move proportionally to the robot's movement this->mode = FOLLOW; this->travel_ratio = (relative_extrusion_distance * this->volumetric_multiplier * this->extruder_multiplier) / gcode->millimeters_of_travel; // adjust for volumetric extrusion and extruder multiplier } this->en_pin.set(0); } // NOTE this is only used in SOLO mode, but any F on a G0/G1 will set the speed for future retracts that are not firmware retracts if (gcode->has_letter('F')) { feed_rate = gcode->get_value('F') / THEKERNEL->robot->get_seconds_per_minute(); if (stepper_motor->get_max_rate() > 0 && feed_rate > stepper_motor->get_max_rate()) feed_rate = stepper_motor->get_max_rate(); } } } }
void Extruder::on_gcode_received(void *argument) { Gcode *gcode = static_cast<Gcode *>(argument); // M codes most execute immediately, most only execute if enabled if (gcode->has_m) { if (gcode->m == 114 && gcode->subcode == 0 && this->enabled) { char buf[16]; int n = snprintf(buf, sizeof(buf), " E:%1.3f ", this->current_position); gcode->txt_after_ok.append(buf, n); } else if (gcode->m == 92 && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier) ) ) { float spm = this->steps_per_millimeter; if (gcode->has_letter('E')) { spm = gcode->get_value('E'); this->steps_per_millimeter = spm; } gcode->stream->printf("E:%g ", spm); gcode->add_nl = true; } else if (gcode->m == 200 && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier)) ) { if (gcode->has_letter('D')) { THEKERNEL->conveyor->wait_for_empty_queue(); // only apply after the queue has emptied this->filament_diameter = gcode->get_value('D'); if(filament_diameter > 0.01F) { this->volumetric_multiplier = 1.0F / (powf(this->filament_diameter / 2, 2) * PI); } else { this->volumetric_multiplier = 1.0F; } } else { if(filament_diameter > 0.01F) { gcode->stream->printf("Filament Diameter: %f\n", this->filament_diameter); } else { gcode->stream->printf("Volumetric extrusion is disabled\n"); } } } else if (gcode->m == 203 && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier)) ) { // M203 Exxx Vyyy Set maximum feedrates xxx mm/sec and/or yyy mm³/sec if(gcode->get_num_args() == 0) { gcode->stream->printf("E:%g V:%g", this->stepper_motor->get_max_rate(), this->max_volumetric_rate); gcode->add_nl = true; } else { if(gcode->has_letter('E')) { this->stepper_motor->set_max_rate(gcode->get_value('E')); } if(gcode->has_letter('V')) { this->max_volumetric_rate = gcode->get_value('V'); } } } else if (gcode->m == 204 && gcode->has_letter('E') && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier)) ) { // extruder acceleration M204 Ennn mm/sec^2 (Pnnn sets the specific extruder for M500) this->acceleration = gcode->get_value('E'); } else if (gcode->m == 207 && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier)) ) { // M207 - set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop] Q[zlift feedrate mm/min] if(gcode->has_letter('S')) retract_length = gcode->get_value('S'); if(gcode->has_letter('F')) retract_feedrate = gcode->get_value('F') / 60.0F; // specified in mm/min converted to mm/sec if(gcode->has_letter('Z')) retract_zlift_length = gcode->get_value('Z'); if(gcode->has_letter('Q')) retract_zlift_feedrate = gcode->get_value('Q'); } else if (gcode->m == 208 && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier)) ) { // M208 - set retract recover length S[positive mm surplus to the M207 S*] F[feedrate mm/min] if(gcode->has_letter('S')) retract_recover_length = gcode->get_value('S'); if(gcode->has_letter('F')) retract_recover_feedrate = gcode->get_value('F') / 60.0F; // specified in mm/min converted to mm/sec } else if (gcode->m == 221 && this->enabled) { // M221 S100 change flow rate by percentage if(gcode->has_letter('S')) { this->extruder_multiplier = gcode->get_value('S') / 100.0F; } else { gcode->stream->printf("Flow rate at %6.2f %%\n", this->extruder_multiplier * 100.0F); } } else if (gcode->m == 500 || gcode->m == 503) { // M500 saves some volatile settings to config override file, M503 just prints the settings if( this->single_config ) { gcode->stream->printf(";E Steps per mm:\nM92 E%1.4f\n", this->steps_per_millimeter); gcode->stream->printf(";E Filament diameter:\nM200 D%1.4f\n", this->filament_diameter); gcode->stream->printf(";E retract length, feedrate, zlift length, feedrate:\nM207 S%1.4f F%1.4f Z%1.4f Q%1.4f\n", this->retract_length, this->retract_feedrate * 60.0F, this->retract_zlift_length, this->retract_zlift_feedrate); gcode->stream->printf(";E retract recover length, feedrate:\nM208 S%1.4f F%1.4f\n", this->retract_recover_length, this->retract_recover_feedrate * 60.0F); gcode->stream->printf(";E acceleration mm/sec²:\nM204 E%1.4f\n", this->acceleration); gcode->stream->printf(";E max feed rate mm/sec:\nM203 E%1.4f\n", this->stepper_motor->get_max_rate()); if(this->max_volumetric_rate > 0) { gcode->stream->printf(";E max volumetric rate mm³/sec:\nM203 V%1.4f\n", this->max_volumetric_rate); } } else { gcode->stream->printf(";E Steps per mm:\nM92 E%1.4f P%d\n", this->steps_per_millimeter, this->identifier); gcode->stream->printf(";E Filament diameter:\nM200 D%1.4f P%d\n", this->filament_diameter, this->identifier); gcode->stream->printf(";E retract length, feedrate:\nM207 S%1.4f F%1.4f Z%1.4f Q%1.4f P%d\n", this->retract_length, this->retract_feedrate * 60.0F, this->retract_zlift_length, this->retract_zlift_feedrate, this->identifier); gcode->stream->printf(";E retract recover length, feedrate:\nM208 S%1.4f F%1.4f P%d\n", this->retract_recover_length, this->retract_recover_feedrate * 60.0F, this->identifier); gcode->stream->printf(";E acceleration mm/sec²:\nM204 E%1.4f P%d\n", this->acceleration, this->identifier); gcode->stream->printf(";E max feed rate mm/sec:\nM203 E%1.4f P%d\n", this->stepper_motor->get_max_rate(), this->identifier); if(this->max_volumetric_rate > 0) { gcode->stream->printf(";E max volumetric rate mm³/sec:\nM203 V%1.4f P%d\n", this->max_volumetric_rate, this->identifier); } } } else if( gcode->m == 17 || gcode->m == 18 || gcode->m == 82 || gcode->m == 83 || gcode->m == 84 ) { // Mcodes to pass along to on_gcode_execute THEKERNEL->conveyor->append_gcode(gcode); } } else if(gcode->has_g) { // G codes, NOTE some are ignored if not enabled if( (gcode->g == 92 && gcode->has_letter('E')) || (gcode->g == 90 || gcode->g == 91) ) { // Gcodes to pass along to on_gcode_execute THEKERNEL->conveyor->append_gcode(gcode); } else if( this->enabled && gcode->g < 4 && gcode->has_letter('E') && fabsf(gcode->millimeters_of_travel) < 0.00001F ) { // With floating numbers, we can have 0 != 0, NOTE needs to be same as in Robot.cpp#745 // NOTE was ... gcode->has_letter('E') && !gcode->has_letter('X') && !gcode->has_letter('Y') && !gcode->has_letter('Z') ) { // This is a SOLO move, we add an empty block to the queue to prevent subsequent gcodes being executed at the same time THEKERNEL->conveyor->append_gcode(gcode); THEKERNEL->conveyor->queue_head_block(); } else if( this->enabled && (gcode->g == 10 || gcode->g == 11) && !gcode->has_letter('L') ) { // firmware retract command (Ignore if has L parameter that is not for us) // check we are in the correct state of retract or unretract if(gcode->g == 10 && !retracted) { this->retracted = true; this->cancel_zlift_restore = false; } else if(gcode->g == 11 && retracted) { this->retracted = false; } else return; // ignore duplicates // now we do a special hack to add zlift if needed, this should go in Robot but if it did the zlift would be executed before retract which is bad // this way zlift will happen after retract, (or before for unretract) NOTE we call the robot->on_gcode_receive directly to avoid recursion if(retract_zlift_length > 0 && gcode->g == 11 && !this->cancel_zlift_restore) { // reverse zlift happens before unretract // NOTE we do not do this if cancel_zlift_restore is set to true, which happens if there is an absolute Z move inbetween G10 and G11 char buf[32]; int n = snprintf(buf, sizeof(buf), "G0 Z%1.4f F%1.4f", -retract_zlift_length, retract_zlift_feedrate); string cmd(buf, n); Gcode gc(cmd, &(StreamOutput::NullStream)); bool oldmode = THEKERNEL->robot->absolute_mode; THEKERNEL->robot->absolute_mode = false; // needs to be relative mode THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly THEKERNEL->robot->absolute_mode = oldmode; // restore mode } // This is a solo move, we add an empty block to the queue to prevent subsequent gcodes being executed at the same time THEKERNEL->conveyor->append_gcode(gcode); THEKERNEL->conveyor->queue_head_block(); if(retract_zlift_length > 0 && gcode->g == 10) { char buf[32]; int n = snprintf(buf, sizeof(buf), "G0 Z%1.4f F%1.4f", retract_zlift_length, retract_zlift_feedrate); string cmd(buf, n); Gcode gc(cmd, &(StreamOutput::NullStream)); bool oldmode = THEKERNEL->robot->absolute_mode; THEKERNEL->robot->absolute_mode = false; // needs to be relative mode THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly THEKERNEL->robot->absolute_mode = oldmode; // restore mode } } else if( this->enabled && this->retracted && (gcode->g == 0 || gcode->g == 1) && gcode->has_letter('Z')) { // NOTE we cancel the zlift restore for the following G11 as we have moved to an absolute Z which we need to stay at this->cancel_zlift_restore = true; } } // handle some codes now for the volumetric rate limiting // G90 G91 G92 M82 M83 if(gcode->has_m) { switch(gcode->m) { case 82: this->milestone_absolute_mode = true; break; case 83: this->milestone_absolute_mode = false; break; } } else if(gcode->has_g) { switch(gcode->g) { case 90: this->milestone_absolute_mode = true; break; case 91: this->milestone_absolute_mode = false; break; case 92: if(this->enabled) { if(gcode->has_letter('E')) { this->milestone_last_position = gcode->get_value('E'); } else if(gcode->get_num_args() == 0) { this->milestone_last_position = 0; } } break; } } }
//A GCode has been received //See if the current Gcode line has some orders for us void Robot::on_gcode_received(void *argument) { Gcode *gcode = static_cast<Gcode *>(argument); this->motion_mode = -1; //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly if( gcode->has_g) { switch( gcode->g ) { case 0: this->motion_mode = MOTION_MODE_SEEK; gcode->mark_as_taken(); break; case 1: this->motion_mode = MOTION_MODE_LINEAR; gcode->mark_as_taken(); break; case 2: this->motion_mode = MOTION_MODE_CW_ARC; gcode->mark_as_taken(); break; case 3: this->motion_mode = MOTION_MODE_CCW_ARC; gcode->mark_as_taken(); break; case 4: { uint32_t delay_ms= 0; if (gcode->has_letter('P')) { delay_ms= gcode->get_int('P'); } if (gcode->has_letter('S')) { delay_ms += gcode->get_int('S') * 1000; } if (delay_ms > 0){ // drain queue THEKERNEL->conveyor->wait_for_empty_queue(); // wait for specified time uint32_t start= us_ticker_read(); // mbed call while ((us_ticker_read() - start) < delay_ms*1000) { THEKERNEL->call_event(ON_IDLE, this); } } gcode->mark_as_taken(); } break; case 17: this->select_plane(X_AXIS, Y_AXIS, Z_AXIS); gcode->mark_as_taken(); break; case 18: this->select_plane(X_AXIS, Z_AXIS, Y_AXIS); gcode->mark_as_taken(); break; case 19: this->select_plane(Y_AXIS, Z_AXIS, X_AXIS); gcode->mark_as_taken(); break; case 20: this->inch_mode = true; gcode->mark_as_taken(); break; case 21: this->inch_mode = false; gcode->mark_as_taken(); break; case 90: this->absolute_mode = true; gcode->mark_as_taken(); break; case 91: this->absolute_mode = false; gcode->mark_as_taken(); break; case 92: { if(gcode->get_num_args() == 0) { for (int i = X_AXIS; i <= Z_AXIS; ++i) { reset_axis_position(0, i); } } else { for (char letter = 'X'; letter <= 'Z'; letter++) { if ( gcode->has_letter(letter) ) { reset_axis_position(this->to_millimeters(gcode->get_value(letter)), letter - 'X'); } } } gcode->mark_as_taken(); return; } } } else if( gcode->has_m) { switch( gcode->m ) { case 0: // M0 - Pause until pause button pressed again THEKERNEL->pauser->take(); return; case 92: // M92 - set steps per mm if (gcode->has_letter('X')) actuators[0]->change_steps_per_mm(this->to_millimeters(gcode->get_value('X'))); if (gcode->has_letter('Y')) actuators[1]->change_steps_per_mm(this->to_millimeters(gcode->get_value('Y'))); if (gcode->has_letter('Z')) actuators[2]->change_steps_per_mm(this->to_millimeters(gcode->get_value('Z'))); if (gcode->has_letter('F')) seconds_per_minute = gcode->get_value('F'); gcode->stream->printf("X:%g Y:%g Z:%g F:%g ", actuators[0]->steps_per_mm, actuators[1]->steps_per_mm, actuators[2]->steps_per_mm, seconds_per_minute); gcode->add_nl = true; gcode->mark_as_taken(); check_max_actuator_speeds(); return; case 114: { char buf[64]; int n = snprintf(buf, sizeof(buf), "C: X:%1.3f Y:%1.3f Z:%1.3f A:%1.3f B:%1.3f C:%1.3f ", from_millimeters(this->last_milestone[0]), from_millimeters(this->last_milestone[1]), from_millimeters(this->last_milestone[2]), actuators[X_AXIS]->get_current_position(), actuators[Y_AXIS]->get_current_position(), actuators[Z_AXIS]->get_current_position() ); gcode->txt_after_ok.append(buf, n); gcode->mark_as_taken(); } return; case 120: { // push state gcode->mark_as_taken(); bool b= this->absolute_mode; saved_state_t s(this->feed_rate, this->seek_rate, b); state_stack.push(s); } break; case 121: // pop state gcode->mark_as_taken(); 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); } break; case 203: // M203 Set maximum feedrates in mm/sec if (gcode->has_letter('X')) this->max_speeds[X_AXIS] = gcode->get_value('X'); if (gcode->has_letter('Y')) this->max_speeds[Y_AXIS] = gcode->get_value('Y'); if (gcode->has_letter('Z')) this->max_speeds[Z_AXIS] = gcode->get_value('Z'); if (gcode->has_letter('A')) alpha_stepper_motor->set_max_rate(gcode->get_value('A')); if (gcode->has_letter('B')) beta_stepper_motor->set_max_rate(gcode->get_value('B')); if (gcode->has_letter('C')) gamma_stepper_motor->set_max_rate(gcode->get_value('C')); check_max_actuator_speeds(); gcode->stream->printf("X:%g Y:%g Z:%g A:%g B:%g C:%g ", this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS], alpha_stepper_motor->get_max_rate(), beta_stepper_motor->get_max_rate(), gamma_stepper_motor->get_max_rate()); gcode->add_nl = true; gcode->mark_as_taken(); break; case 204: // M204 Snnn - set acceleration to nnn, Znnn sets z acceleration gcode->mark_as_taken(); if (gcode->has_letter('S')) { float acc = gcode->get_value('S'); // mm/s^2 // enforce minimum if (acc < 1.0F) acc = 1.0F; THEKERNEL->planner->acceleration = acc; } if (gcode->has_letter('Z')) { float acc = gcode->get_value('Z'); // mm/s^2 // enforce positive if (acc < 0.0F) acc = 0.0F; THEKERNEL->planner->z_acceleration = acc; } break; case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed, Ynnn - set minimum step rate gcode->mark_as_taken(); if (gcode->has_letter('X')) { float jd = gcode->get_value('X'); // enforce minimum if (jd < 0.0F) jd = 0.0F; THEKERNEL->planner->junction_deviation = jd; } if (gcode->has_letter('Z')) { float jd = gcode->get_value('Z'); // enforce minimum, -1 disables it and uses regular junction deviation if (jd < -1.0F) jd = -1.0F; THEKERNEL->planner->z_junction_deviation = jd; } if (gcode->has_letter('S')) { float mps = gcode->get_value('S'); // enforce minimum if (mps < 0.0F) mps = 0.0F; THEKERNEL->planner->minimum_planner_speed = mps; } if (gcode->has_letter('Y')) { alpha_stepper_motor->default_minimum_actuator_rate = gcode->get_value('Y'); } break; case 220: // M220 - speed override percentage gcode->mark_as_taken(); if (gcode->has_letter('S')) { float factor = gcode->get_value('S'); // enforce minimum 10% speed if (factor < 10.0F) factor = 10.0F; // enforce maximum 10x speed if (factor > 1000.0F) factor = 1000.0F; seconds_per_minute = 6000.0F / factor; } break; case 400: // wait until all moves are done up to this point gcode->mark_as_taken(); THEKERNEL->conveyor->wait_for_empty_queue(); break; case 500: // M500 saves some volatile settings to config override file case 503: { // M503 just prints the settings gcode->stream->printf(";Steps per unit:\nM92 X%1.5f Y%1.5f Z%1.5f\n", actuators[0]->steps_per_mm, actuators[1]->steps_per_mm, actuators[2]->steps_per_mm); gcode->stream->printf(";Acceleration mm/sec^2:\nM204 S%1.5f Z%1.5f\n", THEKERNEL->planner->acceleration, THEKERNEL->planner->z_acceleration); gcode->stream->printf(";X- Junction Deviation, Z- Z junction deviation, S - Minimum Planner speed mm/sec:\nM205 X%1.5f Z%1.5f S%1.5f\n", THEKERNEL->planner->junction_deviation, THEKERNEL->planner->z_junction_deviation, THEKERNEL->planner->minimum_planner_speed); gcode->stream->printf(";Max feedrates in mm/sec, XYZ cartesian, ABC actuator:\nM203 X%1.5f Y%1.5f Z%1.5f A%1.5f B%1.5f C%1.5f\n", this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS], alpha_stepper_motor->get_max_rate(), beta_stepper_motor->get_max_rate(), gamma_stepper_motor->get_max_rate()); // get or save any arm solution specific optional values BaseSolution::arm_options_t options; if(arm_solution->get_optional(options) && !options.empty()) { gcode->stream->printf(";Optional arm solution specific settings:\nM665"); for(auto &i : options) { gcode->stream->printf(" %c%1.4f", i.first, i.second); } gcode->stream->printf("\n"); } gcode->mark_as_taken(); break; } case 665: { // M665 set optional arm solution variables based on arm solution. gcode->mark_as_taken(); // the parameter args could be any letter each arm solution only accepts certain ones BaseSolution::arm_options_t options= gcode->get_args(); options.erase('S'); // don't include the S options.erase('U'); // don't include the U if(options.size() > 0) { // set the specified options arm_solution->set_optional(options); } options.clear(); if(arm_solution->get_optional(options)) { // foreach optional value for(auto &i : options) { // print all current values of supported options gcode->stream->printf("%c: %8.4f ", i.first, i.second); gcode->add_nl = true; } } if(gcode->has_letter('S')) { // set delta segments per second, not saved by M500 this->delta_segments_per_second = gcode->get_value('S'); gcode->stream->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second); }else if(gcode->has_letter('U')) { // or set mm_per_line_segment, not saved by M500 this->mm_per_line_segment = gcode->get_value('U'); this->delta_segments_per_second = 0; gcode->stream->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment); } break; } } } if( this->motion_mode < 0) return; //Get parameters float target[3], offset[3]; clear_vector(offset); memcpy(target, this->last_milestone, sizeof(target)); //default to last target for(char letter = 'I'; letter <= 'K'; letter++) { if( gcode->has_letter(letter) ) { offset[letter - 'I'] = this->to_millimeters(gcode->get_value(letter)); } } for(char letter = 'X'; letter <= 'Z'; letter++) { if( gcode->has_letter(letter) ) { target[letter - 'X'] = this->to_millimeters(gcode->get_value(letter)) + (this->absolute_mode ? this->toolOffset[letter - 'X'] : target[letter - 'X']); } } if( gcode->has_letter('F') ) { if( this->motion_mode == MOTION_MODE_SEEK ) this->seek_rate = this->to_millimeters( gcode->get_value('F') ); else this->feed_rate = this->to_millimeters( gcode->get_value('F') ); } //Perform any physical actions switch(this->motion_mode) { case MOTION_MODE_CANCEL: break; case MOTION_MODE_SEEK : this->append_line(gcode, target, this->seek_rate / seconds_per_minute ); break; case MOTION_MODE_LINEAR: this->append_line(gcode, target, this->feed_rate / seconds_per_minute ); break; case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: this->compute_arc(gcode, offset, target ); break; } // last_milestone was set to target in append_milestone, no need to do it again }
void RotaryDeltaCalibration::on_gcode_received(void *argument) { Gcode *gcode = static_cast<Gcode *>(argument); if( gcode->has_m) { switch( gcode->m ) { case 206: { float *theta_offset= get_homing_offset(); // points to theta offset in Endstop module if (theta_offset == nullptr) { gcode->stream->printf("error:no endstop module found\n"); return; } // set theta offset, set directly in the Endstop module (bad practice really) if (gcode->has_letter('A')) theta_offset[0] = gcode->get_value('A'); if (gcode->has_letter('B')) theta_offset[1] = gcode->get_value('B'); if (gcode->has_letter('C')) theta_offset[2] = gcode->get_value('C'); gcode->stream->printf("Theta offset set: A %8.5f B %8.5f C %8.5f\n", theta_offset[0], theta_offset[1], theta_offset[2]); break; } case 306: { // for a rotary delta M306 calibrates the homing angle // by doing M306 A-56.17 it will calculate the M206 A value (the theta offset for actuator A) based on the difference // between what it thinks is the current angle and what the current angle actually is specified by A (ditto for B and C) ActuatorCoordinates current_angle; // get the current angle for each actuator, NOTE we only deal with ABC so if there are more than 3 actuators this will probably go wonky for (size_t i = 0; i < THEKERNEL->robot->actuators.size(); i++) { current_angle[i]= THEKERNEL->robot->actuators[i]->get_current_position(); } if (gcode->has_letter('L') && gcode->get_value('L') != 0) { // specifying L1 it will take the last probe position (set by G30 or G38.x ) and set the home offset based on that // this allows the use of G30 to find the angle tool uint8_t ok; std::tie(current_angle[0], current_angle[1], current_angle[2], ok) = THEKERNEL->robot->get_last_probe_position(); if(ok == 0) { gcode->stream->printf("error:Nothing set as probe failed or not run\n"); return; } } float *theta_offset= get_homing_offset(); // points to theta offset in Endstop module if (theta_offset == nullptr) { gcode->stream->printf("error:no endstop module found\n"); return; } int cnt= 0; //figure out what home_offset needs to be to correct the homing_position if (gcode->has_letter('A')) { float a = gcode->get_value('A'); // what actual angle is theta_offset[0] -= (current_angle[0] - a); current_angle[0]= a; cnt++; } if (gcode->has_letter('B')) { float b = gcode->get_value('B'); theta_offset[1] -= (current_angle[1] - b); current_angle[1]= b; cnt++; } if (gcode->has_letter('C')) { float c = gcode->get_value('C'); theta_offset[2] -= (current_angle[2] - c); current_angle[2]= c; cnt++; } // reset the actuator positions (and machine position accordingly) // But only if all three actuators have been specified at the same time if(cnt == 3 || (gcode->has_letter('R') && gcode->get_value('R') != 0)) { THEKERNEL->robot->reset_actuator_position(current_angle); gcode->stream->printf("NOTE: actuator position reset\n"); }else{ gcode->stream->printf("NOTE: actuator position NOT reset\n"); } gcode->stream->printf("Theta Offset: A %8.5f B %8.5f C %8.5f\n", theta_offset[0], theta_offset[1], theta_offset[2]); } } } }
//A GCode has been received //See if the current Gcode line has some orders for us void SCARAcal::on_gcode_received(void *argument) { Gcode *gcode = static_cast<Gcode *>(argument); if( gcode->has_m) { switch( gcode->m ) { case 114: { // Extra stuff for Morgan calibration char buf[32]; float cartesian[6], actuators[6]; THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position int n = snprintf(buf, sizeof(buf), " A: Th:%1.3f Ps:%1.3f", actuators[0], actuators[1]); // display actuator angles Theta and Psi. gcode->txt_after_ok.append(buf, n); gcode->mark_as_taken(); } return; case 360: { float target[2] = {0.0F, 120.0F}, S_trim[3]; this->get_trim(S_trim[0], S_trim[1], S_trim[2]); // get current trim to conserve other calbration values if(gcode->has_letter('P')) { // Program the current position as target float cartesian[6], actuators[6], S_delta[2], S_trim[3]; THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position S_delta[0] = actuators[0] - target[0]; set_trim(S_delta[0], S_trim[1], 0, gcode->stream); } else { set_trim(0, S_trim[1], 0, gcode->stream); // reset trim for calibration move this->home(); // home SCARA_ang_move(target[0], target[1], 100.0F, slow_rate * 3.0F); // move to target } gcode->mark_as_taken(); } return; case 361: { float target[2] = {90.0F, 130.0F}; if(gcode->has_letter('P')) { // Program the current position as target float cartesian[6], actuators[6]; THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position STEPPER[0]->change_steps_per_mm(actuators[0] / target[0] * STEPPER[0]->get_steps_per_mm()); // Find angle difference STEPPER[1]->change_steps_per_mm(STEPPER[0]->get_steps_per_mm()); // and change steps_per_mm to ensure correct steps per *angle* } else { this->home(); // home - This time leave trims as adjusted. SCARA_ang_move(target[0], target[1], 100.0F, slow_rate * 3.0F); // move to target } gcode->mark_as_taken(); } return; case 364: { float target[2] = {45.0F, 135.0F}, S_trim[3]; this->get_trim(S_trim[0], S_trim[1], S_trim[2]); // get current trim to conserve other calbration values if(gcode->has_letter('P')) { // Program the current position as target float cartesian[6], actuators[6], S_delta[2]; THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate it to get actual actuator angles S_delta[1] = actuators[1] - target[1]; // Find difference, and set_trim(S_trim[0], S_delta[1], 0, gcode->stream); // set trim to reflect the difference } else { set_trim(S_trim[0], 0, 0, gcode->stream); // reset trim for calibration move this->home(); // home SCARA_ang_move(target[0], target[1], 100.0F, slow_rate * 3.0F); // move to target } gcode->mark_as_taken(); } return; } } }