#include "libs/Module.h"
#include "libs/Kernel.h"
#include "modules/communication/utils/Gcode.h"
-#include "modules/robot/Player.h"
+#include "modules/robot/Conveyor.h"
#include "Endstops.h"
#include "libs/nuts_bolts.h"
#include "libs/Pin.h"
}
void Endstops::on_module_loaded() {
+ // Do not do anything if not enabled
+ if( this->kernel->config->value( endstops_module_enable_checksum )->by_default(true)->as_bool() == false ){ return; }
+
+ register_for_event(ON_CONFIG_RELOAD);
this->register_for_event(ON_GCODE_RECEIVED);
// Take StepperMotor objects from Robot and keep them here
// Settings
this->on_config_reload(this);
-
+
}
+//#pragma GCC push_options
+//#pragma GCC optimize ("O0")
+
// Get config
void Endstops::on_config_reload(void* argument){
- this->pins[0] = this->kernel->config->value(alpha_min_endstop_checksum )->by_default("nc" )->as_pin()->as_input();
- this->pins[1] = this->kernel->config->value(beta_min_endstop_checksum )->by_default("nc" )->as_pin()->as_input();
- this->pins[2] = this->kernel->config->value(gamma_min_endstop_checksum )->by_default("nc" )->as_pin()->as_input();
- this->fast_rates[0] = this->kernel->config->value(alpha_fast_homing_rate_checksum )->by_default("500" )->as_number();
- this->fast_rates[1] = this->kernel->config->value(beta_fast_homing_rate_checksum )->by_default("500" )->as_number();
- this->fast_rates[2] = this->kernel->config->value(gamma_fast_homing_rate_checksum )->by_default("500" )->as_number();
- this->slow_rates[0] = this->kernel->config->value(alpha_slow_homing_rate_checksum )->by_default("100" )->as_number();
- this->slow_rates[1] = this->kernel->config->value(beta_slow_homing_rate_checksum )->by_default("100" )->as_number();
- this->slow_rates[2] = this->kernel->config->value(gamma_slow_homing_rate_checksum )->by_default("100" )->as_number();
- this->retract_steps[0] = this->kernel->config->value(alpha_homing_retract_checksum )->by_default("30" )->as_number();
- this->retract_steps[1] = this->kernel->config->value(beta_homing_retract_checksum )->by_default("30" )->as_number();
- this->retract_steps[2] = this->kernel->config->value(gamma_homing_retract_checksum )->by_default("30" )->as_number();
+ this->pins[0].from_string( this->kernel->config->value(alpha_min_endstop_checksum )->by_default("nc" )->as_string())->as_input();
+ this->pins[1].from_string( this->kernel->config->value(beta_min_endstop_checksum )->by_default("nc" )->as_string())->as_input();
+ this->pins[2].from_string( this->kernel->config->value(gamma_min_endstop_checksum )->by_default("nc" )->as_string())->as_input();
+ this->pins[3].from_string( this->kernel->config->value(alpha_max_endstop_checksum )->by_default("nc" )->as_string())->as_input();
+ this->pins[4].from_string( this->kernel->config->value(beta_max_endstop_checksum )->by_default("nc" )->as_string())->as_input();
+ this->pins[5].from_string( this->kernel->config->value(gamma_max_endstop_checksum )->by_default("nc" )->as_string())->as_input();
+ this->fast_rates[0] = this->kernel->config->value(alpha_fast_homing_rate_checksum )->by_default(500 )->as_number();
+ this->fast_rates[1] = this->kernel->config->value(beta_fast_homing_rate_checksum )->by_default(500 )->as_number();
+ this->fast_rates[2] = this->kernel->config->value(gamma_fast_homing_rate_checksum )->by_default(5 )->as_number();
+ this->slow_rates[0] = this->kernel->config->value(alpha_slow_homing_rate_checksum )->by_default(100 )->as_number();
+ this->slow_rates[1] = this->kernel->config->value(beta_slow_homing_rate_checksum )->by_default(100 )->as_number();
+ this->slow_rates[2] = this->kernel->config->value(gamma_slow_homing_rate_checksum )->by_default(5 )->as_number();
+ this->retract_steps[0] = this->kernel->config->value(alpha_homing_retract_checksum )->by_default(30 )->as_number();
+ this->retract_steps[1] = this->kernel->config->value(beta_homing_retract_checksum )->by_default(30 )->as_number();
+ this->retract_steps[2] = this->kernel->config->value(gamma_homing_retract_checksum )->by_default(10 )->as_number();
+ this->trim[0] = this->kernel->config->value(alpha_trim_checksum )->by_default(0 )->as_number();
+ this->trim[1] = this->kernel->config->value(beta_trim_checksum )->by_default(0 )->as_number();
+ this->trim[2] = this->kernel->config->value(gamma_trim_checksum )->by_default(0 )->as_number();
+ this->debounce_count = this->kernel->config->value(endstop_debounce_count_checksum )->by_default(100 )->as_number();
+
+ // get homing direction and convert to boolean where true is home to min, and false is home to max
+ int home_dir = get_checksum(this->kernel->config->value(alpha_homing_direction_checksum)->by_default("home_to_min")->as_string());
+ this->home_direction[0] = home_dir != home_to_max_checksum;
+
+ home_dir = get_checksum(this->kernel->config->value(beta_homing_direction_checksum)->by_default("home_to_min")->as_string());
+ this->home_direction[1] = home_dir != home_to_max_checksum;
+
+ home_dir = get_checksum(this->kernel->config->value(gamma_homing_direction_checksum)->by_default("home_to_min")->as_string());
+ this->home_direction[2] = home_dir != home_to_max_checksum;
+
+ this->homing_position[0] = this->home_direction[0]?this->kernel->config->value(alpha_min_checksum)->by_default(0)->as_number():this->kernel->config->value(alpha_max_checksum)->by_default(200)->as_number();
+ this->homing_position[1] = this->home_direction[1]?this->kernel->config->value(beta_min_checksum )->by_default(0)->as_number():this->kernel->config->value(beta_max_checksum )->by_default(200)->as_number();;
+ this->homing_position[2] = this->home_direction[2]?this->kernel->config->value(gamma_min_checksum)->by_default(0)->as_number():this->kernel->config->value(gamma_max_checksum)->by_default(200)->as_number();;
+}
+
+//#pragma GCC pop_options
+
+void Endstops::wait_for_homed(char axes_to_move){
+ bool running = true;
+ unsigned int debounce[3] = {0,0,0};
+ while(running){
+ running = false;
+ this->kernel->call_event(ON_IDLE);
+ for( char c = 'X'; c <= 'Z'; c++ ){
+ if( ( axes_to_move >> ( c - 'X' ) ) & 1 ){
+ if( this->pins[c - 'X' + (this->home_direction[c - 'X']?0:3)].get() ){
+ if( debounce[c - 'X'] < debounce_count ) {
+ debounce[c - 'X'] ++;
+ running = true;
+ } else if ( this->steppers[c - 'X']->moving ){
+ this->steppers[c - 'X']->move(0,0);
+ //this->kernel->streams->printf("move done %c\r\n", c);
+ }
+ }else{
+ // The endstop was not hit yet
+ running = true;
+ debounce[c - 'X'] = 0;
+ }
+ }
+ }
+ }
}
// Start homing sequences by response to GCode commands
-void Endstops::on_gcode_received(void* argument){
+void Endstops::on_gcode_received(void* argument)
+{
Gcode* gcode = static_cast<Gcode*>(argument);
- if( gcode->has_g){
- if( gcode->g == 28 ){
+ if( gcode->has_g)
+ {
+ if( gcode->g == 28 )
+ {
+ gcode->mark_as_taken();
// G28 is received, we have homing to do
// First wait for the queue to be empty
- while(this->kernel->player->queue.size() > 0) { wait_us(500); }
+ this->kernel->conveyor->wait_for_empty_queue();
// Do we move select axes or all of them
- char axes_to_move = ( ( gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') ) ? 0x00 : 0xff );
+ char axes_to_move = 0;
+ // only enable homing if the endstop is defined
+ bool home_all= !( gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') );
+
for( char c = 'X'; c <= 'Z'; c++ ){
- if( gcode->has_letter(c) && this->pins[c - 'X']->connected() ){ axes_to_move += ( 1 << (c - 'X' ) ); }
+ if( (home_all || gcode->has_letter(c)) && this->pins[c - 'X' + (this->home_direction[c - 'X']?0:3)].connected() ){ axes_to_move += ( 1 << (c - 'X' ) ); }
}
+ // Enable the motors
+ this->kernel->stepper->turn_enable_pins_on();
+
// Start moving the axes to the origin
this->status = MOVING_TO_ORIGIN_FAST;
for( char c = 'X'; c <= 'Z'; c++ ){
if( ( axes_to_move >> ( c - 'X' ) ) & 1 ){
- this->steppers[c - 'X']->move(0,10000000);
- this->steppers[c - 'X']->set_speed(this->fast_rates[c -'X']);
+ gcode->stream->printf("homing axis %c\r\n", c);
+ this->steppers[c - 'X']->set_speed(this->fast_rates[c - 'X']);
+ this->steppers[c - 'X']->move(this->home_direction[c - 'X'],10000000);
}
}
// Wait for all axes to have homed
- bool running = true;
- while(running){
- running = false;
- for( char c = 'X'; c <= 'Z'; c++ ){
- if( ( axes_to_move >> ( c - 'X' ) ) & 1 ){
- if( this->pins[c - 'X']->get() ){
- // The endstop was hit, stop moving
- if( this->steppers[c - 'X']->moving ){
- this->steppers[c - 'X']->move(0,0);
- }
- }else{
- // The endstop was not hit yet
- running = true;
- }
- }
- }
- }
+ this->wait_for_homed(axes_to_move);
-
- printf("test a\r\n");
+ gcode->stream->printf("test a\r\n");
// Move back a small distance
this->status = MOVING_BACK;
+ int inverted_dir;
for( char c = 'X'; c <= 'Z'; c++ ){
if( ( axes_to_move >> ( c - 'X' ) ) & 1 ){
- this->steppers[c - 'X']->move(1,this->retract_steps[c - 'X']);
+ inverted_dir = -(this->home_direction[c - 'X'] - 1);
this->steppers[c - 'X']->set_speed(this->slow_rates[c - 'X']);
+ this->steppers[c - 'X']->move(inverted_dir,this->retract_steps[c - 'X']);
}
}
- printf("test b\r\n");
+ gcode->stream->printf("test b\r\n");
// Wait for moves to be done
for( char c = 'X'; c <= 'Z'; c++ ){
if( ( axes_to_move >> ( c - 'X' ) ) & 1 ){
- printf("axis %c \r\n", c );
- while( this->steppers[c - 'X']->moving ){ }
+ this->kernel->streams->printf("axis %c \r\n", c );
+ while( this->steppers[c - 'X']->moving ){
+ this->kernel->call_event(ON_IDLE);
+ }
}
}
- printf("test c\r\n");
+ gcode->stream->printf("test c\r\n");
// Start moving the axes to the origin slowly
this->status = MOVING_TO_ORIGIN_SLOW;
for( char c = 'X'; c <= 'Z'; c++ ){
if( ( axes_to_move >> ( c - 'X' ) ) & 1 ){
- this->steppers[c - 'X']->move(0,10000000);
this->steppers[c - 'X']->set_speed(this->slow_rates[c -'X']);
+ this->steppers[c - 'X']->move(this->home_direction[c - 'X'],10000000);
}
}
// Wait for all axes to have homed
- running = true;
- while(running){
- running = false;
- for( char c = 'X'; c <= 'Z'; c++ ){
- if( ( axes_to_move >> ( c - 'X' ) ) & 1 ){
- if( this->pins[c - 'X']->get() ){
- // The endstop was hit, stop moving
- if( this->steppers[c - 'X']->moving ){
- this->steppers[c - 'X']->move(0,0);
- }
- }else{
- // The endstop was not hit yet
- running = true;
- }
- }
+ this->wait_for_homed(axes_to_move);
+
+ // move for soft trim
+ this->status = MOVING_BACK;
+ for( char c = 'X'; c <= 'Z'; c++ ){
+ if( ( axes_to_move >> ( c - 'X' ) ) & 1 ){
+ inverted_dir = -(this->home_direction[c - 'X'] - 1);
+ this->steppers[c - 'X']->set_speed(this->slow_rates[c - 'X']);
+ this->steppers[c - 'X']->move(inverted_dir,this->trim[c - 'X']);
}
}
+ // Wait for moves to be done
+ for( char c = 'X'; c <= 'Z'; c++ ){
+ if( ( axes_to_move >> ( c - 'X' ) ) & 1 ){
+ this->kernel->streams->printf("axis %c \r\n", c );
+ while( this->steppers[c - 'X']->moving ){
+ this->kernel->call_event(ON_IDLE);
+ }
+ }
+ }
// Homing is done
this->status = NOT_HOMING;
-
+
+ // Zero the ax(i/e)s position
+ for( char c = 'X'; c <= 'Z'; c++ ){
+ if( ( axes_to_move >> ( c - 'X' ) ) & 1 ){
+
+ this->kernel->robot->reset_axis_position(this->homing_position[c - 'X'], c - 'X');
+ }
+ }
+
+ }
+ }
+ else if (gcode->has_m){
+ switch(gcode->m){
+ case 119:
+ gcode->stream->printf("X min:%d max:%d Y min:%d max:%d Z min:%d max:%d\n", this->pins[0].get(), this->pins[3].get(), this->pins[1].get(), this->pins[4].get(), this->pins[2].get(), this->pins[5].get() );
+ gcode->mark_as_taken();
+ break;
}
}
}
HCoop / clinton / Smoothieware.git / blobdiff