this->digipot = new Digipot();
// LPC17xx-specific
- NVIC_SetPriority(TIMER0_IRQn, 1);
- NVIC_SetPriority(TIMER2_IRQn, 2);
+ NVIC_SetPriorityGrouping(0);
+ NVIC_SetPriority(TIMER0_IRQn, 2);
+ NVIC_SetPriority(TIMER1_IRQn, 1);
+ NVIC_SetPriority(TIMER2_IRQn, 3);
// Configure the step ticker
int base_stepping_frequency = this->config->value(base_stepping_frequency_checksum )->by_default(100000)->as_number();
Hook* hook = new Hook();
hook->frequency = frequency;
hook->attach(optr, fptr);
- hook->counter = -2;
+ hook->counter = -1.5;
if( frequency > this->max_frequency ){
this->max_frequency = frequency;
}
StepTicker::StepTicker(){
global_step_ticker = this;
LPC_TIM0->MR0 = 10000000; // Initial dummy value for Match Register
- LPC_TIM0->MR1 = 10000000;
- LPC_TIM0->MCR = 11; // Match on MR0, reset on MR0, match on MR1
+ LPC_TIM0->MCR = 3; // Match on MR0, reset on MR0, match on MR1
LPC_TIM0->TCR = 1; // Enable interrupt
+ LPC_SC->PCONP |= (1 << 2); // Power Ticker ON
+ LPC_TIM1->MR0 = 1000000;
+ LPC_TIM1->MCR = 1;
+ LPC_TIM1->TCR = 1; // Enable interrupt
+
// Default start values
this->debug = 0;
this->has_axes = 0;
this->active_motors[0] = NULL;
NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler
+ NVIC_EnableIRQ(TIMER1_IRQn); // Enable interrupt handler
}
// Set the base stepping frequency
// Set the reset delay
void StepTicker::set_reset_delay( double seconds ){
this->delay = int(floor(double(SystemCoreClock/4)*( seconds ))); // SystemCoreClock/4 = Timer increments in a second
+ LPC_TIM1->MR0 = this->delay;
}
// Add a stepper motor object to our list of steppers we must take care of
}
inline void StepTicker::tick(){
- //for(unsigned int i=0; i < this->stepper_motors.size(); i++){
- // this->stepper_motors[i]->tick();
- //}
-
uint8_t current_id = 0;
StepperMotor* current = this->active_motors[0];
while(current != NULL ){
}
}
+extern "C" void TIMER1_IRQHandler (void){
+ LPC_TIM1->IR |= 1 << 0;
+ global_step_ticker->reset_tick();
+}
+
// The actual interrupt handler where we do all the work
extern "C" void TIMER0_IRQHandler (void){
+
uint32_t start_time = LPC_TIM0->TC;
+
+ LPC_TIM0->IR |= 1 << 0;
+
global_step_ticker->debug++;
-
// If no axes enabled, just ignore for now
if( !global_step_ticker->has_axes ){
- if((LPC_TIM0->IR >> 0) & 1){ LPC_TIM0->IR |= 1 << 0; }
- if((LPC_TIM0->IR >> 1) & 1){ LPC_TIM0->IR |= 1 << 1; }
return;
}
-
- LPC_TIM0->MR1 = 2000000;
- if((LPC_TIM0->IR >> 0) & 1){ // If interrupt register set for MR0
+ // Do not get out of here before everything is nice and tidy
+ LPC_TIM0->MR0 = 2000000;
+ // Step pins
+ global_step_ticker->tick();
- // Do not get out of here before everything is nice and tidy
- LPC_TIM0->MR0 = 2000000;
- LPC_TIM0->IR |= 1 << 0; // Reset it
-
- // Step pins
- global_step_ticker->tick();
-
- // Maybe we have spent enough time in this interrupt so that we have to reset the pins ourself
- if( LPC_TIM0->TC > global_step_ticker->delay ){
- global_step_ticker->reset_tick();
- }else{
- // Else we have to trigger this a tad later, using MR1
- LPC_TIM0->MR1 = global_step_ticker->delay;
- }
-
- // If we went over the duration an interrupt is supposed to last, we have a problem
- // That can happen tipically when we change blocks, where more than usual computation is done
- // This can be OK, if we take notice of it, which we do now
- if( LPC_TIM0->TC > global_step_ticker->period ){ // TODO : remove the size condition
-
- uint32_t start_tc = LPC_TIM0->TC;
-
- // How many ticks we want to skip ( this does not include the current tick, but we add the time we spent doing this computation last time )
- uint32_t ticks_to_skip = ( ( LPC_TIM0->TC + global_step_ticker->last_duration ) / global_step_ticker->period );
-
- // Next step is now to reduce this to how many steps we can *actually* skip
- uint32_t ticks_we_actually_can_skip = ticks_to_skip;
- for(unsigned int i=0; i < global_step_ticker->stepper_motors.size(); i++){
- StepperMotor* stepper = global_step_ticker->stepper_motors[i];
- if( stepper->moving ){ ticks_we_actually_can_skip = min( ticks_we_actually_can_skip, (uint32_t)((uint64_t)( (uint64_t)stepper->fx_ticks_per_step - (uint64_t)stepper->fx_counter ) >> 32) ); }
- }
+ LPC_TIM1->TCR = 3;
+ LPC_TIM1->TCR = 1;
- // If the number of ticks we can actually skip is smaller than the number we wanted to skip, there is something wrong in the settings
- //if( ticks_we_actually_can_skip < ticks_to_skip ){ }
+ // If we went over the duration an interrupt is supposed to last, we have a problem
+ // That can happen tipically when we change blocks, where more than usual computation is done
+ // This can be OK, if we take notice of it, which we do now
+ if( LPC_TIM0->TC > global_step_ticker->period ){ // TODO : remove the size condition
- // Adding to MR0 for this time is not enough, we must also increment the counters ourself artificially
- for(unsigned int i=0; i < global_step_ticker->stepper_motors.size(); i++){
- StepperMotor* stepper = global_step_ticker->stepper_motors[i];
- if( stepper->moving ){ stepper->fx_counter += (uint64_t)((uint64_t)(ticks_we_actually_can_skip)<<32); }
- }
+ uint32_t start_tc = LPC_TIM0->TC;
- // When must we have our next MR0 ? ( +1 is here to account that we are actually a legit MR0 too, not only overtime )
- LPC_TIM0->MR0 = ( ticks_we_actually_can_skip + 1 ) * global_step_ticker->period;
+ // How many ticks we want to skip ( this does not include the current tick, but we add the time we spent doing this computation last time )
+ uint32_t ticks_to_skip = ( ( LPC_TIM0->TC + global_step_ticker->last_duration ) / global_step_ticker->period );
- // This is so that we know how long this computation takes, and we can take it into account next time
- global_step_ticker->last_duration = LPC_TIM0->TC - start_tc;
+ // Next step is now to reduce this to how many steps we can *actually* skip
+ uint32_t ticks_we_actually_can_skip = ticks_to_skip;
+ for(unsigned int i=0; i < global_step_ticker->stepper_motors.size(); i++){
+ StepperMotor* stepper = global_step_ticker->stepper_motors[i];
+ if( stepper->moving ){ ticks_we_actually_can_skip = min( ticks_we_actually_can_skip, (uint32_t)((uint64_t)( (uint64_t)stepper->fx_ticks_per_step - (uint64_t)stepper->fx_counter ) >> 32) ); }
+ }
- }else{
- LPC_TIM0->MR0 = global_step_ticker->period;
+ // If the number of ticks we can actually skip is smaller than the number we wanted to skip, there is something wrong in the settings
+ //if( ticks_we_actually_can_skip < ticks_to_skip ){ }
+
+ // Adding to MR0 for this time is not enough, we must also increment the counters ourself artificially
+ for(unsigned int i=0; i < global_step_ticker->stepper_motors.size(); i++){
+ StepperMotor* stepper = global_step_ticker->stepper_motors[i];
+ if( stepper->moving ){ stepper->fx_counter += (uint64_t)((uint64_t)(ticks_we_actually_can_skip)<<32); }
}
+ // When must we have our next MR0 ? ( +1 is here to account that we are actually a legit MR0 too, not only overtime )
+ LPC_TIM0->MR0 = ( ticks_we_actually_can_skip + 1 ) * global_step_ticker->period;
+ //LPC_TIM0->MR0 = ( ticks_to_skip + 1 ) * global_step_ticker->period;
+
+ // This is so that we know how long this computation takes, and we can take it into account next time
+ global_step_ticker->last_duration = LPC_TIM0->TC - start_tc;
+
+
}else{
- // Else obviously it's MR1
- LPC_TIM0->IR |= 1 << 1; // Reset it
- // Reset pins
- global_step_ticker->reset_tick();
+ LPC_TIM0->MR0 = global_step_ticker->period;
}
+
+
if( LPC_TIM0->TC > LPC_TIM0->MR0 ){
- LPC_TIM0->TCR = 3; // Reset
- LPC_TIM0->TCR = 1; // Reset
+ // LPC_TIM0->TCR = 3; // Reset
+ // LPC_TIM0->TCR = 1; // Reset
+ LPC_TIM0->TC = LPC_TIM0->MR0 - 2;
}
+
}
+
// We make a list of steppers that want to be called so that we don't call them for nothing
void StepTicker::add_motor_to_active_list(StepperMotor* motor){
//printf("adding %p with exit: %u \r\n", motor, motor->exit_tick );
// if we are to step now 10t
if( this->fx_counter >= this->fx_ticks_per_step ){
-
+
+
// move counter back 11t
this->fx_counter -= this->fx_ticks_per_step;
// Instruct the StepperMotor to move a certain number of steps
void StepperMotor::move( bool direction, unsigned int steps ){
-
+
//printf("stepper %p moving %u \r\n", this, steps );
// We do not set the direction directly, we will set the pin just before the step pin on the next tick
this->direction_bit = direction;
bool exit_tick;
bool dont_remove_from_active_list_yet;
+
};
kernel->streams->printf("start\r\n");
+ struct SerialMessage message;
+ message.message = "G90";
+ message.stream = kernel->serial;
+ kernel->call_event(ON_CONSOLE_LINE_RECEIVED, &message );
+
+
+ int i = 0;
+ while( i <= 60 ){
+ // Debug : launch file on startup
+
+ message.message = "G1 X20 Y20 F9000";
+ message.stream = kernel->serial;
+ kernel->call_event(ON_CONSOLE_LINE_RECEIVED, &message );
+
+ message.message = "G1 X20 Y20.5 F9000";
+ message.stream = kernel->serial;
+ kernel->call_event(ON_CONSOLE_LINE_RECEIVED, &message );
+
+
+ message.message = "G1 X0 Y0.5 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;
- message.message = "play /sd/laurana.g -q";
+ //message.message = "G1 X1000 F2000";
+ message.message = "play /sd/victor.g -q";
message.stream = kernel->serial;
kernel->call_event(ON_CONSOLE_LINE_RECEIVED, &message );
+ */
while(1){
kernel->call_event(ON_MAIN_LOOP);
// time -->
void Block::calculate_trapezoid( double entryfactor, double exitfactor ){
+ //this->player->kernel->streams->printf("%p calculating trapezoid\r\n", this);
+
this->initial_rate = ceil(this->nominal_rate * entryfactor); // (step/min)
this->final_rate = ceil(this->nominal_rate * exitfactor); // (step/min)
+
+ //this->player->kernel->streams->printf("initrate:%f finalrate:%f\r\n", this->initial_rate, this->final_rate);
+
double acceleration_per_minute = this->rate_delta * this->planner->kernel->stepper->acceleration_ticks_per_second * 60.0; // ( step/min^2)
int accelerate_steps = ceil( this->estimate_acceleration_distance( this->initial_rate, this->nominal_rate, acceleration_per_minute ) );
- int decelerate_steps = ceil( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate, -acceleration_per_minute ) );
+ int decelerate_steps = floor( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate, -acceleration_per_minute ) );
+
// Calculate the size of Plateau of Nominal Rate.
int plateau_steps = this->steps_event_count-accelerate_steps-decelerate_steps;
+
+ //this->player->kernel->streams->printf("accelperminute:%f accelerate_steps:%d decelerate_steps:%d plateau:%d \r\n", acceleration_per_minute, accelerate_steps, decelerate_steps, plateau_steps );
// Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
// have to use intersection_distance() to calculate when to abort acceleration and start braking
this->accelerate_until = accelerate_steps;
this->decelerate_after = accelerate_steps+plateau_steps;
+ //this->debug(this->player->kernel);
+
/*
// TODO: FIX THIS: DIRTY HACK so that we don't end too early for blocks with 0 as final_rate. Doing the math right would be better. Probably fixed in latest grbl
if( this->final_rate < 0.01 ){
// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
// given acceleration:
double Block::estimate_acceleration_distance(double initialrate, double targetrate, double acceleration) {
- return( (targetrate*targetrate-initialrate*initialrate)/(2L*acceleration));
+ return( ((targetrate*targetrate)-(initialrate*initialrate))/(2L*acceleration));
}
// This function gives you the point at which you must start braking (at the rate of -acceleration) if
#include "libs/nuts_bolts.h"
Stepper* stepper;
-double debug_max_reached;
-uint32_t debug_decelerations;
uint32_t previous_step_count;
uint32_t skipped_speed_updates;
+uint32_t speed_ticks_counter;
+
Stepper::Stepper(){
this->current_block = NULL;
void Stepper::on_config_reload(void* argument){
this->acceleration_ticks_per_second = this->kernel->config->value(acceleration_ticks_per_second_checksum)->by_default(100 )->as_number();
- this->minimum_steps_per_minute = this->kernel->config->value(minimum_steps_per_minute_checksum )->by_default(1200 )->as_number();
+ this->minimum_steps_per_minute = this->kernel->config->value(minimum_steps_per_minute_checksum )->by_default(3000 )->as_number();
// Steppers start on by default
this->turn_enable_pins_on();
void Stepper::on_block_begin(void* argument){
Block* block = static_cast<Block*>(argument);
- //if( block->initial_rate < 0.1 ){ LPC_GPIO1->FIOCLR = 1<<18; }else{ LPC_GPIO1->FIOSET = 1<<18; }
- //if( block->final_rate < 0.1 ){ LPC_GPIO1->FIOCLR = 1<<19; }else{ LPC_GPIO1->FIOSET = 1<<19; }
-
-
-
-
// The stepper does not care about 0-blocks
if( block->millimeters == 0.0 ){ return; }
// Mark the new block as of interrest to us
if( block->steps[ALPHA_STEPPER] > 0 || block->steps[BETA_STEPPER] > 0 || block->steps[GAMMA_STEPPER] > 0 ){
block->take();
- //printf("taken for stepper: %u %u %u\r\n", block->steps[ALPHA_STEPPER], block->steps[BETA_STEPPER], block->steps[GAMMA_STEPPER]);
}else{
return;
}
-
+
+ //block->debug(this->kernel);
+
+ LPC_GPIO1->FIOCLR = 1<<18;
+
// We can't move with the enable pins off
if( this->enable_pins_status == false ){
this->turn_enable_pins_on();
// Current block is discarded
void Stepper::on_block_end(void* argument){
Block* block = static_cast<Block*>(argument);
- this->current_block = NULL; //stfu !
- /*
- bool error = false;
- for(unsigned int i=0; i < this->kernel->step_ticker->stepper_motors.size(); i++){
- StepperMotor* stepper = this->kernel->step_ticker->stepper_motors[i];
- if( stepper->stepped != stepper->steps_to_move ){
- error = true;
- }
- }
-
- if( error ){
-
- this->kernel->streams->printf("be(%u):",this->kernel->step_ticker->stepper_motors.size());
-
- for(unsigned int i=0; i < this->kernel->step_ticker->stepper_motors.size(); i++){
- StepperMotor* stepper = this->kernel->step_ticker->stepper_motors[i];
- this->kernel->streams->printf("[%1u:%5u/%5u]", stepper->moving, stepper->stepped, stepper->steps_to_move);
- if( stepper->stepped != stepper->steps_to_move ){
- this->kernel->streams->printf("\r\nerror\r\n");
- while(1){
- for(unsigned int j=0; j<0xffff;j++){
- j++;
- }
- }
- }
- }
-
- printf("\r\n");
-
- }
- */
+ this->current_block = NULL; //stfu !
}
// This block is finished, release it
if( this->current_block != NULL ){
+
+ LPC_GPIO1->FIOSET = 1<<18;
+
this->current_block->release();
+
+
}
}
// 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 ) {
-
- uint32_t current_steps_completed = this->main_stepper->stepped;
-
- 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;
- }else{
- previous_step_count = current_steps_completed;
- }
-
- double previous_rate = this->trapezoid_adjusted_rate;
-
- if( this->force_speed_update ){
- this->force_speed_update = false;
- this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
- }
- if(this->current_block && !this->paused ) {
- if(current_steps_completed < this->current_block->accelerate_until) {
+ 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 ){
+ // We must skip this step update because no step has happened
+ skipped_speed_updates++;
+ return 0;
+ }else{
+ previous_step_count = current_steps_completed;
+ }
+
+ if( this->force_speed_update ){
+ this->force_speed_update = false;
+ this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
+ }
+
+ if(current_steps_completed < this->current_block->accelerate_until) {
+
+
+
+ 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 );
- skipped_speed_updates = 0;
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->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) {
- uint32_t before = this->trapezoid_adjusted_rate;
+
+ 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 );
- debug_decelerations += 1 + skipped_speed_updates;
- skipped_speed_updates = 0;
- 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("{%u>=%u} tar:%f im:%u sps:%f pos:%u/%u cnt:%u.%u/%u.%u before:%u \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_counter>>32), (uint32_t)((this->main_stepper->fx_counter<<32)>>32), (uint32_t)(this->main_stepper->fx_ticks_per_step>>32), (uint32_t)((this->main_stepper->fx_ticks_per_step<<32)>>32), before );
- LPC_TIM0->TC = start_tc0;
- LPC_TIM2->TC = start_tc2;
- __enable_irq();
- }
-
}else{
- this->trapezoid_adjusted_rate = this->current_block->rate_delta * 1.5;
+ //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 = floor(double(this->trapezoid_adjusted_rate / 2 ));
}
if(this->trapezoid_adjusted_rate < this->current_block->final_rate ) {
+ // this->kernel->streams->printf("final 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->final_rate;
- }
- if( this->trapezoid_adjusted_rate < 1000 && 0 ){
- __disable_irq();
- uint32_t start_tc0 = LPC_TIM0->TC;
- uint32_t start_tc2 = LPC_TIM2->TC;
- this->kernel->streams->printf("tar:%f (was:%f) c:%u{%u}/stm:%u [au:%u da:%u] [ir:%u nr:%u fr:%u] rd:%f mm:%f steps:%u|%u|%u debug: mr:%f decels:%u \r\n",this->trapezoid_adjusted_rate, previous_rate, this->main_stepper->stepped, current_steps_completed, this->main_stepper->steps_to_move , this->current_block->accelerate_until, this->current_block->decelerate_after, this->current_block->initial_rate, this->current_block->nominal_rate, this->current_block->final_rate, this->current_block->rate_delta, this->current_block->millimeters, this->current_block->steps[ALPHA_STEPPER], this->current_block->steps[BETA_STEPPER], this->current_block->steps[GAMMA_STEPPER], debug_max_reached, debug_decelerations );
- LPC_TIM0->TC = start_tc0;
- LPC_TIM2->TC = start_tc2;
- __enable_irq();
- }
+ }
this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
}else {
// Make sure we cruise at exactly 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;
}
}
- /*
- //LPC_GPIO1->FIOCLR = 1<<20;
- if( this->trapezoid_adjusted_rate == this->current_block->initial_rate ){ LPC_GPIO1->FIOSET = 1<<18; }else{ LPC_GPIO1->FIOCLR = 1<<18; }
- if( this->trapezoid_adjusted_rate == this->current_block->nominal_rate ){ LPC_GPIO1->FIOSET = 1<<19; }else{ LPC_GPIO1->FIOCLR = 1<<19; }
- if( this->trapezoid_adjusted_rate == this->current_block->final_rate ){ LPC_GPIO1->FIOSET = 1<<20; }else{ LPC_GPIO1->FIOCLR = 1<<20; }
- //LPC_GPIO1->FIOSET = 1<<20;
- */
-
+ skipped_speed_updates = 0;
}
// Initializes the trapezoid generator from the current block. Called whenever a new
this->trapezoid_adjusted_rate = this->current_block->initial_rate;
this->force_speed_update = true;
this->trapezoid_tick_cycle_counter = 0;
- debug_decelerations = 0;
- debug_max_reached = 0;
previous_step_count = 0;
skipped_speed_updates = 0;
+ speed_ticks_counter = 0;
}
// Update the speed for all steppers
void Stepper::set_step_events_per_minute( double steps_per_minute ){
- debug_max_reached = max(steps_per_minute,debug_max_reached);
-
// We do not step slower than this
- steps_per_minute = max(steps_per_minute, this->minimum_steps_per_minute);
+ //steps_per_minute = max(steps_per_minute, this->minimum_steps_per_minute);
+ if( steps_per_minute < this->minimum_steps_per_minute ){
+ steps_per_minute = this->minimum_steps_per_minute;
+ }
- if( steps_per_minute < this->minimum_steps_per_minute * 1.1 ){ LPC_GPIO1->FIOSET = 1<<21; }else{ LPC_GPIO1->FIOCLR = 1<<21; }
// Instruct the stepper motors
if( this->kernel->robot->alpha_stepper_motor->moving ){ this->kernel->robot->alpha_stepper_motor->set_speed( (steps_per_minute/60L) * ( (double)this->current_block->steps[ALPHA_STEPPER] / (double)this->current_block->steps_event_count ) ); }
// 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 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( (this->kernel->stepper->trapezoid_adjusted_rate/60L) * ( (double)this->stepper_motor->steps_to_move / (double)this->current_block->steps_event_count ) );
+ this->stepper_motor->set_speed( (adjusted_rate/60L) * ( (double)this->stepper_motor->steps_to_move / (double)this->current_block->steps_event_count ) );
}
HCoop / clinton / Smoothieware.git / commitdiff