C++ Gcode::mark_as_taken方法代码示例

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 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

    }
}

void SimpleShell::on_gcode_received(void *argument)
{
    Gcode *gcode = static_cast<Gcode *>(argument);
    string args= get_arguments(gcode->command);

    if (gcode->has_m) {
        if (gcode->m == 20) { // list sd card
            gcode->mark_as_taken();
            gcode->stream->printf("Begin file list\r\n");
            ls_command("/sd", gcode->stream);
            gcode->stream->printf("End file list\r\n");
        }
        else if (gcode->m == 30) { // remove file
            gcode->mark_as_taken();
            rm_command("/sd/" + args, gcode->stream);
        }
    }
}

void Panel::on_gcode_received(void *argument)
{
    Gcode *gcode = static_cast<Gcode *>(argument);
    if ( gcode->has_m) {
        if ( gcode->m == 117 ) { // set LCD message
            this->message = get_arguments(gcode->get_command());
            if (this->message.size() > 20) this->message = this->message.substr(0, 20);
            gcode->mark_as_taken();
        }
    }
}

void SimpleShell::on_gcode_received(void *argument) {
    Gcode *gcode = static_cast<Gcode*>(argument);

    if (gcode->has_m) {
        if (gcode->m == 20) { // list sd card
            gcode->mark_as_taken();
            gcode->stream->printf("Begin file list\r\n");
            ls_command("/sd", gcode->stream);
            gcode->stream->printf("End file list\r\n");
        }
    }
}

//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);
//.........这里部分代码省略.........

