src/libs/StepperMotor.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 #include "StepperMotor.h"
   8
   9 #include "Kernel.h"
  10 #include "MRI_Hooks.h"
  11 #include "StepTicker.h"
  12
  13 #include <math.h>
  14 #include "mbed.h"
  15
  16 StepperMotor::StepperMotor(Pin &step, Pin &dir, Pin &en) : step_pin(step), dir_pin(dir), en_pin(en)
  17 {
  18     set_high_on_debug(en.port_number, en.pin);
  19
  20     steps_per_mm         = 1.0F;
  21     max_rate             = 50.0F;
  22
  23     last_milestone_steps = 0;
  24     last_milestone_mm    = 0.0F;
  25     current_position_steps= 0;
  26     moving= false;
  27     acceleration= NAN;
  28     selected= true;
  29     extruder= false;
  30
  31     enable(false);
  32     unstep(); // initialize step pin
  33     set_direction(false); // initialize dor pin
  34
  35     this->register_for_event(ON_HALT);
  36     this->register_for_event(ON_ENABLE);
  37 }
  38
  39 StepperMotor::~StepperMotor()
  40 {
  41     THEKERNEL->unregister_for_event(ON_HALT, this);
  42     THEKERNEL->unregister_for_event(ON_ENABLE, this);
  43 }
  44
  45 void StepperMotor::on_halt(void *argument)
  46 {
  47     if(argument == nullptr) {
  48         enable(false);
  49         moving= false;
  50     }
  51 }
  52
  53 void StepperMotor::on_enable(void *argument)
  54 {
  55     // argument is a uin32_t where bit0 is on or off, and bit 1:X, 2:Y, 3:Z, 4:A, 5:B, 6:C etc
  56     // for now if bit0 is 1 we turn all on, if 0 we turn all off otherwise we turn selected axis off
  57     uint32_t bm= (uint32_t)argument;
  58     if(bm == 0x01) {
  59         enable(true);
  60
  61     }else if(bm == 0 || ((bm&0x01) == 0 && ((bm&(0x02<<motor_id)) != 0)) ) {
  62         enable(false);
  63     }
  64 }
  65
  66 void StepperMotor::change_steps_per_mm(float new_steps)
  67 {
  68     steps_per_mm = new_steps;
  69     last_milestone_steps = lroundf(last_milestone_mm * steps_per_mm);
  70     current_position_steps = last_milestone_steps;
  71 }
  72
  73 void StepperMotor::change_last_milestone(float new_milestone)
  74 {
  75     last_milestone_mm = new_milestone;
  76     last_milestone_steps = lroundf(last_milestone_mm * steps_per_mm);
  77     current_position_steps = last_milestone_steps;
  78 }
  79
  80 void StepperMotor::set_last_milestones(float mm, int32_t steps)
  81 {
  82     last_milestone_mm= mm;
  83     last_milestone_steps= steps;
  84     current_position_steps= last_milestone_steps;
  85 }
  86
  87 void StepperMotor::update_last_milestones(float mm, int32_t steps)
  88 {
  89     last_milestone_steps += steps;
  90     last_milestone_mm = mm;
  91 }
  92
  93 int32_t StepperMotor::steps_to_target(float target)
  94 {
  95     int32_t target_steps = lroundf(target * steps_per_mm);
  96     return target_steps - last_milestone_steps;
  97 }
  98
  99 // Does a manual step pulse, used for direct encoder control of a stepper
 100 // NOTE this is experimental and may change and/or be reomved in the future, it is an unsupported feature.
 101 // use at your own risk
 102 void StepperMotor::manual_step(bool dir)
 103 {
 104     if(!is_enabled()) enable(true);
 105
 106     // set direction if needed
 107     if(this->direction != dir) {
 108         this->direction= dir;
 109         this->dir_pin.set(dir);
 110         wait_us(1);
 111     }
 112
 113     // pulse step pin
 114     this->step_pin.set(1);
 115     wait_us(3);
 116     this->step_pin.set(0);
 117
 118
 119     // keep track of actuators actual position in steps
 120     this->current_position_steps += (dir ? -1 : 1);
 121 }