if (c == '\n'){
// We have a new line
if( buffer[0] == '#' ){ buffer.clear(); continue; } // Ignore comments
+ if( buffer.length() < 3 ){ buffer.clear(); continue; } //Ignore empty lines
size_t begin_key = buffer.find_first_not_of(" ");
size_t begin_value = buffer.find_first_not_of(" ", buffer.find_first_of(" ", begin_key));
// If this line matches the checksum
kernel->serial->printf("Smoothie ( grbl port ) version 0.2 \r\nstart\r\n");
kernel->add_module( new Laser(p21) );
- kernel->add_module( new Extruder(p22) );
+ kernel->add_module( new Extruder(p26) );
kernel->add_module( new SimpleShell() );
- kernel->add_module( new Pauser(p29,p30) );
+ //kernel->add_module( new Pauser(p29,p30) );
while(1){
kernel->call_event(ON_MAIN_LOOP);
#include "libs/Module.h"
#include "libs/Kernel.h"
#include "utils/Gcode.h"
+#include "libs/nuts_bolts.h"
#include "GcodeDispatch.h"
GcodeDispatch::GcodeDispatch(){}
string possible_command = *static_cast<string*>(line);
char first_char = possible_command[0];
if( first_char == 'G' || first_char == 'M' || first_char == 'T' || first_char == 'S' ){
+
+ //Remove comments
+ size_t comment = possible_command.find_first_of(";");
+ if( comment != string::npos ){ possible_command = possible_command.substr(0, comment); }
+
Gcode gcode = Gcode();
gcode.command = possible_command;
this->kernel->call_event(ON_GCODE_RECEIVED, &gcode );
this->kernel->serial->printf("ok\r\n");
+ }else if( first_char == ';' || first_char == '(' ){
+ this->kernel->serial->printf("ok\r\n");
}
}
// Actual event calling must happen in the main loop because if it happens in the interrupt we will loose data
void SerialConsole::on_main_loop(void * argument){
- if( this->received_lines.size() < 1 ){ return; }
- this->kernel->call_event(ON_CONSOLE_LINE_RECEIVED, &this->received_lines.back() );
- this->received_lines.pop_back();
+ if( this->received_lines.size() < 1 ){ return; }
+ this->kernel->call_event(ON_CONSOLE_LINE_RECEIVED, &this->received_lines.back() );
+ this->received_lines.pop_back();
}
-
double get_value ( char letter );
string command;
+ double millimeters_of_travel;
+ bool call_on_gcode_execute_event_immediatly;
+ bool on_gcode_execute_event_called;
};
#endif
// Gcodes are attached to their respective blocks so that on_gcode_execute can be called with it
void Block::append_gcode(Gcode* gcode){
this->commands.push_back(gcode->command);
+ this->travel_distances.push_back(gcode->millimeters_of_travel);
}
// 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){
for(unsigned short index=0; index<this->commands.size(); index++){
string command = this->commands.at(index);
+ double distance = this->travel_distances.at(index);
Gcode gcode = Gcode();
gcode.command = command;
+ gcode.millimeters_of_travel = distance;
kernel->call_event(ON_GCODE_EXECUTE, &gcode );
}
}
void append_gcode(Gcode* gcode);
void pop_and_execute_gcode(Kernel* &kernel);
double get_duration_left(unsigned int already_taken_steps);
- vector<std::string> commands;
+ vector<std::string> commands;
+ vector<double> travel_distances;
+
unsigned int steps[3]; // Number of steps for each axis for this block
unsigned int steps_event_count; // Steps for the longest axis
unsigned int nominal_rate; // Nominal rate in steps per minute
void Planner::append_block( int target[], double feed_rate, double distance, double deltas[] ){
// Do not append block with no movement
- if( target[ALPHA_STEPPER] == this->position[ALPHA_STEPPER] && target[BETA_STEPPER] == this->position[BETA_STEPPER] && target[GAMMA_STEPPER] == this->position[GAMMA_STEPPER] ){ this->computing = false; return; }
+ //if( target[ALPHA_STEPPER] == this->position[ALPHA_STEPPER] && target[BETA_STEPPER] == this->position[BETA_STEPPER] && target[GAMMA_STEPPER] == this->position[GAMMA_STEPPER] ){ this->computing = false; return; }
// Stall here if the queue is ful
while( this->queue.size() >= this->queue.capacity() ){ wait_us(100); }
if( block->planner == this ){
for(short index=0; index<block->commands.size(); index++){
block->commands.pop_back();
+ block->travel_distances.pop_back();
}
}
// Max number of steps, for all axes
block->steps_event_count = max( block->steps[ALPHA_STEPPER], max( block->steps[BETA_STEPPER], block->steps[GAMMA_STEPPER] ) );
- if( block->steps_event_count == 0 ){ this->computing = false; return; }
+ //if( block->steps_event_count == 0 ){ this->computing = false; return; }
block->millimeters = distance;
- double inverse_millimeters = 1.0/distance;
+ double inverse_millimeters = 0;
+ if( distance > 0 ){ inverse_millimeters = 1.0/distance; }
// Calculate speed in mm/minute for each axis. No divide by zero due to previous checks.
// NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c
double inverse_minute = feed_rate * inverse_millimeters;
- block->nominal_speed = block->millimeters * inverse_minute; // (mm/min) Always > 0
- block->nominal_rate = ceil(block->steps_event_count * inverse_minute); // (step/min) Always > 0
+ if( distance > 0 ){
+ block->nominal_speed = block->millimeters * inverse_minute; // (mm/min) Always > 0
+ block->nominal_rate = ceil(block->steps_event_count * inverse_minute); // (step/min) Always > 0
+ }else{
+ block->nominal_speed = 0;
+ block->nominal_rate = 0;
+ }
//this->kernel->serial->printf("nom_speed: %f nom_rate: %u step_event_count: %u block->steps_z: %u \r\n", block->nominal_speed, block->nominal_rate, block->steps_event_count, block->steps[2] );
else { block->nominal_length_flag = false; }
block->recalculate_flag = true; // Always calculate trapezoid for new block
-
// Update previous path unit_vector and nominal speed
memcpy(this->previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[]
this->previous_nominal_speed = block->nominal_speed;
//A GCode has been received
void Robot::on_gcode_received(void * argument){
Gcode* gcode = static_cast<Gcode*>(argument);
- this->kernel->planner->attach_gcode_to_queue(gcode);
- this->execute_gcode(gcode);
+ gcode->call_on_gcode_execute_event_immediatly = false;
+ gcode->on_gcode_execute_event_called = false;
+
+ //If the queue is empty, execute immediatly, otherwise attach to the last added block
+ if( this->kernel->planner->queue.size() == 0 ){
+ gcode->call_on_gcode_execute_event_immediatly = true;
+ this->execute_gcode(gcode);
+ if( gcode->on_gcode_execute_event_called == false ){
+ this->kernel->call_event(ON_GCODE_EXECUTE, gcode );
+ }
+ }else{
+ Block* block = this->kernel->planner->queue.get_ref( this->kernel->planner->queue.size() - 1 );
+ block->append_gcode(gcode);
+ this->execute_gcode(gcode);
+ }
+
+
}
+
//See if the current Gcode line has some orders for us
void Robot::execute_gcode(Gcode* gcode){
case NEXT_ACTION_DEFAULT:
switch(this->motion_mode){
case MOTION_MODE_CANCEL: break;
- case MOTION_MODE_SEEK : this->append_line(target, this->seek_rate ); break;
- case MOTION_MODE_LINEAR: this->append_line(target, this->feed_rate ); break;
- case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: this->compute_arc(offset, target ); break;
+ case MOTION_MODE_SEEK : this->append_line(gcode, target, this->seek_rate ); break;
+ case MOTION_MODE_LINEAR: this->append_line(gcode, target, this->feed_rate ); break;
+ case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: this->compute_arc(gcode, offset, target ); break;
}
break;
}
double millimeters_of_travel = sqrt( pow( deltas[X_AXIS], 2 ) + pow( deltas[Y_AXIS], 2 ) + pow( deltas[Z_AXIS], 2 ) );
- if( millimeters_of_travel < 0.001 ){ return; }
- double duration = millimeters_of_travel / rate;
+ //if( millimeters_of_travel < 0.001 ){ return; }
+ //double duration = millimeters_of_travel / rate;
+ double duration = 0;
+ if( rate > 0 ){ duration = millimeters_of_travel / rate; }
for(int axis=X_AXIS;axis<=Z_AXIS;axis++){
if( this->max_speeds[axis] > 0 ){
double axis_speed = ( fabs(deltas[axis]) / ( millimeters_of_travel / rate )) * 60;
- rate = rate * ( this->max_speeds[axis] / axis_speed );
+ if( axis_speed > this->max_speeds[axis] ){
+ rate = rate * ( this->max_speeds[axis] / axis_speed );
+ }
}
}
-
//this->kernel->serial->printf("dur: %f mm: %f rate: %f target_z: %f steps_z: %d deltas_z: %f \r\n", duration, millimeters_of_travel, rate, target[2], steps[2], deltas[2] );
this->kernel->planner->append_block( steps, rate*60, millimeters_of_travel, deltas );
}
-void Robot::append_line(double target[], double rate ){
+void Robot::append_line(Gcode* gcode, double target[], double rate ){
// We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
// In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
- double millimeters_of_travel = sqrt( pow( target[X_AXIS]-this->current_position[X_AXIS], 2 ) + pow( target[Y_AXIS]-this->current_position[Y_AXIS], 2 ) + pow( target[Z_AXIS]-this->current_position[Z_AXIS], 2 ) );
- if( millimeters_of_travel == 0.0 ){ return; }
+ gcode->millimeters_of_travel = sqrt( pow( target[X_AXIS]-this->current_position[X_AXIS], 2 ) + pow( target[Y_AXIS]-this->current_position[Y_AXIS], 2 ) + pow( target[Z_AXIS]-this->current_position[Z_AXIS], 2 ) );
- uint16_t segments = ceil( millimeters_of_travel/ this->mm_per_line_segment);
+ if( gcode->call_on_gcode_execute_event_immediatly == true ){
+ this->kernel->call_event(ON_GCODE_EXECUTE, gcode );
+ gcode->on_gcode_execute_event_called = true;
+ }
+
+ if (gcode->millimeters_of_travel == 0.0) {
+ this->append_milestone(this->current_position, 0.0);
+ return;
+ }
+
+ uint16_t segments = ceil( gcode->millimeters_of_travel/ this->mm_per_line_segment);
// A vector to keep track of the endpoint of each segment
double temp_target[3];
//Initialize axes
}
-void Robot::append_arc( double target[], double offset[], double radius, bool is_clockwise ){
+void Robot::append_arc(Gcode* gcode, double target[], double offset[], double radius, bool is_clockwise ){
double center_axis0 = this->current_position[this->plane_axis_0] + offset[this->plane_axis_0];
double center_axis1 = this->current_position[this->plane_axis_1] + offset[this->plane_axis_1];
if (angular_travel < 0) { angular_travel += 2*M_PI; }
if (is_clockwise) { angular_travel -= 2*M_PI; }
- double millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
- if (millimeters_of_travel == 0.0) { return; }
- uint16_t segments = floor(millimeters_of_travel/this->mm_per_arc_segment);
+ gcode->millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
+
+ if( gcode->call_on_gcode_execute_event_immediatly == true ){
+ this->kernel->call_event(ON_GCODE_EXECUTE, gcode );
+ gcode->on_gcode_execute_event_called = true;
+ }
+
+ if (gcode->millimeters_of_travel == 0.0) {
+ this->append_milestone(this->current_position, 0.0);
+ return;
+ }
+
+ uint16_t segments = floor(gcode->millimeters_of_travel/this->mm_per_arc_segment);
double theta_per_segment = angular_travel/segments;
double linear_per_segment = linear_travel/segments;
}
-void Robot::compute_arc(double offset[], double target[]){
+void Robot::compute_arc(Gcode* gcode, double offset[], double target[]){
// Find the radius
double radius = hypot(offset[this->plane_axis_0], offset[this->plane_axis_1]);
if( this->motion_mode == MOTION_MODE_CW_ARC ){ is_clockwise = true; }
// Append arc
- this->append_arc( target, offset, radius, is_clockwise );
+ this->append_arc(gcode, target, offset, radius, is_clockwise );
}
void on_gcode_received(void* argument);
void execute_gcode(Gcode* gcode);
void append_milestone( double target[], double feed_rate);
- void append_line( double target[], double feed_rate);
+ void append_line( Gcode* gcode, double target[], double feed_rate);
//void append_arc(double theta_start, double angular_travel, double radius, double depth, double rate);
- void append_arc( double target[], double offset[], double radius, bool is_clockwise );
+ void append_arc( Gcode* gcode, double target[], double offset[], double radius, bool is_clockwise );
- void compute_arc(double offset[], double target[]);
+ void compute_arc(Gcode* gcode, double offset[], double target[]);
double to_millimeters(double value);
double theta(double x, double y);
void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2);
this->step_gpio_port->FIOSET = ( this->out_bits ^ this->step_invert_mask ) & this->step_mask;
this->step_gpio_port->FIOCLR = ( ~ ( this->out_bits ^ this->step_invert_mask ) ) & this->step_mask;
+
// If there is no current block, attempt to pop one from the buffer, if there is none, go to sleep, if there is none, go to sleep
if( this->current_block == NULL ){
this->current_block = this->kernel->planner->get_current_block();
if( this->current_block != NULL ){
if( this->current_block->computed == false ){
this->current_block = NULL;
- }else{
+ }else{
this->trapezoid_generator_reset();
this->update_offsets();
for( int stpr=ALPHA_STEPPER; stpr<=GAMMA_STEPPER; stpr++){ this->counters[stpr] = 0; this->stepped[stpr] = 0; }
}
}
-Extruder::Extruder(PinName stppin) : step_pin(stppin){}
+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() ){ return; }
+ if( this->kernel->config->value( extruder_module_enable_checksum )->by_default(false)->as_bool() == false ){ return; }
extruder_for_irq = 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);
}
void Extruder::on_config_reload(void* argument){
- this->microseconds_per_step_pulse = this->kernel->config->value(microseconds_per_step_pulse_ckeckusm)->by_default(5)->as_number();
+ 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);
+
+ // 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; }
+ }
+
+ // Extrusion length
+ if( gcode->has_letter('E' )){
+ double extrusion_distance = gcode->get_value('E');
+ //this->kernel->serial->printf("extrusion_distance: %f, millimeters_of_travel: %f\r\n", extrusion_distance, gcode->millimeters_of_travel);
+ if( gcode->millimeters_of_travel == 0.0 ){
+ this->solo_mode = true;
+ this->travel_distance = extrusion_distance;
+ //this->kernel->serial->printf("solo mode distance: %f\r\n", this->travel_distance );
+ }else{
+ this->solo_mode = false;
+ this->travel_ratio = extrusion_distance / gcode->millimeters_of_travel;
+ //this->kernel->serial->printf("follow mode ratio: %f\r\n", this->travel_ratio);
+ }
+ }else{
+ this->travel_ratio = 0;
+ }
}
+
+
void Extruder::on_block_begin(void* argument){
Block* block = static_cast<Block*>(argument);
this->current_block = block;
+ //this->kernel->serial->printf("block: solomode: %d, travel_distance: %f, travel_ratio: %f \r\n", this->solo_mode, this->travel_distance, this->travel_ratio);
this->start_position = this->current_position;
- this->target_position = this->start_position + ( this->current_block->millimeters * 159 ); //TODO : Get from config ( extruder_steps_per_mm )
+ this->target_position = this->start_position + ( this->current_block->millimeters * this->travel_ratio * 159 ); //TODO : Get from config ( extruder_steps_per_mm )
+ this->acceleration_tick();
}
void Extruder::on_block_end(void* argument){
if( this->current_block ){
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
- double next_position_ratio = ( this->kernel->stepper->step_events_completed + steps_by_acceleration_tick ) / this->kernel->stepper->current_block->steps_event_count;
+ 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 = ( (this->kernel->stepper->step_events_completed>>16) + steps_by_acceleration_tick ) / this->kernel->stepper->current_block->steps_event_count;
// Get wanted next position
double next_absolute_position = ( this->target_position - this->start_position ) * next_position_ratio;
// Get desired speed in steps per minute to get to the next position by the next acceleration tick
- double desired_speed = ( ( next_absolute_position + this->start_position ) - this->current_position ) * double(this->kernel->stepper->acceleration_ticks_per_second) * 60L; //TODO : Replace with the actual current_position
+ double desired_speed = ( ( next_absolute_position + this->start_position ) - this->current_position ) * double(this->kernel->stepper->acceleration_ticks_per_second); //TODO : Replace with the actual current_position
+
+ //this->kernel->serial->printf("stp->sec: %d, stp->cb->secnt: %u, sbat: %.4f, npr: %.4f, ds: %f --> tap: %u, nap: %f, sp: %u, cp: %u, (tp-sp): %d, cpr: %f\r\n", this->kernel->stepper->step_events_completed>>16, this->kernel->stepper->current_block->steps_event_count, steps_by_acceleration_tick, next_position_ratio, desired_speed,this->target_position, next_absolute_position, this->start_position, this->current_position, int(this->target_position-this->start_position), current_position_ratio );
+
+ if( desired_speed <= 0 ){ return; }
// Set timer
- LPC_TIM1->MR0 = ((SystemCoreClock/4)*60)/int(floor(desired_speed));
+ LPC_TIM1->MR0 = ((SystemCoreClock/4))/int(floor(desired_speed));
// In case we are trying to set the timer to a limit it has already past by
if( LPC_TIM1->TC >= LPC_TIM1->MR0 ){
#include "modules/robot/Block.h"
#define microseconds_per_step_pulse_ckeckusm 42333
-#define extruder_module_enable_checksum 31452
+#define extruder_module_enable_checksum 6183
class Extruder : public Module{
public:
Extruder(PinName stppin);
void on_module_loaded();
void on_config_reload(void* argument);
+ void on_gcode_execute(void* argument);
void on_block_begin(void* argument);
void on_block_end(void* argument);
void acceleration_tick();
Block* current_block; // Current block we are stepping, same as Stepper's one
int microseconds_per_step_pulse; // Pulse duration for step pulses
+ bool solo_mode;
+ double travel_ratio;
+ double travel_distance;
+ bool absolute_mode;
};
#endif
}
void Laser::on_module_loaded() {
- if( this->kernel->config->value( laser_module_enable_checksum )->by_default(false)->as_bool() ){ return; }
+ if( !this->kernel->config->value( laser_module_enable_checksum )->by_default(false)->as_bool() ){ return; }
this->register_for_event(ON_GCODE_EXECUTE);
this->register_for_event(ON_SPEED_CHANGE);
this->register_for_event(ON_PLAY);
this->set_proportional_power();
}
-
// Turn laser on/off depending on received GCodes
void Laser::on_gcode_execute(void* argument){
Gcode* gcode = static_cast<Gcode*>(argument);
}
}
+
void Laser::on_speed_change(void* argument){
this->set_proportional_power();
}
HCoop / clinton / Smoothieware.git / commitdiff