//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;
        }
    }    
}

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 && this->enabled) {
            char buf[16];
            int n = snprintf(buf, sizeof(buf), " E:%1.3f ", this->current_position);
            gcode->txt_after_ok.append(buf, n);
            gcode->mark_as_taken();

        } 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;
            gcode->mark_as_taken();

        } 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.01) {
                    this->volumetric_multiplier = 1.0F / (powf(this->filament_diameter / 2, 2) * PI);
                } else {
                    this->volumetric_multiplier = 1.0F;
                }
            }
            gcode->mark_as_taken();

        } 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');
            gcode->mark_as_taken();

        } 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');
            gcode->mark_as_taken();

        } 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
            gcode->mark_as_taken();

        } 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;
            gcode->mark_as_taken();

        } 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^2:\nM204 E%1.4f\n", this->acceleration);

            } 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^2:\nM204 E%1.4f P%d\n", this->acceleration, this->identifier);
            }
            gcode->mark_as_taken();
        } 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);
            gcode->mark_as_taken();
        }

    } 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);
            gcode->mark_as_taken();

        } else if( this->enabled && gcode->g < 4 && 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();
            gcode->mark_as_taken();

        } else if( this->enabled && (gcode->g == 10 || gcode->g == 11) ) { // firmware retract command
            gcode->mark_as_taken();
            // check we are in the correct state of retract or unretract
            if(gcode->g == 10 && !retracted) {
                this->retracted= true;
                this->cancel_zlift_restore= false;
//.........这里部分代码省略.........

// 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 == 17 ){ this->en_pin.set(0); }
        if( gcode->m == 18 ){ this->en_pin.set(1); }
        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); }
        if (gcode->m == 92 ){
            if (gcode->has_letter('E')){
                this->steps_per_millimeter = gcode->get_value('E');
            }
        }
    }

    // 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 ){
            gcode->mark_as_taken();
            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 == 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 )
                double extrusion_distance = gcode->get_value('E');
                double 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.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;
                }else{
                    // We move proportionally to the robot's movement
                    this->mode = FOLLOW;
                    this->travel_ratio = relative_extrusion_distance / gcode->millimeters_of_travel;
                    // TODO: check resulting flowrate, limit robot speed if it exceeds max_speed
                }

                this->en_pin.set(0);
            }
            if (gcode->has_letter('F'))
            {
                this->feed_rate = gcode->get_value('F');
                if (this->feed_rate > (this->max_speed * kernel->robot->seconds_per_minute))
                    this->feed_rate = this->max_speed * kernel->robot->seconds_per_minute;
                feed_rate /= kernel->robot->seconds_per_minute;
            }
        }else if( gcode->g == 90 ){ this->absolute_mode = true;
        }else if( gcode->g == 91 ){ this->absolute_mode = false;
        }
    }
}

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 == UNDEFINED) ? 0.0 : target_temperature), this->o);
            gcode->txt_after_ok.append(buf, n);
            gcode->mark_as_taken();
            return;
        }

        if (gcode->m == 305) { // set or get sensor settings
            gcode->mark_as_taken();
            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 == 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');
                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());

            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");
                }
            }
            gcode->mark_as_taken();

        } else if( ( gcode->m == this->set_m_code || gcode->m == this->set_and_wait_m_code ) && gcode->has_letter('S')) {
            gcode->mark_as_taken();
            // 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
            /* TOADDBACK 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();
//.........这里部分代码省略.........

void Player::on_gcode_received(void *argument) {
    Gcode *gcode = static_cast<Gcode*>(argument);
    string args= get_arguments(gcode->command);
    if (gcode->has_m) {
        if (gcode->m == 21) { // Dummy code; makes Octoprint happy -- supposed to initialize SD card
            gcode->mark_as_taken();
            gcode->stream->printf("SD card ok\r\n");

        }else if (gcode->m == 23) { // select file
            gcode->mark_as_taken();
            // Get filename
            this->filename= "/sd/" + this->absolute_from_relative(shift_parameter( args ));
            this->current_stream = &(StreamOutput::NullStream);

            if(this->current_file_handler != NULL) {
                this->playing_file = false;
                fclose(this->current_file_handler);
            }
            this->current_file_handler = fopen( this->filename.c_str(), "r");
            // get size of file
            int result = fseek(this->current_file_handler, 0, SEEK_END);
            if (0 != result){
                    gcode->stream->printf("WARNING - Could not get file size\r\n");
                    file_size= -1;
            }else{
                    file_size= ftell(this->current_file_handler);
                    fseek(this->current_file_handler, 0, SEEK_SET);
            }

            if(this->current_file_handler == NULL){
                gcode->stream->printf("file.open failed: %s\r\n", this->filename.c_str());
            }else{
                gcode->stream->printf("File opened:%s Size:%ld\r\n", this->filename.c_str(),file_size);
                gcode->stream->printf("File selected\r\n");
            }

            this->played_cnt= 0;
            this->elapsed_secs= 0;

        }else if (gcode->m == 24) { // start print
            gcode->mark_as_taken();
            if (this->current_file_handler != NULL) {
                this->playing_file = true;
                this->reply_stream= gcode->stream;
            }

        }else if (gcode->m == 25) { // pause print
            gcode->mark_as_taken();
            this->playing_file = false;

        }else if (gcode->m == 26) { // Reset print. Slightly different than M26 in Marlin and the rest
            gcode->mark_as_taken();
            if(this->current_file_handler != NULL){
                // abort the print
                abort_command("", gcode->stream);

                // reload the last file opened
                this->current_file_handler = fopen( this->filename.c_str(), "r");

                if(this->current_file_handler == NULL){
                    gcode->stream->printf("file.open failed: %s\r\n", this->filename.c_str());
                }else{
                    // get size of file
                    int result = fseek(this->current_file_handler, 0, SEEK_END);
                    if (0 != result){
                            gcode->stream->printf("WARNING - Could not get file size\r\n");
                            file_size= 0;
                    }else{
                            file_size= ftell(this->current_file_handler);
                            fseek(this->current_file_handler, 0, SEEK_SET);
                    }
                }
            }else{
                gcode->stream->printf("No file loaded\r\n");
            }

        }else if (gcode->m == 27) { // report print progress, in format used by Marlin
            gcode->mark_as_taken();
            progress_command("-b", gcode->stream);

        }else if (gcode->m == 32) { // select file and start print
            gcode->mark_as_taken();
            // Get filename
            this->filename= "/sd/" + this->absolute_from_relative(shift_parameter( args ));
            this->current_stream = &(StreamOutput::NullStream);

            if(this->current_file_handler != NULL) {
                this->playing_file = false;
                fclose(this->current_file_handler);
            }

            this->current_file_handler = fopen( this->filename.c_str(), "r");
            if(this->current_file_handler == NULL){
                gcode->stream->printf("file.open failed: %s\r\n", this->filename.c_str());
            }else{
                this->playing_file = true;
            }
        }
    }
}

//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);
                    }
//.........这里部分代码省略.........