src/modules/tools/endstops/Endstops.cpp

   1 /*
   2       This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
   3       Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
   4       Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
   5       You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
   6 */
   7
   8 #include "libs/Module.h"
   9 #include "libs/Kernel.h"
  10 #include "modules/communication/utils/Gcode.h"
  11 #include "modules/robot/Conveyor.h"
  12 #include "Endstops.h"
  13 #include "libs/nuts_bolts.h"
  14 #include "libs/Pin.h"
  15 #include "libs/StepperMotor.h"
  16 #include "wait_api.h" // mbed.h lib
  17
  18 Endstops::Endstops(){
  19     this->status = NOT_HOMING;
  20 }
  21
  22 void Endstops::on_module_loaded() {
  23     register_for_event(ON_CONFIG_RELOAD);
  24     this->register_for_event(ON_GCODE_RECEIVED);
  25
  26     // Take StepperMotor objects from Robot and keep them here
  27     this->steppers[0] = this->kernel->robot->alpha_stepper_motor;
  28     this->steppers[1] = this->kernel->robot->beta_stepper_motor;
  29     this->steppers[2] = this->kernel->robot->gamma_stepper_motor;
  30
  31     // Settings
  32     this->on_config_reload(this);
  33
  34 }
  35
  36 // Get config
  37 void Endstops::on_config_reload(void* argument){
  38     this->pins[0].from_string(         this->kernel->config->value(alpha_min_endstop_checksum          )->by_default("nc" )->as_string())->as_input()->pull_up();
  39     this->pins[1].from_string(         this->kernel->config->value(beta_min_endstop_checksum           )->by_default("nc" )->as_string())->as_input()->pull_up();
  40     this->pins[2].from_string(         this->kernel->config->value(gamma_min_endstop_checksum          )->by_default("nc" )->as_string())->as_input()->pull_up();
  41     this->pins[3].from_string(         this->kernel->config->value(alpha_max_endstop_checksum          )->by_default("nc" )->as_string())->as_input()->pull_up();
  42     this->pins[4].from_string(         this->kernel->config->value(beta_max_endstop_checksum           )->by_default("nc" )->as_string())->as_input()->pull_up();
  43     this->pins[5].from_string(         this->kernel->config->value(gamma_max_endstop_checksum          )->by_default("nc" )->as_string())->as_input()->pull_up();
  44     this->fast_rates[0]             =  this->kernel->config->value(alpha_fast_homing_rate_checksum     )->by_default("500" )->as_number();
  45     this->fast_rates[1]             =  this->kernel->config->value(beta_fast_homing_rate_checksum      )->by_default("500" )->as_number();
  46     this->fast_rates[2]             =  this->kernel->config->value(gamma_fast_homing_rate_checksum     )->by_default("5" )->as_number();
  47     this->slow_rates[0]             =  this->kernel->config->value(alpha_slow_homing_rate_checksum     )->by_default("100" )->as_number();
  48     this->slow_rates[1]             =  this->kernel->config->value(beta_slow_homing_rate_checksum      )->by_default("100" )->as_number();
  49     this->slow_rates[2]             =  this->kernel->config->value(gamma_slow_homing_rate_checksum     )->by_default("5" )->as_number();
  50     this->retract_steps[0]          =  this->kernel->config->value(alpha_homing_retract_checksum       )->by_default("30" )->as_number();
  51     this->retract_steps[1]          =  this->kernel->config->value(beta_homing_retract_checksum        )->by_default("30" )->as_number();
  52     this->retract_steps[2]          =  this->kernel->config->value(gamma_homing_retract_checksum       )->by_default("10" )->as_number();
  53     this->debounce_count            =  this->kernel->config->value(endstop_debounce_count_checksum     )->by_default("100" )->as_number();
  54     this->direction[0]              =  this->kernel->config->value(alpha_homing_direction_checksum     )->by_default(-1   )->as_number();
  55     this->direction[1]              =  this->kernel->config->value(beta_homing_direction_checksum      )->by_default(-1   )->as_number();
  56     this->direction[2]              =  this->kernel->config->value(gamma_homing_direction_checksum     )->by_default(1   )->as_number();
  57     for (int i=0; i<3; i++) direction[i] = direction[i] > 0;
  58     this->homing_position[0]        =  this->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();
  59     this->homing_position[1]        =  this->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();;
  60     this->homing_position[2]        =  this->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();;
  61 }
  62
  63 void Endstops::wait_for_homed(char axes_to_move)
  64 {
  65     bool running = true;
  66     unsigned int debounce[3] = {0,0,0};
  67     while(running){
  68         running = false;
  69         for( char c = 'X'; c <= 'Z'; c++ ){
  70             if( ( axes_to_move >> ( c - 'X' ) ) & 1 ){
  71                 if( this->pins[c - 'X' + (this->direction[c - 'X']?0:3)].get() ){
  72                     if( debounce[c - 'X'] < debounce_count ) {
  73                         debounce[c - 'X'] ++;
  74                         running = true;
  75                     } else if ( this->steppers[c - 'X']->moving ){
  76                         this->steppers[c - 'X']->move(0,0);
  77                         printf("move done %c\r\n", c);
  78                     }
  79                 }else{
  80                     // The endstop was not hit yet
  81                     running = true;
  82                     debounce[c - 'X'] = 0;
  83                 }
  84             }
  85         }
  86     }
  87 }
  88
  89 // Start homing sequences by response to GCode commands
  90 void Endstops::on_gcode_received(void* argument)
  91 {
  92     Gcode* gcode = static_cast<Gcode*>(argument);
  93     if( gcode->has_g)
  94     {
  95         if( gcode->g == 28 )
  96         {
  97             // G28 is received, we have homing to do
  98
  99             // First wait for the queue to be empty
 100             while(this->kernel->conveyor->queue.size() > 0) { wait_us(500); }
 101
 102             // Do we move select axes or all of them
 103             char axes_to_move = ( ( gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') ) ? 0x00 : 0xff );
 104             for( char c = 'X'; c <= 'Z'; c++ ){
 105                 if( gcode->has_letter(c) && this->pins[c - 'X'].connected() ){ axes_to_move += ( 1 << (c - 'X' ) ); }
 106             }
 107
 108             // Enable the motors
 109             this->kernel->stepper->turn_enable_pins_on();
 110
 111             // Start moving the axes to the origin
 112             this->status = MOVING_TO_ORIGIN_FAST;
 113             for( char c = 'X'; c <= 'Z'; c++ ){
 114                 if( ( axes_to_move >> ( c - 'X' ) ) & 1 ){
 115                     this->steppers[c - 'X']->set_speed(this->fast_rates[c - 'X']);
 116                     this->steppers[c - 'X']->move(this->direction[c - 'X'],10000000);
 117                 }
 118             }
 119
 120             // Wait for all axes to have homed
 121             this->wait_for_homed(axes_to_move);
 122
 123             printf("test a\r\n");
 124             // Move back a small distance
 125             this->status = MOVING_BACK;
 126             int inverted_dir;
 127             for( char c = 'X'; c <= 'Z'; c++ ){
 128                 if( ( axes_to_move >> ( c - 'X' ) ) & 1 ){
 129                     inverted_dir = -(this->direction[c - 'X'] - 1);
 130                     this->steppers[c - 'X']->set_speed(this->slow_rates[c - 'X']);
 131                     this->steppers[c - 'X']->move(inverted_dir,this->retract_steps[c - 'X']);
 132                 }
 133             }
 134
 135             printf("test b\r\n");
 136             // Wait for moves to be done
 137             for( char c = 'X'; c <= 'Z'; c++ ){
 138                 if(  ( axes_to_move >> ( c - 'X' ) ) & 1 ){
 139                     printf("axis %c \r\n", c );
 140                     while( this->steppers[c - 'X']->moving ){ }
 141                 }
 142             }
 143
 144             printf("test c\r\n");
 145
 146             // Start moving the axes to the origin slowly
 147             this->status = MOVING_TO_ORIGIN_SLOW;
 148             for( char c = 'X'; c <= 'Z'; c++ ){
 149                 if( ( axes_to_move >> ( c - 'X' ) ) & 1 ){
 150                     this->steppers[c - 'X']->set_speed(this->slow_rates[c -'X']);
 151                     this->steppers[c - 'X']->move(this->direction[c - 'X'],10000000);
 152                 }
 153             }
 154
 155             // Wait for all axes to have homed
 156             this->wait_for_homed(axes_to_move);
 157
 158             // Homing is done
 159             this->status = NOT_HOMING;
 160
 161             // Zero the ax(i/e)s position
 162             for( char c = 'X'; c <= 'Z'; c++ ){
 163                 if( ( axes_to_move >> ( c - 'X' ) ) & 1 ){
 164
 165                     this->kernel->robot->reset_axis_position(this->homing_position[c - 'X'], c - 'X');
 166                 }
 167             }
 168
 169         }
 170     }
 171     else if (gcode->has_m)
 172     {
 173         switch(gcode->m)
 174         {
 175             case 119:
 176                 gcode->stream->printf("X min:%d max:%d Y min:%d max:%d Z min:%d max:%d\n",
 177                                                 this->pins[0].get(),
 178                                                         this->pins[3].get(),
 179                                                                 this->pins[1].get(),
 180                                                                         this->pins[4].get(),
 181                                                                                 this->pins[2].get(),
 182                                                                                     this->pins[5].get()
 183                                         );
 184                 break;
 185         }
 186     }
 187 }
 188