StepperMotor.cpp

00001 /*
00002       This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
00003       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.
00004       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.
00005       You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
00006 */
00007 #include "StepperMotor.h"
00008 
00009 #include "Kernel.h"
00010 #include "MRI_Hooks.h"
00011 
00012 // A StepperMotor represents an actual stepper motor. It is used to generate steps that move the actual motor at a given speed
00013 // TODO : Abstract this into Actuator
00014 
00015 StepperMotor::StepperMotor(){
00016     this->moving = false;
00017     this->paused = false;
00018     this->fx_counter = 0;
00019     this->stepped = 0;
00020     this->fx_ticks_per_step = 0;
00021     this->steps_to_move = 0;
00022     this->remove_from_active_list_next_reset = false;
00023     this->is_move_finished = false;
00024     this->signal_step = false;
00025     this->step_signal_hook = new Hook();
00026 
00027     steps_per_mm         = 1.0F;
00028     max_rate             = 50.0F;
00029 
00030     last_milestone_steps = 0;
00031     last_milestone_mm    = 0.0F;
00032 }
00033 
00034 StepperMotor::StepperMotor(Pin& step, Pin& dir, Pin& en) : step_pin(step), dir_pin(dir), en_pin(en) {
00035     this->moving = false;
00036     this->paused = false;
00037     this->fx_counter = 0;
00038     this->stepped = 0;
00039     this->fx_ticks_per_step = 0;
00040     this->steps_to_move = 0;
00041     this->remove_from_active_list_next_reset = false;
00042     this->is_move_finished = false;
00043     this->signal_step = false;
00044     this->step_signal_hook = new Hook();
00045 
00046     enable(false);
00047     set_high_on_debug(en.port_number, en.pin);
00048 
00049     steps_per_mm         = 1.0F;
00050     max_rate             = 50.0F;
00051 
00052     last_milestone_steps = 0;
00053     last_milestone_mm    = 0.0F;
00054 }
00055 
00056 // 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
00057 // we also here check if the move is finished etc ...
00058 void StepperMotor::step(){
00059 
00060     // output to pins 37t
00061     this->step_pin.set( 1                   );
00062     this->step_ticker->reset_step_pins = true;
00063 
00064     // move counter back 11t
00065     this->fx_counter -= this->fx_ticks_per_step;
00066 
00067     // we have moved a step 9t
00068     this->stepped++;
00069 
00070     // Do we need to signal this step
00071     if( this->stepped == this->signal_step_number && this->signal_step ){
00072         this->step_signal_hook->call();
00073     }
00074 
00075     // Is this move finished ?
00076     if( this->stepped == this->steps_to_move ){
00077         // Mark it as finished, then StepTicker will call signal_mode_finished() 
00078         // This is so we don't call that before all the steps have been generated for this tick()
00079         this->is_move_finished = true;
00080         this->step_ticker->moves_finished = true;
00081     }
00082 
00083 }
00084 
00085 
00086 // If the move is finished, the StepTicker will call this ( because we asked it to in tick() )
00087 void StepperMotor::signal_move_finished(){
00088 
00089             // work is done ! 8t
00090             this->moving = false;
00091             this->steps_to_move = 0;
00092 
00093             // signal it to whatever cares 41t 411t
00094             this->end_hook->call();
00095 
00096             // We only need to do this if we were not instructed to move
00097             if( this->moving == false ){
00098                 this->update_exit_tick();
00099             }
00100 
00101             this->is_move_finished = false;
00102 }
00103 
00104 // This is just a way not to check for ( !this->moving || this->paused || this->fx_ticks_per_step == 0 ) at every tick()
00105 inline void StepperMotor::update_exit_tick(){
00106     if( !this->moving || this->paused || this->steps_to_move == 0 ){
00107         // We must exit tick() after setting the pins, no bresenham is done
00108         //this->remove_from_active_list_next_reset = true;
00109         this->step_ticker->remove_motor_from_active_list(this);
00110     }else{
00111         // We must do the bresenham in tick()
00112         // We have to do this or there could be a bug where the removal still happens when it doesn't need to
00113         this->step_ticker->add_motor_to_active_list(this);
00114     }
00115 }
00116 
00117 
00118 
00119 // Instruct the StepperMotor to move a certain number of steps
00120 void StepperMotor::move( bool direction, unsigned int steps ){
00121     // We do not set the direction directly, we will set the pin just before the step pin on the next tick
00122     this->dir_pin.set(direction);
00123     this->direction = direction;
00124 
00125     // How many steps we have to move until the move is done
00126     this->steps_to_move = steps;
00127 
00128     // Zero our tool counters
00129     this->fx_counter = 0;      // Bresenheim counter
00130     this->stepped = 0;
00131 
00132     // Do not signal steps until we get instructed to
00133     this->signal_step = false;
00134 
00135     // Starting now we are moving
00136     if( steps > 0 ){
00137         this->moving = true;
00138     }else{
00139         this->moving = false;
00140     }
00141     this->update_exit_tick();
00142 
00143 }
00144 
00145 // Set the speed at which this steper moves
00146 void StepperMotor::set_speed( float speed ){
00147 
00148     if (speed < 20.0)
00149         speed = 20.0;
00150 
00151     // How many steps we must output per second
00152     this->steps_per_second = speed;
00153 
00154     // How many ticks ( base steps ) between each actual step at this speed, in fixed point 64
00155     float ticks_per_step = (float)( (float)this->step_ticker->frequency / speed );
00156     float double_fx_ticks_per_step = (float)(1<<8) * ( (float)(1<<8) * ticks_per_step ); // 8x8 because we had to do 16x16 because 32 did not work
00157     this->fx_ticks_per_step = (uint32_t)( floor(double_fx_ticks_per_step) );
00158 
00159 }
00160 
00161 // Pause this stepper motor
00162 void StepperMotor::pause(){
00163     this->paused = true;
00164     this->update_exit_tick();
00165 }
00166 
00167 // Unpause this stepper motor
00168 void StepperMotor::unpause(){
00169     this->paused = false;
00170     this->update_exit_tick();
00171 }
00172 
00173 
00174 void StepperMotor::change_steps_per_mm(float new_steps)
00175 {
00176     steps_per_mm = new_steps;
00177     last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
00178 }
00179 
00180 void StepperMotor::change_last_milestone(float new_milestone)
00181 {
00182     last_milestone_mm = new_milestone;
00183     last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
00184 }
00185 
00186 int  StepperMotor::steps_to_target(float target)
00187 {
00188     int target_steps = lround(target * steps_per_mm);
00189     return target_steps - last_milestone_steps;
00190 }