extruder module completely messed up, trying something completely

[clinton/Smoothieware.git] / src / modules / tools / extruder / Extruder.cpp

#include "mbed.h"
#include "libs/Module.h"
#include "libs/Kernel.h"
#include "modules/robot/Block.h"
#include "modules/tools/extruder/Extruder.h"



Extruder* extruder_for_irq; // We need this global because ISRs can't be attached to an object
extern "C" void TIMER1_IRQHandler (void){
    if((LPC_TIM1->IR >> 1) & 1){
        LPC_TIM1->IR |= 1 << 1;
        extruder_for_irq->step_pin = 0;
    }
    if((LPC_TIM1->IR >> 0) & 1){
        LPC_TIM1->IR |= 1 << 0;
        extruder_for_irq->stepping_tick();
    }
}

Extruder::Extruder(PinName stppin) : step_pin(stppin){
    this->absolute_mode = true;
}

void Extruder::on_module_loaded() {

    if( this->kernel->config->value( extruder_module_enable_checksum )->by_default(false)->as_bool() == false ){ return; } 

    extruder_for_irq = this;

    // Settings
    this->on_config_reload(this);

    // We work on the same Block as Stepper, so we need to know when it gets a new one and drops one
    this->register_for_event(ON_BLOCK_BEGIN);
    this->register_for_event(ON_BLOCK_END);
    this->register_for_event(ON_GCODE_EXECUTE);

    // Configuration
    this->acceleration_ticker.attach_us(this, &Extruder::acceleration_tick, 1000000/this->kernel->stepper->acceleration_ticks_per_second);

    this->start_position = 0;
    this->target_position = 0;
    this->current_position = 0;
    this->current_block = NULL;

    // Start Timer1
    LPC_TIM1->MR0 = 10000; 
    LPC_TIM1->MR1 = 500;
    LPC_TIM1->MCR = 11;            // For MR0 and MR1, with no reset at MR1
    NVIC_EnableIRQ(TIMER1_IRQn);
    LPC_TIM1->TCR = 1;  
}

// Get config
void Extruder::on_config_reload(void* argument){
    this->microseconds_per_step_pulse = 5; //this->kernel->config->value(microseconds_per_step_pulse_ckeckusm)->by_default(5)->as_number();
}

// Computer extrusion speed based on parameters and gcode distance of travel
void Extruder::on_gcode_execute(void* argument){
    Gcode* gcode = static_cast<Gcode*>(argument);
   
    this->kernel->serial->printf("exec: %s \r\n", gcode->command.c_str() );

    // Absolute/relative mode
    if( gcode->has_letter('M')){
        int code = gcode->get_value('M');
        if( code == 82 ){ this->absolute_mode == true; }
        if( code == 83 ){ this->absolute_mode == false; }
    } 
   
    if( gcode->has_letter('G') ){

        if( gcode->get_value('G') == 92 ){

            if( gcode->has_letter('E') ){
                this->current_position = gcode->get_value('E');
                this->target_position  = this->current_position;
                this->start_position   = this->current_position; 
            }            

        }else{

            // Extrusion length 
            if( gcode->has_letter('E' )){
                double extrusion_distance = gcode->get_value('E');
                if( fabs(gcode->millimeters_of_travel) < 0.0001 ){
                    this->solo_mode = true;
                    this->travel_distance = extrusion_distance;
                    this->travel_ratio = 0.0;
                }else{
                    this->solo_mode = false;
                    this->travel_ratio = extrusion_distance / gcode->millimeters_of_travel; 
                    this->travel_distance = 0.0;
                } 
            // Else do not extrude
            }else{
                this->travel_ratio = 0.0;
                this->travel_distance = 0.0;
            }

        }
 
    }    
    
}



void Extruder::on_block_begin(void* argument){
    Block* block = static_cast<Block*>(argument);

    if( fabs(this->travel_distance) > 0.001 ){
        block->take(); // In solo mode we take the block so we can move even if the stepper has nothing to do
        this->current_block = block; 
        this->start_position = this->target_position;
        this->target_position = ( !this->absolute_mode ? this->start_position : 0 ) + this->travel_distance ;
        //this->acceleration_tick();
    }   
    if( fabs(this->travel_ratio) > 0.001 ){
        // In non-solo mode, we just follow the stepper module
        this->current_block = block; 
        this->start_position = this->target_position;
        this->target_position =  this->start_position + ( this->current_block->millimeters * this->travel_ratio );
        //this->acceleration_tick();
    } 


}

