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 // A StepperMotor represents an actual stepper motor. It is used to generate steps that move the actual motor at a given speed
  20
  21 StepperMotor::StepperMotor()
  22 {
  23     init();
  24 }
  25
  26 StepperMotor::StepperMotor(Pin &step, Pin &dir, Pin &en) : step_pin(step), dir_pin(dir), en_pin(en)
  27 {
  28     init();
  29     enable(false);
  30     set_high_on_debug(en.port_number, en.pin);
  31 }
  32
  33 StepperMotor::~StepperMotor()
  34 {
  35 }
  36
  37 void StepperMotor::init()
  38 {
  39     // register this motor with the step ticker, and get its index in that array and bit position
  40     this->index= THEKERNEL->step_ticker->register_motor(this);
  41     this->moving = false;
  42     this->fx_counter = 0;
  43     this->fx_ticks_per_step = 0xFFFFF000UL; // some big number so we don't start stepping before it is set
  44     this->stepped = 0;
  45     this->steps_to_move = 0;
  46     this->is_move_finished = false;
  47     this->last_step_tick_valid= false;
  48     this->last_step_tick= 0;
  49     this->force_finish= false;
  50
  51     steps_per_mm         = 1.0F;
  52     max_rate             = 50.0F;
  53     minimum_step_rate    = default_minimum_actuator_rate;
  54
  55     last_milestone_steps = 0;
  56     last_milestone_mm    = 0.0F;
  57     current_position_steps= 0;
  58     signal_step= 0;
  59 }
  60
  61
  62 // This is called ( see the .h file, we had to put a part of things there for obscure inline reasons ) when a step has to be generated
  63 // we also here check if the move is finished etc ..
  64 // This is in highest priority interrupt so cannot be pre-empted
  65 void StepperMotor::step()
  66 {
  67     // ignore if we are still processing the end of a block
  68     if(this->is_move_finished) return;
  69
  70     if(!this->force_finish) {
  71         // output to pins 37t
  72         this->step_pin.set( 1 );
  73
  74         // move counter back 11t
  75         this->fx_counter -= this->fx_ticks_per_step;
  76
  77         // we have moved a step 9t
  78         this->stepped++;
  79
  80         // keep track of actuators actual position in steps
  81         this->current_position_steps += (this->direction ? -1 : 1);
  82
  83         // we may need to callback on a specific step, usually used to synchronize deceleration timer
  84         if(this->signal_step != 0 && this->stepped == this->signal_step) {
  85             THEKERNEL->step_ticker->synchronize_acceleration(true);
  86             this->signal_step= 0;
  87         }
  88     }
  89
  90     // Is this move finished ?
  91     if( this->force_finish || this->stepped == this->steps_to_move) {
  92         // Mark it as finished, then StepTicker will call signal_move_finished()
  93         // This is so we don't call that before all the steps have been generated for this tick()
  94         this->is_move_finished = true;
  95         THEKERNEL->step_ticker->a_move_finished= true;
  96         this->last_step_tick= THEKERNEL->step_ticker->get_tick_cnt(); // remember when last step was
  97         if(this->force_finish) this->steps_to_move = stepped;
  98     }
  99 }
 100
 101 // Does a manual step pulse, used for direct encoder control of a stepper
 102 void StepperMotor::manual_step(bool dir)
 103 {
 104     if(!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     // set step pin
 114     this->step_pin.set(1);
 115
 116     // schedule for unstep
 117     THEKERNEL->step_ticker->schedule_unstep(this->index);
 118
 119     // keep track of actuators actual position in steps
 120     this->current_position_steps += (dir ? -1 : 1);
 121 }
 122
 123 // If the move is finished, the StepTicker will call this ( because we asked it to in tick() )
 124 void StepperMotor::signal_move_finished()
 125 {
 126     // work is done ! 8t
 127     this->moving = false;
 128     this->steps_to_move = 0;
 129     this->minimum_step_rate = default_minimum_actuator_rate;
 130
 131     // signal it to whatever cares
 132     // in this call a new block may start, new moves set and new speeds
 133     this->end_hook->call();
 134
 135     // We only need to do this if we were not instructed to move
 136     if( !this->moving ) {
 137         this->update_exit_tick();
 138     }
 139
 140     this->is_move_finished = false;
 141 }
 142
 143 // This is just a way not to check for ( !this->moving || this->fx_ticks_per_step == 0 ) at every tick()
 144 void StepperMotor::update_exit_tick()
 145 {
 146     if( !this->moving || this->steps_to_move == 0 ) {
 147         // No more ticks will be received and no more events from StepTicker
 148         THEKERNEL->step_ticker->remove_motor_from_active_list(this);
 149     } else {
 150         // we will now get ticks and StepTIcker will send us events
 151         THEKERNEL->step_ticker->add_motor_to_active_list(this);
 152     }
 153 }
 154
 155 // Instruct the StepperMotor to move a certain number of steps
 156 StepperMotor* StepperMotor::move( bool direction, unsigned int steps, float initial_speed)
 157 {
 158     this->dir_pin.set(direction);
 159     this->direction = direction;
 160     this->force_finish= false;
 161
 162     // How many steps we have to move until the move is done
 163     this->steps_to_move = steps;
 164
 165     // Zero our tool counters
 166     this->stepped = 0;
 167     this->fx_ticks_per_step = 0xFFFFF000UL; // some big number so we don't start stepping before it is set again
 168     if(this->last_step_tick_valid) {
 169         // we set this based on when the last step was, thus compensating for missed ticks
 170         uint32_t ts= THEKERNEL->step_ticker->ticks_since(this->last_step_tick);
 171         // if an axis stops too soon then we can get a huge number of ticks here which causes problems, so if the number of ticks is too great we ignore them
 172         // example of when this happens is when one axis is going very slow and the min 20steps/sec kicks in, the axis will reach its target much sooner leaving a long gap
 173         // until the end of the block.
 174         // TODO we may need to set this based on the current step rate, trouble is we don't know what that is yet, we could use the last fx_ticks_per_step as a guide
 175         if(ts > 5) ts= 5; // limit to 50us catch up around 1-2 steps
 176         else if(ts > 15) ts= 0; // no way to know what the delay was
 177         this->fx_counter= ts*fx_increment;
 178     }else{
 179         this->fx_counter = 0; // set to zero as there was no step last block
 180     }
 181
 182     // Starting now we are moving
 183     if( steps > 0 ) {
 184         if(initial_speed >= 0.0F) set_speed(initial_speed);
 185         this->moving = true;
 186     } else {
 187         this->moving = false;
 188     }
 189     this->update_exit_tick();
 190     return this;
 191 }
 192
 193 // Set the speed at which this stepper moves in steps/sec, should be called set_step_rate()
 194 // we need to make sure that we have a minimum speed here and that it fits the 32bit fixed point fx counters
 195 // Note nothing will really ever go as slow as the minimum speed here, it is just forced to avoid bad errors
 196 // fx_ticks_per_step is what actually sets the step rate, it is fixed point 18.14
 197 StepperMotor* StepperMotor::set_speed( float speed )
 198 {
 199     if(speed < minimum_step_rate) {
 200         speed= minimum_step_rate;
 201     }
 202
 203     // if(speed <= 0.0F) { // we can't actually do 0 but we can get close, need to avoid divide by zero later on
 204     //     this->fx_ticks_per_step= 0xFFFFFFFFUL; // 0.381 steps/sec
 205     //     this->steps_per_second = THEKERNEL->step_ticker->get_frequency() / (this->fx_ticks_per_step >> fx_shift);
 206     //     return;
 207     // }
 208
 209     // How many steps we must output per second
 210     this->steps_per_second = speed;
 211
 212     // set the new speed, NOTE this can be pre-empted by stepticker so the following write needs to be atomic
 213     this->fx_ticks_per_step= floor(fx_increment * THEKERNEL->step_ticker->get_frequency() / speed);
 214     return this;
 215 }
 216
 217 void StepperMotor::change_steps_per_mm(float new_steps)
 218 {
 219     steps_per_mm = new_steps;
 220     last_milestone_steps = lroundf(last_milestone_mm * steps_per_mm);
 221     current_position_steps = last_milestone_steps;
 222 }
 223
 224 void StepperMotor::change_last_milestone(float new_milestone)
 225 {
 226     last_milestone_mm = new_milestone;
 227     last_milestone_steps = lroundf(last_milestone_mm * steps_per_mm);
 228     current_position_steps = last_milestone_steps;
 229 }
 230
 231 int  StepperMotor::steps_to_target(float target)
 232 {
 233     int target_steps = lroundf(target * steps_per_mm);
 234     return target_steps - last_milestone_steps;
 235 }