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 // in steps/sec the default minimum speed (was 20steps/sec hardcoded)
  17 float StepperMotor::default_minimum_actuator_rate= 20.0F;
  18
  19 StepperMotor::StepperMotor(Pin &step, Pin &dir, Pin &en) : step_pin(step), dir_pin(dir), en_pin(en)
  20 {
  21     set_high_on_debug(en.port_number, en.pin);
  22     // register this motor with the step ticker, and get its index in that array and bit position
  23     this->index= THEKERNEL->step_ticker->register_motor(this);
  24
  25     steps_per_mm         = 1.0F;
  26     max_rate             = 50.0F;
  27
  28     last_milestone_steps = 0;
  29     last_milestone_mm    = 0.0F;
  30     current_position_steps= 0;
  31     enable(false);
  32     moving= false;
  33
  34     this->register_for_event(ON_HALT);
  35     this->register_for_event(ON_ENABLE);
  36 }
  37
  38 StepperMotor::~StepperMotor()
  39 {
  40     THEKERNEL->unregister_for_event(ON_HALT, this);
  41     THEKERNEL->unregister_for_event(ON_ENABLE, this);
  42 }
  43
  44 void StepperMotor::on_halt(void *argument)
  45 {
  46     if(argument == nullptr) {
  47         enable(false);
  48     }
  49 }
  50
  51 void StepperMotor::on_enable(void *argument)
  52 {
  53     enable(argument != nullptr);
  54 }
  55
  56 void StepperMotor::change_steps_per_mm(float new_steps)
  57 {
  58     steps_per_mm = new_steps;
  59     last_milestone_steps = lroundf(last_milestone_mm * steps_per_mm);
  60     current_position_steps = last_milestone_steps;
  61 }
  62
  63 void StepperMotor::change_last_milestone(float new_milestone)
  64 {
  65     last_milestone_mm = new_milestone;
  66     last_milestone_steps = lroundf(last_milestone_mm * steps_per_mm);
  67     current_position_steps = last_milestone_steps;
  68 }
  69
  70 int  StepperMotor::steps_to_target(float target)
  71 {
  72     int target_steps = lroundf(target * steps_per_mm);
  73     return target_steps - last_milestone_steps;
  74 }
  75
  76 // Does a manual step pulse, used for direct encoder control of a stepper
  77 void StepperMotor::manual_step(bool dir)
  78 {
  79     if(!is_enabled()) enable(true);
  80
  81     // set direction if needed
  82     if(this->direction != dir) {
  83         this->direction= dir;
  84         this->dir_pin.set(dir);
  85         wait_us(1);
  86     }
  87
  88     // pulse step pin
  89     this->step_pin.set(1);
  90     wait_us(3);
  91     this->step_pin.set(0);
  92
  93
  94     // keep track of actuators actual position in steps
  95     this->current_position_steps += (dir ? -1 : 1);
  96 }