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

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

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

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

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

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

}

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

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

    if (gcode->has_m) {
        if(gcode->m == 906) {
            if (gcode->has_letter(designator)) {
                // set motor currents in mA (Note not using M907 as digipots use that)
                current= gcode->get_value(designator);
                current= std::min(current, max_current);
                set_current(current);
                current_override= true;
            }

        } else if(gcode->m == 909) { // M909 Annn set microstepping, M909.1 also change steps/mm
            if (gcode->has_letter(designator)) {
                uint32_t current_microsteps= microsteps;
                microsteps= gcode->get_value(designator);
                microsteps= set_microstep(microsteps); // driver may change the steps it sets to
                if(gcode->subcode == 1 && current_microsteps != microsteps) {
                    // also reset the steps/mm
                    int a= designator-'A';
                    if(a >= 0 && a <=2) {
                        float s= THEKERNEL->robot->actuators[a]->get_steps_per_mm()*((float)microsteps/current_microsteps);
                        THEKERNEL->robot->actuators[a]->change_steps_per_mm(s);
                        gcode->stream->printf("steps/mm for %c changed to: %f\n", designator, s);
                        THEKERNEL->robot->check_max_actuator_speeds();
                    }
                }
                microstep_override= true;
            }

        // } else if(gcode->m == 910) { // set decay mode
        //     if (gcode->has_letter(designator)) {
        //         decay_mode= gcode->get_value(designator);
        //         set_decay_mode(decay_mode);
        //     }

        } else if(gcode->m == 911) {
            // set or get raw registers
            // M911 will dump all the registers and status of all the motors
            // M911.1 Pn (or A0) will dump the registers and status of the selected motor. X0 will request format in processing machine readable format
            // M911.2 Pn (or B0) Rxxx Vyyy sets Register xxx to value yyy for motor nnn, xxx == 255 writes the registers, xxx == 0 shows what registers are mapped to what
            // M911.3 Pn (or C0) will set the options based on the parameters passed as below...
            // TMC2660:-
            // M911.3 Onnn Qnnn setStallGuardThreshold O=stall_guard_threshold, Q=stall_guard_filter_enabled
            // M911.3 Hnnn Innn Jnnn Knnn Lnnn setCoolStepConfiguration H=lower_SG_threshold, I=SG_hysteresis, J=current_decrement_step_size, K=current_increment_step_size, L=lower_current_limit
            // M911.3 S0 Unnn Vnnn Wnnn Xnnn Ynnn setConstantOffTimeChopper  U=constant_off_time, V=blank_time, W=fast_decay_time_setting, X=sine_wave_offset, Y=use_current_comparator
            // M911.3 S1 Unnn Vnnn Wnnn Xnnn Ynnn setSpreadCycleChopper  U=constant_off_time, V=blank_time, W=hysteresis_start, X=hysteresis_end, Y=hysteresis_decrement
            // M911.3 S2 Zn setRandomOffTime Z=on|off Z1 is on Z0 is off
            // M911.3 S3 Zn setDoubleEdge Z=on|off Z1 is on Z0 is off
            // M911.3 S4 Zn setStepInterpolation Z=on|off Z1 is on Z0 is off
            // M911.3 S5 Zn setCoolStepEnabled Z=on|off Z1 is on Z0 is off

            if(gcode->subcode == 0 && gcode->get_num_args() == 0) {
                // M911 no args dump status for all drivers, M911.1 P0|A0 dump for specific driver
                gcode->stream->printf("Motor %d (%c)...\n", id, designator);
                dump_status(gcode->stream, true);

            }else if(gcode->get_value('P') == id || gcode->has_letter(designator)) {
                if(gcode->subcode == 1) {
                    dump_status(gcode->stream, !gcode->has_letter('X'));

                }else if(gcode->subcode == 2 && gcode->has_letter('R') && gcode->has_letter('V')) {
                    set_raw_register(gcode->stream, gcode->get_value('R'), gcode->get_value('V'));

                }else if(gcode->subcode == 3 ) {
                    set_options(gcode);
                }
            }

        } else if(gcode->m == 500 || gcode->m == 503) {
            if(current_override) {
                gcode->stream->printf(";Motor %c id %d  current mA:\n", designator, id);
                gcode->stream->printf("M906 %c%lu\n", designator, current);
            }
            if(microstep_override) {
                gcode->stream->printf(";Motor %c id %d  microsteps:\n", designator, id);
                gcode->stream->printf("M909 %c%lu\n", designator, microsteps);
            }
            //gcode->stream->printf("M910 %c%d\n", designator, decay_mode);
        }
    }
}