You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
*/
-/*
#include "libs/Module.h"
#include "libs/Kernel.h"
#include "modules/robot/Player.h"
Extruder::Extruder() {
this->absolute_mode = true;
- this->direction = 1;
- this->acceleration_lock = false;
this->step_counter = 0;
this->counter_increment = 0;
this->paused = false;
this->register_for_event(ON_GCODE_EXECUTE);
this->register_for_event(ON_PLAY);
this->register_for_event(ON_PAUSE);
+ this->register_for_event(ON_SPEED_CHANGE);
// Start values
- this->start_position = 0;
this->target_position = 0;
this->current_position = 0;
this->current_block = NULL;
// TODO: Make this an independent setting
this->kernel->slow_ticker->attach( this->kernel->stepper->acceleration_ticks_per_second , this, &Extruder::acceleration_tick );
- // Initiate main_interrupt timer and step reset timer
- this->kernel->step_ticker->attach( this, &Extruder::stepping_tick );
- this->kernel->step_ticker->reset_attach( this, &Extruder::reset_step_pin );
+ // Stepper motor object for the extruder
+ this->stepper_motor = this->kernel->step_ticker->add_stepper_motor( new StepperMotor(this->step_pin,this->dir_pin,this->en_pin) );
+ this->stepper_motor->attach(this, &Extruder::stepper_motor_finished_move );
}
// Get config
void Extruder::on_config_reload(void* argument){
this->microseconds_per_step_pulse = this->kernel->config->value(microseconds_per_step_pulse_checksum)->by_default(5)->as_number();
- this->steps_per_millimeter = this->kernel->config->value(extruder_steps_per_mm_checksum )->by_default(1)->as_number();
- this->feed_rate = this->kernel->config->value(default_feed_rate_checksum )->by_default(1)->as_number();
- this->acceleration = this->kernel->config->value(acceleration_checksum )->by_default(1)->as_number();
+ this->steps_per_millimeter = this->kernel->config->value(extruder_steps_per_mm_checksum )->by_default(1)->as_number();
+ this->feed_rate = this->kernel->config->value(default_feed_rate_checksum )->by_default(1000)->as_number();
+ this->acceleration = this->kernel->config->value(extruder_acceleration_checksum )->by_default(1000)->as_number();
this->step_pin = this->kernel->config->value(extruder_step_pin_checksum )->by_default("nc" )->as_pin()->as_output();
this->dir_pin = this->kernel->config->value(extruder_dir_pin_checksum )->by_default("nc" )->as_pin()->as_output();
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 from 'G' Gcode
Block* block = static_cast<Block*>(argument);
if( this->mode == SOLO ){
// In solo mode we take the block so we can move even if the stepper has nothing to do
+
block->take();
this->current_block = block;
- this->start_position = this->target_position;
- this->target_position = this->start_position + this->travel_distance ;
- this->travel_ratio = 0.2; // TODO : Make a real acceleration thing
- if( this->target_position > this->current_position ){ this->direction = 1; }else if( this->target_position < this->current_position ){ this->direction = -1; }
- this->set_speed(int(floor((this->feed_rate/60)*this->steps_per_millimeter)));//Speed in steps per second
+ this->target_position = this->current_position + this->travel_distance ;
+
+ this->stepper_motor->move( ( this->travel_distance > 0 ), int(floor(this->steps_per_millimeter * fabs(this->travel_distance))));
+
}else if( this->mode == FOLLOW ){
// In non-solo mode, we just follow the stepper module
+
+ block->take();
this->current_block = block;
- this->start_position = this->target_position;
- this->target_position = this->start_position + ( this->current_block->millimeters * this->travel_ratio );
- if( this->target_position > this->current_position ){ this->direction = 1; }else if( this->target_position < this->current_position ){ this->direction = -1; }
- this->acceleration_tick(0);
+ this->target_position = this->current_position + ( this->current_block->millimeters * this->travel_ratio );
+
+ this->stepper_motor->move( ( this->travel_distance > 0 ), int(floor(this->steps_per_millimeter * fabs(this->travel_distance))));
+
+ }else if( this->mode == OFF ){
+ // No movement means we must reset our speed
+
+ this->stepper_motor->set_speed(0);
+
}
}
uint32_t Extruder::acceleration_tick(uint32_t dummy){
// Avoid trying to work when we really shouldn't ( between blocks or re-entry )
- if( this->current_block == NULL || this->acceleration_lock || this->paused ){ return 0; }
- this->acceleration_lock = true;
-
- // In solo mode, we mode independently from the robot
- if( this->mode == SOLO ){
- // TODO : Do real acceleration here
- this->travel_ratio += 0.03;
- if( this->travel_ratio > 1 ){ this->travel_ratio = 1; }
- this->set_speed( int(floor(((this->feed_rate/60)*this->steps_per_millimeter)*this->travel_ratio)) ); // Speed in steps per second
+ if( this->current_block == NULL || this->paused || this->mode != SOLO ){ return 0; }
+
+ uint32_t current_rate = this->stepper_motor->steps_per_second;
+ uint32_t target_rate = int(floor((this->feed_rate/60)*this->steps_per_millimeter));
+
+ if( current_rate < target_rate ){
+ uint32_t rate_increase = int(floor((this->acceleration/this->kernel->stepper->acceleration_ticks_per_second)*this->steps_per_millimeter));
+ uint32_t new_rate = min( target_rate, current_rate + rate_increase );
+ this->stepper_motor->set_speed(new_rate);
+ }
// In follow mode we match the speed of the robot, + eventually advance
- }else if( this->mode == FOLLOW ){
+
+ /*
+ //}else if( this->mode == FOLLOW ){
Stepper* stepper = this->kernel->stepper; // Just for convenience
// Strategy :
int ticks_forward = 3;
// We need to take those values here, and then use those instead of the live values, because using the live values inside the loop can break things ( infinite loops etc ... )
double next_stepper_rate = stepper->trapezoid_adjusted_rate;
- double step_events_completed = (double(double(stepper->step_events_completed)/double(1<<16)));
- double position = ( this->current_position - this->start_position ) * this->direction ;
- double length = fabs( this->start_position - this->target_position );
+ double step_events_completed = 0;// (double(double(stepper->step_events_completed)/double(1<<16)));
+ double position = 0; //( this->current_position - this->start_position ) * this->direction ;
+ double length = 0; //fabs( this->start_position - this->target_position );
double last_ratio = -1;
// Do the startegy above, but if it does not work, look a bit further and try again, and again ...
next_relative_position += ( advance );
// TODO : all of those "if->return" is very hacky, we should do the math in a way where most of those don't happen, but that requires doing tons of drawing ...
- if( last_ratio == next_ratio ){ this->acceleration_lock = false; return 0; }else{ last_ratio = next_ratio; }
- if( next_ratio == 0 || next_ratio > 1 ){ this->acceleration_lock = false; return 0; }
- if( ticks_forward > 1000 ){ this->acceleration_lock = false; return 0; } // This is very ugly
+ if( last_ratio == next_ratio ){ return 0; }else{ last_ratio = next_ratio; }
+ if( next_ratio == 0 || next_ratio > 1 ){ return 0; }
+ if( ticks_forward > 1000 ){ return 0; } // This is very ugly
// Hack : We have not looked far enough, we compute how far ahead we must look to get a relevant value
if( position > next_relative_position ){
double ticks_to_equilibrium = ceil(far_back_ratio / ratio_per_tick) + 1;
ticks_forward += ticks_to_equilibrium;
// Because this is a loop, and we can be interrupted by the stepping interrupt, if that interrupt changes block, the new block may not be solo, and we may get trapped into an infinite loop
- if( this->mode != FOLLOW ){ this->acceleration_lock = false; return 0; }
+ if( this->mode != FOLLOW ){ return 0; }
continue;
}
// Finally, compute the speed to get to that next position
- double next_absolute_position = this->start_position + ( this->direction * next_relative_position );
- double steps_to_next_tick = ( next_relative_position - position ) * this->steps_per_millimeter;
- double speed_to_next_tick = steps_to_next_tick / ( 1 / double(double(this->kernel->stepper->acceleration_ticks_per_second) / ticks_forward) );
+ //double next_absolute_position = this->start_position + ( this->direction * next_relative_position );
+ //double steps_to_next_tick = ( next_relative_position - position ) * this->steps_per_millimeter;
+ //double speed_to_next_tick = steps_to_next_tick / ( 1 / double(double(this->kernel->stepper->acceleration_ticks_per_second) / ticks_forward) );
// Change stepping speed
- this->set_speed( speed_to_next_tick );
+ //this->set_speed( speed_to_next_tick );
- this->acceleration_lock = false;
return 0;
}
}
+ */
+
- this->acceleration_lock = false;
- return 0;
+ return 0;
}
-// Convenience function to set stepping speed
-void Extruder::set_speed( int steps_per_second ){
+// Speed has been updated for the robot's stepper, we must update accordingly
+void Extruder::on_speed_change( void* argument ){
- if( steps_per_second < 10 ){ steps_per_second = 10; }
+ // Avoid trying to work when we really shouldn't ( between blocks or re-entry )
+ if( this->current_block == NULL || this->paused || this->mode != FOLLOW ){ return; }
- // TODO : Proper limit config value
- if( steps_per_second > (this->feed_rate*double(this->steps_per_millimeter))/60 ){
- steps_per_second = (this->feed_rate*double(this->steps_per_millimeter))/60;
- }
- this->counter_increment = int(floor(double(1<<16)/double(this->kernel->stepper->base_stepping_frequency / steps_per_second)));
}
-inline uint32_t Extruder::stepping_tick(uint32_t dummy){
- if( this->paused ){ return 0; }
- this->step_counter += this->counter_increment;
- if( this->step_counter > 1<<16 ){
- this->step_counter -= 1<<16;
- // If we still have steps to do
- // TODO: Step using the same timer as the robot, and count steps instead of absolute float position
- if( ( this->current_position < this->target_position && this->direction == 1 ) || ( this->current_position > this->target_position && this->direction == -1 ) ){
- this->current_position += (double(double(1)/double(this->steps_per_millimeter)))*double(this->direction);
- this->dir_pin->set((this->direction > 0) ? 1 : 0);
- this->step_pin->set(1);
- }else{
- // Move finished
- if( this->mode == SOLO && this->current_block != NULL ){
- // In follow mode, the robot takes and releases the block, in solo mode we do
- this->current_block->release();
- }
- }
- }
- return 0;
-}
+// When the stepper has finished it's move
+uint32_t Extruder::stepper_motor_finished_move(uint32_t dummy){
-uint32_t Extruder::reset_step_pin(uint32_t dummy){
- this->step_pin->set(0);
- return 0;
-}
-
-*/
+ this->current_position = this->target_position;
+ this->current_block->release();
+}
HCoop / clinton / Smoothieware.git / commitdiff