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     enable(false);
  27     moving= false;
  28     acceleration= NAN;
  29     selected= true;
  30
  31     this->register_for_event(ON_HALT);
  32     this->register_for_event(ON_ENABLE);
  33 }
  34
  35 StepperMotor::~StepperMotor()
  36 {
  37     THEKERNEL->unregister_for_event(ON_HALT, this);
  38     THEKERNEL->unregister_for_event(ON_ENABLE, this);
  39 }
  40
  41 void StepperMotor::on_halt(void *argument)
  42 {
  43     if(argument == nullptr) {
  44         enable(false);
  45         moving= false;
  46     }
  47 }
  48
  49 void StepperMotor::on_enable(void *argument)
  50 {
  51     enable(argument != nullptr);
  52 }
  53
  54 void StepperMotor::change_steps_per_mm(float new_steps)
  55 {
  56     steps_per_mm = new_steps;
  57     last_milestone_steps = lroundf(last_milestone_mm * steps_per_mm);
  58     current_position_steps = last_milestone_steps;
  59 }
  60
  61 void StepperMotor::change_last_milestone(float new_milestone)
  62 {
  63     last_milestone_mm = new_milestone;
  64     last_milestone_steps = lroundf(last_milestone_mm * steps_per_mm);
  65     current_position_steps = last_milestone_steps;
  66 }
  67
  68 void StepperMotor::set_last_milestones(float mm, int32_t steps)
  69 {
  70     last_milestone_mm= mm;
  71     last_milestone_steps= steps;
  72     current_position_steps= last_milestone_steps;
  73 }
  74
  75 int  StepperMotor::steps_to_target(float target)
  76 {
  77     int32_t target_steps = lroundf(target * steps_per_mm);
  78     return target_steps - last_milestone_steps;
  79 }
  80
  81 // Does a manual step pulse, used for direct encoder control of a stepper
  82 void StepperMotor::manual_step(bool dir)
  83 {
  84     if(!is_enabled()) enable(true);
  85
  86     // set direction if needed
  87     if(this->direction != dir) {
  88         this->direction= dir;
  89         this->dir_pin.set(dir);
  90         wait_us(1);
  91     }
  92
  93     // pulse step pin
  94     this->step_pin.set(1);
  95     wait_us(3);
  96     this->step_pin.set(0);
  97
  98
  99     // keep track of actuators actual position in steps
 100     this->current_position_steps += (dir ? -1 : 1);
 101 }