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/Player.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     this->register_for_event(ON_GCODE_RECEIVED);
  24
  25     // Take StepperMotor objects from Robot and keep them here
  26     this->steppers[0] = this->kernel->robot->alpha_stepper_motor;
  27     this->steppers[1] = this->kernel->robot->beta_stepper_motor;
  28     this->steppers[2] = this->kernel->robot->gamma_stepper_motor;
  29
  30     // Settings
  31     this->on_config_reload(this);
  32
  33 }
  34
  35 // Get config
  36 void Endstops::on_config_reload(void* argument){
  37     this->pins[0]                    = this->kernel->config->value(alpha_min_endstop_checksum          )->by_default("nc" )->as_pin()->as_input();
  38     this->pins[1]                    = this->kernel->config->value(beta_min_endstop_checksum           )->by_default("nc" )->as_pin()->as_input();
  39     this->pins[2]                    = this->kernel->config->value(gamma_min_endstop_checksum          )->by_default("nc" )->as_pin()->as_input();
  40 }
  41
  42 // Start homing sequences by response to GCode commands
  43 void Endstops::on_gcode_received(void* argument){
  44     Gcode* gcode = static_cast<Gcode*>(argument);
  45     if( gcode->has_letter('G' )){
  46         if( gcode->get_value('G') == 28 ){
  47             // G28 is received, we have homing to do
  48
  49             // First wait for the queue to be empty
  50             while(this->kernel->player->queue.size() > 0) { wait_us(500); }
  51
  52             // Do we move select axes or all of them
  53             char axes_to_move = ( ( gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') ) ? 0x00 : 0xff );
  54             for( char c = 'X'; c <= 'Z'; c++ ){
  55                 if( gcode->has_letter(c) && this->pins[c - 'X']->connected() ){ axes_to_move += ( 1 << (c - 'X' ) ); }
  56             }
  57
  58             // Start moving the axes to the origin
  59             this->status = MOVING_TO_ORIGIN_FAST;
  60             for( char c = 'X'; c <= 'Z'; c++ ){
  61                 if( ( axes_to_move >> ( c - 'X' ) ) & 1 ){
  62                     this->steppers[c - 'X']->move(0,10000000);
  63                     this->steppers[c - 'X']->set_speed(10000);
  64                 }
  65             }
  66
  67             // Wait for all axes to have homed
  68             bool running = true;
  69             while(running){
  70                 running = false;
  71                 for( char c = 'X'; c <= 'Z'; c++ ){
  72                     if( ( axes_to_move >> ( c - 'X' ) ) & 1 ){
  73                          if( this->pins[c - 'X']->get() ){
  74                             // The endstop was hit, stop moving
  75                             if( this->steppers[c - 'X']->moving ){
  76                                 this->steppers[c - 'X']->move(0,0);
  77                             }
  78                         }else{
  79                             // The endstop was not hit yet
  80                             running = true;
  81                          }
  82                     }
  83                 }
  84             }
  85
  86
  87             printf("test a\r\n");
  88             // Move back a small distance
  89             this->status = MOVING_BACK;
  90             for( char c = 'X'; c <= 'Z'; c++ ){
  91                 if( ( axes_to_move >> ( c - 'X' ) ) & 1 ){
  92                     this->steppers[c - 'X']->move(1,1000);
  93                     this->steppers[c - 'X']->set_speed(1000);
  94                 }
  95             }
  96
  97             printf("test b\r\n");
  98             // Wait for moves to be done
  99             for( char c = 'X'; c <= 'Z'; c++ ){
 100                 if(  ( axes_to_move >> ( c - 'X' ) ) & 1 ){
 101                     printf("axis %c \r\n", c );
 102                     while( this->steppers[c - 'X']->moving ){ }
 103                 }
 104             }
 105
 106             printf("test c\r\n");
 107
 108             // Homing is done
 109             this->status = NOT_HOMING;
 110
 111         }
 112     }
 113 }
 114