// The attached gcodes are then poped and the on_gcode_execute event is called with them as a parameter
void Block::pop_and_execute_gcode(Kernel* &kernel){
- while( this->commands.size() > 0 ){
- string command = this->commands.back();
+ for(unsigned short index=0; index<this->commands.size(); index++){
+ string command = this->commands.at(index);
Gcode gcode = Gcode();
gcode.command = command;
kernel->call_event(ON_GCODE_EXECUTE, &gcode );
- this->commands.pop_back();
}
}
unsigned int rate_delta; // Nomber of steps to add to the speed for each acceleration tick
unsigned int initial_rate; // Initial speed in steps per minute
unsigned int final_rate; // Final speed in steps per minute
- unsigned short accelerate_until; // Stop accelerating after this number of steps
- unsigned short decelerate_after; // Start decelerating after this number of steps
+ unsigned int accelerate_until; // Stop accelerating after this number of steps
+ unsigned int decelerate_after; // Start decelerating after this number of steps
unsigned int direction_bits; // Direction for each axis in bit form, relative to the direction port's mask
// Stall here if the queue is ful
while( this->queue.size() >= this->queue.capacity() ){ wait_us(100); }
-
+
+ // Clean up the vector of commands in the block we are about to replace
+ // It is quite strange to do this here, we really should do it inside Block->pop_and_execute_gcode
+ // but that function is called inside an interrupt and thus can break everything if the interrupt was trigerred during a memory access
+ Block* block = this->queue.get_ref( this->queue.size()-1 );
+ if( block->planner == this ){
+ for(short index=0; index<block->commands.size(); index++){
+ block->commands.pop_back();
+ }
+ }
+
// Add/get a new block from the queue
this->queue.push_back(Block());
- Block* block = this->queue.get_ref( this->queue.size()-1 );
+ block = this->queue.get_ref( this->queue.size()-1 );
block->planner = this;
this->computing = true; //TODO: Check if this is necessary
int position[3]; // Current position, in steps
double previous_unit_vec[3];
- RingBuffer<Block,128> queue; // Queue of Blocks
+ RingBuffer<Block,32> queue; // Queue of Blocks
bool computing; // Whether or not we are currently computing the queue, TODO: Checks if this is necessary
Block last_deleted_block; // Item -1 in the queue, TODO: Grbl does not need this, but Smoothie won't work without it, we are probably doing something wrong
bool has_deleted_block; // Flag for above value
//A GCode has been received
void Robot::on_gcode_received(void * argument){
Gcode* gcode = static_cast<Gcode*>(argument);
- this->execute_gcode(gcode);
this->kernel->planner->attach_gcode_to_queue(gcode);
+ this->execute_gcode(gcode);
}
//See if the current Gcode line has some orders for us
this->gamma_dir_pin = this->kernel->config->get(gamma_dir_pin_checksum );
this->step_mask = ( 1 << this->alpha_step_pin ) + ( 1 << this->beta_step_pin ) + ( 1 << this->gamma_step_pin );
this->dir_mask = ( 1 << this->alpha_dir_pin ) + ( 1 << this->beta_dir_pin ) + ( 1 << this->gamma_dir_pin );
+ this->step_bits[ALPHA_STEPPER ] = this->alpha_step_pin;
+ this->step_bits[BETA_STEPPER ] = this->beta_step_pin;
+ this->step_bits[GAMMA_STEPPER ] = this->gamma_step_pin;
}
// When the play/pause button is set to pause, or a module calls the ON_PAUSE event
for( int stpr=ALPHA_STEPPER; stpr<=GAMMA_STEPPER; stpr++){ this->counters[stpr] = 0; this->stepped[stpr] = 0; }
this->step_events_completed = 0;
this->kernel->call_event(ON_BLOCK_BEGIN, this->current_block);
- //this->kernel->planner->dump_queue();
}else{
// Go to sleep
//LPC_TIM0->MR0 = 10000;
//LPC_TIM0->TCR = 0;
}
}
-
+
if( this->current_block != NULL ){
// Set bits for direction and steps
this->out_bits = this->current_block->direction_bits;
- unsigned short step_bits[3] = {this->alpha_step_pin, this->beta_step_pin, this->gamma_step_pin};
for( int stpr=ALPHA_STEPPER; stpr<=GAMMA_STEPPER; stpr++){
- this->counters[stpr] += (1<<16)>>this->divider; // Basically += 1/divider if we were in floating point arythmetic, but here the counters precision is 1/(2^16).
+ this->counters[stpr] += this->counter_increment; // (1<<16)>>this->divider; // Basically += 1/divider if we were in floating point arythmetic, but here the counters precision is 1/(2^16).
if( this->counters[stpr] > this->offsets[stpr] && this->stepped[stpr] < this->current_block->steps[stpr] ){
this->counters[stpr] -= this->offsets[stpr] ;
this->stepped[stpr]++;
- this->out_bits |= (1 << step_bits[stpr]);
+ this->out_bits |= (1 << this->step_bits[stpr]);
}
}
-
// If current block is finished, reset pointer
- this->step_events_completed += (1<<16)>>this->divider; // /this->divider;
+ this->step_events_completed += this->counter_increment; // (1<<16)>>this->divider; // /this->divider;
if( this->step_events_completed >= this->current_block->steps_event_count<<16 ){
if( this->stepped[ALPHA_STEPPER] == this->current_block->steps[ALPHA_STEPPER] && this->stepped[BETA_STEPPER] == this->current_block->steps[BETA_STEPPER] && this->stepped[GAMMA_STEPPER] == this->current_block->steps[GAMMA_STEPPER] ){
this->kernel->call_event(ON_BLOCK_END, this->current_block);
}else{
this->out_bits = 0;
}
-
}
void Stepper::update_offsets(){
// 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.
void Stepper::trapezoid_generator_tick() {
- if(this->current_block) {
+ if(this->current_block && !this->kernel->planner->computing ) {
if(this->step_events_completed < this->current_block->accelerate_until<<16) {
this->trapezoid_adjusted_rate += this->current_block->rate_delta;
if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) {
if (this->trapezoid_adjusted_rate > this->current_block->rate_delta) {
this->trapezoid_adjusted_rate -= this->current_block->rate_delta;
}
- if (this->trapezoid_adjusted_rate < this->current_block->final_rate) {
+ if (this->trapezoid_adjusted_rate < this->current_block->final_rate ) {
this->trapezoid_adjusted_rate = this->current_block->final_rate;
}
this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
// The speed factor is the factor by which we must multiply the minimal step frequency to reach the maximum step frequency
// The higher, the smoother the movement will be, but the closer the maximum and minimum frequencies are, the smaller the factor is
double speed_factor = this->base_stepping_frequency / (steps_per_minute/60L); //TODO: Get from config
- if( speed_factor < 1 ){ speed_factor==1; }
-
- // Find bit corresponding to the biggest power of two that is smaller than the divider we want
- // This is used instead of a floating point number for performance reasons
- int i=0;
- while(1<<i+1 < speed_factor){ i++; } // Probably not optimal ...
- this->divider = i;
+ if( speed_factor < 1 ){ speed_factor=1; }
+ this->counter_increment = int(floor((1<<16)/speed_factor));
// Set the Timer interval
- LPC_TIM0->MR0 = floor( ( SystemCoreClock/4 ) / ( (steps_per_minute/60L) * (1<<this->divider) ) );
-
+ LPC_TIM0->MR0 = floor( ( SystemCoreClock/4 ) / ( (steps_per_minute/60L) * speed_factor ) );
+
// In case we change the Match Register to a value the Timer Counter has past
if( LPC_TIM0->TC >= LPC_TIM0->MR0 ){ LPC_TIM0->TCR = 3; LPC_TIM0->TCR = 1; }
unsigned short gamma_dir_pin;
unsigned int step_mask;
unsigned int dir_mask;
+ unsigned short step_bits[3];
+ int counter_increment;
};
#include "modules/communication/utils/Gcode.h"
#include "modules/robot/Stepper.h"
#include "Laser.h"
+#include "libs/nuts_bolts.h"
Laser::Laser(PinName pin) : laser_pin(pin){
this->laser_pin.period_us(10);
this->register_for_event(ON_SPEED_CHANGE);
this->register_for_event(ON_PLAY);
this->register_for_event(ON_PAUSE);
+ this->register_for_event(ON_BLOCK_BEGIN);
+ this->register_for_event(ON_BLOCK_END);
}
+// Turn laser off laser at the end of a move
+void Laser::on_block_end(void* argument){
+ this->laser_pin = 0;
+ //this->laser_on = false;
+}
+
+// Set laser power at the beginning of a block
+void Laser::on_block_begin(void* argument){
+ this->set_proportional_power();
+}
// When the play/pause button is set to pause, or a module calls the ON_PAUSE event
void Laser::on_pause(void* argument){
// When the play/pause button is set to play, or a module calls the ON_PLAY event
void Laser::on_play(void* argument){
+ this->set_proportional_power();
}
// Turn laser on/off depending on received GCodes
void Laser::on_gcode_execute(void* argument){
+ dd(10);
Gcode* gcode = static_cast<Gcode*>(argument);
+ dd(11);
if( gcode->has_letter('G' )){
+ dd(21);
int code = gcode->get_value('G');
- if( code == 0 ){ // G0
+ if( code == 0 ){ // G0
this->laser_pin = 0;
this->laser_on = false;
- }else if( code > 0 && code < 4 ){ // G1, G2, G3
+ }else if( code >= 1 && code <= 3 ){ // G1, G2, G3
this->laser_on = true;
}
}
+ dd(12);
}
void Laser::on_speed_change(void* argument){
- Stepper* stepper = static_cast<Stepper*>(argument);
- if( this->laser_on ){
- this->laser_pin = double(stepper->trapezoid_adjusted_rate)/double(stepper->current_block->nominal_rate);
+ this->set_proportional_power();
+}
+
+void Laser::set_proportional_power(){
+ if( this->laser_on && this->kernel->stepper->current_block ){
+ this->laser_pin = double(this->kernel->stepper->trapezoid_adjusted_rate)/double(this->kernel->stepper->current_block->nominal_rate);
}
}
public:
Laser(PinName pin);
void on_module_loaded();
+ void on_block_end(void* argument);
+ void on_block_begin(void* argument);
void on_play(void* argument);
void on_pause(void* argument);
void on_gcode_execute(void* argument);
void on_speed_change(void* argument);
+ void set_proportional_power();
PwmOut laser_pin; // PWM output to regulate the laser power
bool laser_on; // Laser status
HCoop / clinton / Smoothieware.git / commitdiff