void Extruder::on_block_end(void* argument){
    Block* block = static_cast<Block*>(argument);
    this->current_block = NULL; 
}       

void Extruder::acceleration_tick(){
   
    // If we are currently taking care of a block            
    if( this->current_block ){

        this->kernel->serial->printf("tick start_position:%f current_position:%f target_position:%f travel_distance:%f travel_ratio:%f mm:%f \r\n", this->start_position, this->current_position, this->target_position, this->travel_distance, this->travel_ratio, this->current_block->millimeters); 

        if( fabs(this->travel_ratio) > 0.001 ){
            double steps_by_acceleration_tick = this->kernel->stepper->trapezoid_adjusted_rate / 60 / this->kernel->stepper->acceleration_ticks_per_second;                
            // Get position along the block at next tick
            if( this->kernel->stepper->current_block != NULL ){ 
                this->kernel->stepper->current_block->debug( this->kernel );  
            }else{
                this->kernel->serial->printf("NULL\r\n");
                return;
            }
            double current_position_ratio = double( (this->kernel->stepper->step_events_completed>>16)                              ) / double(this->kernel->stepper->current_block->steps_event_count);
            double next_position_ratio =    double( (this->kernel->stepper->step_events_completed>>16) + steps_by_acceleration_tick ) / double(this->kernel->stepper->current_block->steps_event_count);
            // Ratio differenc
            double ratio_difference = next_position_ratio - current_position_ratio;
            // Get total move distance
            double distance = this->target_position - this->start_position;           
            // Get distance to run until next acceleration tick
            double desired_distance = distance / ratio_difference;
            // Get speed
            double desired_speed = desired_distance * double(this->kernel->stepper->acceleration_ticks_per_second); //TODO : Replace with the actual current_position

            this->kernel->serial->printf("cur_pos:%f tar_pos:%f start_pos:%f steps_by_acc_tick:%f cur_pos_ratio:%f next_pos_ratio:%f ratio_diff:%f dist:%f desired_dist:%f desired_spd:%f \r\n", this->current_position, this->target_position, this->start_position,  steps_by_acceleration_tick, current_position_ratio, next_position_ratio, ratio_difference, ratio_difference, distance, desired_distance, desired_speed );


            if( desired_speed <= 0 ){ return; }

            // Set timer
            LPC_TIM1->MR0 = ((SystemCoreClock/4))/int(floor(desired_speed*201)); 

            // In case we are trying to set the timer to a limit it has already past by
            if( LPC_TIM1->TC >= LPC_TIM1->MR0 ){
                LPC_TIM1->TCR = 3; 
                LPC_TIM1->TCR = 1; 
            }

            // Update Timer1    
            LPC_TIM1->MR1 = (( SystemCoreClock/4 ) / 1000000 ) * this->microseconds_per_step_pulse;

        }

        if( fabs(this->travel_distance) > 0.001 ){

            // Set timer
            LPC_TIM1->MR0 = ((SystemCoreClock/4))/int(floor(200)); 

            // In case we are trying to set the timer to a limit it has already past by
            if( LPC_TIM1->TC >= LPC_TIM1->MR0 ){
                LPC_TIM1->TCR = 3; 
                LPC_TIM1->TCR = 1; 
            }

            // Update Timer1    
            LPC_TIM1->MR1 = (( SystemCoreClock/4 ) / 1000000 ) * this->microseconds_per_step_pulse;

        }

    }
}

inline void Extruder::stepping_tick(){
    if( this->current_position < this->target_position ){
        this->current_position += double(double(1)/double(201));
        this->step_pin = 1;
    }else{
        // Move finished
        if( fabs(this->travel_distance) > 0.001 && this->current_block != NULL ){
            this->current_block->release();        
        } 
    }
}