}
extern "C" void TIMER2_IRQHandler (void){
+
+ LPC_GPIO1->FIOSET = 1<<21;
+
if((LPC_TIM2->IR >> 0) & 1){ // If interrupt register set for MR0
LPC_TIM2->IR |= 1 << 0; // Reset it
global_slow_ticker->tick();
}
+
+ LPC_GPIO1->FIOCLR = 1<<21;
+
}
while(current != NULL ){
current->step_pin->set(0);
if( current->exit_tick ){
- if( current->dont_remove_from_active_list_yet ){
- current->dont_remove_from_active_list_yet = false;
+ if( current->remove_from_active_list_next_tick ){
+ current->remove_from_active_list_next_tick = false;
}else{
this->remove_motor_from_active_list(current);
current_id--;
// The actual interrupt handler where we do all the work
extern "C" void TIMER0_IRQHandler (void){
-
+
+ LPC_GPIO1->FIOSET = 1<<18;
+
uint32_t start_time = LPC_TIM0->TC;
LPC_TIM0->IR |= 1 << 0;
// If no axes enabled, just ignore for now
if( !global_step_ticker->has_axes ){
+
+ LPC_GPIO1->FIOCLR = 1<<18;
return;
+
}
// Do not get out of here before everything is nice and tidy
}
+ LPC_GPIO1->FIOCLR = 1<<18;
}
this->direction_bit = 0;
this->step_bit = 0;
this->update_exit_tick();
- this->dont_remove_from_active_list_yet = false;
+ this->remove_from_active_list_next_tick = false;
this->exit_tick = true;
}
this->direction_bit = 0;
this->step_bit = 0;
this->update_exit_tick();
- this->dont_remove_from_active_list_yet = false;
+ this->remove_from_active_list_next_tick = false;
this->exit_tick = true;
}
// This is just a way not to check for ( !this->moving || this->paused || this->fx_ticks_per_step == 0 ) at every tick()
inline void StepperMotor::update_exit_tick(){
//printf("try update list %u %u %u\r\n", this->moving, this->paused, this->fx_ticks_per_step == 0 );
- if( !this->moving || this->paused || this->fx_ticks_per_step == 0 || this->steps_to_move == 0 ){
+ if( !this->moving || this->paused || this->steps_to_move == 0 ){
// We must exit tick() after setting the pins, no bresenham is done
if( this->exit_tick == false ){
- //printf("set for removal %p \r\n", this);
this->exit_tick = true;
- this->dont_remove_from_active_list_yet = true;
+ this->remove_from_active_list_next_tick = true;
}
}else{
// We must do the bresenham in tick()
if( this->exit_tick == true ){
+ // We have to do this or there could be a bug where the removal still happens when it doesn't need to
+ this->remove_from_active_list_next_tick = false;
+
this->exit_tick = false;
- //printf("adding motor %p \r\n", this);
this->step_ticker->add_motor_to_active_list(this);
}
}
this->stepped = 0;
// Starting now we are moving
- if( steps > 0 ){ this->moving = true; }else{ this->moving = false; }
+ if( steps > 0 ){
+ this->moving = true;
+ }else{
+ this->moving = false;
+ }
this->update_exit_tick();
}
this->fx_ticks_per_step = 1>>63;
return;
}
-
+
+ //if( speed < this->steps_per_second ){
+ LPC_GPIO1->FIOSET = 1<<19;
+ //}
+
// How many steps we must output per second
this->steps_per_second = speed;
double ticks_per_step = (double)( (double)this->step_ticker->frequency / speed );
double double_fx_ticks_per_step = (double)(1<<16) * ( (double)(1<<16) * ticks_per_step );
this->fx_ticks_per_step = (uint64_t)( floor(double_fx_ticks_per_step) );
- this->update_exit_tick();
+
+ LPC_GPIO1->FIOCLR = 1<<19;
}
+void StepperMotor::pause(){
+ this->paused = true;
+ this->update_exit_tick();
+}
+void StepperMotor::unpause(){
+ this->paused = false;
+ this->update_exit_tick();
+}
void move( bool direction, unsigned int steps );
void set_speed( double speed );
void update_exit_tick();
+ void pause();
+ void unpause();
template<typename T> void attach( T *optr, uint32_t ( T::*fptr )( uint32_t ) ){
Hook* hook = new Hook();
uint64_t fx_ticks_per_step;
bool exit_tick;
- bool dont_remove_from_active_list_yet;
+ bool remove_from_active_list_next_tick;
};
message.stream = kernel->serial;
kernel->call_event(ON_CONSOLE_LINE_RECEIVED, &message );
-
+ /*
int i = 0;
while( i <= 60 ){
// Debug : launch file on startup
i++;
}
+ */
+
+ /*
+ int i = 0;
+ while( i <= 60 ){
+ // Debug : launch file on startup
+
+ message.message = "G1 X40 Y0 F9000";
+ message.stream = kernel->serial;
+ kernel->call_event(ON_CONSOLE_LINE_RECEIVED, &message );
+
+ message.message = "G1 X40 Y1 F9000";
+ message.stream = kernel->serial;
+ kernel->call_event(ON_CONSOLE_LINE_RECEIVED, &message );
+
+
+ message.message = "G1 X0 Y1 F9000";
+ message.stream = kernel->serial;
+ kernel->call_event(ON_CONSOLE_LINE_RECEIVED, &message );
+
+
+ message.message = "G1 X0 Y0 F9000";
+ message.stream = kernel->serial;
+ kernel->call_event(ON_CONSOLE_LINE_RECEIVED, &message );
+
+ i++;
+ }
+ */
- /*
// Debug : launch file on startup
- struct SerialMessage message;
+ //struct SerialMessage message;
//message.message = "G1 X1000 F2000";
- message.message = "play /sd/victor.g -q";
+ message.message = "play /sd/laurana.g -q";
message.stream = kernel->serial;
kernel->call_event(ON_CONSOLE_LINE_RECEIVED, &message );
- */
while(1){
kernel->call_event(ON_MAIN_LOOP);
// When the play/pause button is set to pause, or a module calls the ON_PAUSE event
void Stepper::on_pause(void* argument){
this->paused = true;
- this->kernel->robot->alpha_stepper_motor->paused = true;
- this->kernel->robot->beta_stepper_motor->paused = true;
- this->kernel->robot->gamma_stepper_motor->paused = true;
-
+ this->kernel->robot->alpha_stepper_motor->pause();
+ this->kernel->robot->beta_stepper_motor->pause();
+ this->kernel->robot->gamma_stepper_motor->pause();
}
// When the play/pause button is set to play, or a module calls the ON_PLAY event
void Stepper::on_play(void* argument){
// TODO: Re-compute the whole queue for a cold-start
this->paused = false;
- this->kernel->robot->alpha_stepper_motor->paused = false;
- this->kernel->robot->beta_stepper_motor->paused = false;
- this->kernel->robot->gamma_stepper_motor->paused = false;
+ this->kernel->robot->alpha_stepper_motor->unpause();
+ this->kernel->robot->beta_stepper_motor->unpause();
+ this->kernel->robot->gamma_stepper_motor->unpause();
}
//block->debug(this->kernel);
- LPC_GPIO1->FIOCLR = 1<<18;
// We can't move with the enable pins off
if( this->enable_pins_status == false ){
}
+#pragma GCC push_options
+#pragma GCC optimize ("O0")
+
// When a stepper motor has finished it's assigned movement
-inline uint32_t Stepper::stepper_motor_finished_move(uint32_t dummy){
+uint32_t Stepper::stepper_motor_finished_move(uint32_t dummy){
// We care only if none is still moving
if( this->kernel->robot->alpha_stepper_motor->moving || this->kernel->robot->beta_stepper_motor->moving || this->kernel->robot->gamma_stepper_motor->moving ){ return 0; }
// This block is finished, release it
if( this->current_block != NULL ){
- LPC_GPIO1->FIOSET = 1<<18;
this->current_block->release();
return this->main_stepper->stepped;
}
+
// This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event
// interrupt. It can be assumed that the trapezoid-generator-parameters and the
// current_block stays untouched by outside handlers for the duration of this function call.
uint32_t Stepper::trapezoid_generator_tick( uint32_t dummy ) {
+
if(this->current_block && !this->paused && this->main_stepper->moving ) {
uint32_t current_steps_completed = this->main_stepper->stepped;
-
- speed_ticks_counter++;
-
- if( previous_step_count == current_steps_completed && previous_step_count != 0 ){
+
+ if( current_steps_completed >= this->main_stepper->steps_to_move - 3 ){
+ }
+
+ if( previous_step_count == current_steps_completed && previous_step_count != 0 ){
// We must skip this step update because no step has happened
skipped_speed_updates++;
return 0;
this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
}
- if(current_steps_completed < this->current_block->accelerate_until) {
-
-
+ if(current_steps_completed <= this->current_block->accelerate_until + 1) {
+ /*
if( this->current_block->initial_rate == 0 && 0 ){
__disable_irq();uint32_t start_tc0 = LPC_TIM0->TC; uint32_t start_tc2 = LPC_TIM2->TC;
this->kernel->streams->printf("acc{%u<%u} tar:%f im:%u sps:%f pos:%u/%u cnt:%u delta:%f \r\n", current_steps_completed, this->current_block->accelerate_until, this->trapezoid_adjusted_rate, this->main_stepper->moving, this->main_stepper->steps_per_second, this->main_stepper->stepped, this->main_stepper->steps_to_move, (uint32_t)(this->main_stepper->fx_ticks_per_step>>32), this->current_block->rate_delta );
LPC_TIM0->TC = start_tc0; LPC_TIM2->TC = start_tc2; __enable_irq();
}
+ */
-
- this->trapezoid_adjusted_rate += this->current_block->rate_delta + ( this->current_block->rate_delta * skipped_speed_updates );
+ this->trapezoid_adjusted_rate += ( skipped_speed_updates + 1 ) * this->current_block->rate_delta;
+
if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) {
- //this->kernel->streams->printf("reached after %u when it should be %u\r\n", current_steps_completed, this->current_block->accelerate_until);
+
+ //this->kernel->streams->printf("reached after %u with au %u and da %u \r\n", current_steps_completed, this->current_block->accelerate_until, this->current_block->decelerate_after);
+
this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
+
}
+
this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
+
+
}else if (current_steps_completed > this->current_block->decelerate_after + 1) {
+ LPC_GPIO1->FIOSET = 1<<20;
+ /*
if( this->current_block->final_rate == 0 && 0 ){
__disable_irq();uint32_t start_tc0 = LPC_TIM0->TC; uint32_t start_tc2 = LPC_TIM2->TC;
this->kernel->streams->printf("dec{%u>=%u} tar:%f im:%u sps:%f pos:%u/%u cnt:%u delta:%f\r\n", current_steps_completed, this->current_block->decelerate_after, this->trapezoid_adjusted_rate, this->main_stepper->moving, this->main_stepper->steps_per_second, this->main_stepper->stepped, this->main_stepper->steps_to_move, (uint32_t)(this->main_stepper->fx_ticks_per_step>>32), this->current_block->rate_delta );
LPC_TIM0->TC = start_tc0; LPC_TIM2->TC = start_tc2; __enable_irq();
}
+ */
// NOTE: We will only reduce speed if the result will be > 0. This catches small
// rounding errors that might leave steps hanging after the last trapezoid tick.
if(this->trapezoid_adjusted_rate > this->current_block->rate_delta * 1.5) {
- this->trapezoid_adjusted_rate -= this->current_block->rate_delta + ( this->current_block->rate_delta * skipped_speed_updates );
+ this->trapezoid_adjusted_rate -= ( skipped_speed_updates + 1 ) * this->current_block->rate_delta;
}else{
//this->kernel->streams->printf("delta reached after %u when it should be %u\r\n", current_steps_completed, this->main_stepper->steps_to_move);
this->trapezoid_adjusted_rate = this->current_block->rate_delta * 1.5;
this->trapezoid_adjusted_rate = this->current_block->final_rate;
}
this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
- }else {
+
+
+ LPC_GPIO1->FIOCLR = 1<<20;
+
+ }else {
// Make sure we cruise at exactly nominal rate
if (this->trapezoid_adjusted_rate != this->current_block->nominal_rate) {
this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
}
}
+
+
+
}
skipped_speed_updates = 0;
+
+
}
+#pragma GCC pop_options
+
+
// Initializes the trapezoid generator from the current block. Called whenever a new
// block begins.
inline void Stepper::trapezoid_generator_reset(){
// When the play/pause button is set to pause, or a module calls the ON_PAUSE event
void Extruder::on_pause(void* argument){
this->paused = true;
+ this->stepper_motor->pause();
}
// When the play/pause button is set to play, or a module calls the ON_PLAY event
void Extruder::on_play(void* argument){
this->paused = false;
+ this->stepper_motor->unpause();
}
int old_steps = this->current_steps;
int target_steps = int( floor(this->steps_per_millimeter*this->target_position) );
- int steps_to_step = abs( target_steps - old_steps );
+ int steps_to_step = target_steps - old_steps ;
this->current_steps = target_steps;
//printf("spm:%f td:%f steps:%d ( %f - %f ) \r\n", this->steps_per_millimeter, this->travel_distance, steps_to_step, this->target_position, this->current_position );
- this->stepper_motor->move( ( this->travel_distance > 0 ), steps_to_step );
+ this->stepper_motor->move( ( steps_to_step > 0 ), abs(steps_to_step) );
uint32_t current_rate = this->stepper_motor->steps_per_second;
uint32_t target_rate = int(floor((this->feed_rate/60)*this->steps_per_millimeter));
- //printf("update extruder speed in acceleration tick \r\n");
-
- //if( LPC_TIM0->TC % 7 == 2 ){
- // printf("speed:{current: %u target: %u} position:{stepped:%u total:%u} \r\n", current_rate, target_rate, this->stepper_motor->stepped, this->stepper_motor->steps_to_move);
- //}
-
if( current_rate < target_rate ){
uint32_t rate_increase = int(floor((this->acceleration/this->kernel->stepper->acceleration_ticks_per_second)*this->steps_per_millimeter));
current_rate = min( target_rate, current_rate + rate_increase );
}
if( current_rate > target_rate ){ current_rate = target_rate; }
- this->stepper_motor->set_speed(current_rate);
+ this->stepper_motor->set_speed(max(current_rate, this->kernel->stepper->minimum_steps_per_minute/60));
return 0;
}
// Avoid trying to work when we really shouldn't ( between blocks or re-entry )
if( this->current_block == NULL || this->paused || this->mode != FOLLOW || this->stepper_motor->moving != true ){ return; }
- uint32_t adjusted_rate = max( this->kernel->stepper->trapezoid_adjusted_rate, this->kernel->stepper->minimum_steps_per_minute );
-
/*
* nominal block duration = current block's steps / ( current block's nominal rate / 60 )
* nominal extruder rate = extruder steps / nominal block duration
* or simplified : ( extruder steps * ( stepper's steps per minute / 60 ) ) / current block's steps
* or even : ( stepper steps per minute / 60 ) * ( extruder steps / current block's steps )
*/
- // printf("update extruder speed in speed change \r\n");
- this->stepper_motor->set_speed( (adjusted_rate/60L) * ( (double)this->stepper_motor->steps_to_move / (double)this->current_block->steps_event_count ) );
+
+ this->stepper_motor->set_speed( max( ( this->kernel->stepper->trapezoid_adjusted_rate /60L) * ( (double)this->stepper_motor->steps_to_move / (double)this->current_block->steps_event_count ), this->kernel->stepper->minimum_steps_per_minute/60 ) );
}
HCoop / clinton / Smoothieware.git / commitdiff