tool change

gistfile1.txt

// Compute extrusion speed based on parameters and gcode distance of travel
void ToolsManager::on_gcode_execute(void* argument){
    Gcode* gcode = static_cast<Gcode*>(argument);

    if( gcode->has_letter('T') ){
        int new_tool = gcode->get_value('T');
        bool make_move = false;
        if ( gcode->has_letter('F') ){
            make_move = true;
        }
        if(new_tool != this->active_tool){
            void *returned_data;
            bool ok = THEKERNEL->public_data->get_value( robot_checksum, current_position_checksum, &returned_data );
            if(ok){
                // save current position to return to after applying extruder offset
                float *pos = static_cast<float *>(returned_data);
                float current_pos[3];
                for(int i=0;i<3;i++){
                    current_pos[i] = pos[i];
                }
                // update virtual tool position to the offset of the new tool and select it as active
                float new_pos[3];
                float *active_tool_offset = tools[this->active_tool]->get_offset();
                float *new_tool_offset = tools[new_tool]->get_offset();
                for(int i=0;i<3;i++){
                    new_pos[i] = current_pos[i] - active_tool_offset[i] + new_tool_offset[i];
                }

                this->active_tool = new_tool;
                ok = THEKERNEL->public_data->set_value( robot_checksum, current_position_checksum, new_pos );
                if(ok && make_move){
                    //TODO:move to old position (stored in current_pos[])
                }
            }
        }
    }
}