Michael Spencer / Smoothie

Fork of Smoothie to port to mbed non-LPC targets.

Dependencies:   mbed

Fork of Smoothie by Stéphane Cachat

libs/StepperMotor.cpp@3:f151d08d335c, 2014-03-02 (annotated)

Committer:
Bigcheese
Date:
Sun Mar 02 06:33:08 2014 +0000
Revision:
3:f151d08d335c
Parent:
2:1df0b61d3b5a 
Bunch of stuff. Need to locally merge in updated USB changes.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Michael J. Spencer 2:1df0b61d3b5a 1 /*
Michael J. Spencer 2:1df0b61d3b5a 2 This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
Michael J. Spencer 2:1df0b61d3b5a 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.
Michael J. Spencer 2:1df0b61d3b5a 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.
Michael J. Spencer 2:1df0b61d3b5a 5 You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
Michael J. Spencer 2:1df0b61d3b5a 6 */
Michael J. Spencer 2:1df0b61d3b5a 7 #include "StepperMotor.h"
Michael J. Spencer 2:1df0b61d3b5a 8
Michael J. Spencer 2:1df0b61d3b5a 9 #include "Kernel.h"
Michael J. Spencer 2:1df0b61d3b5a 10 #include "MRI_Hooks.h"
Michael J. Spencer 2:1df0b61d3b5a 11
Michael J. Spencer 2:1df0b61d3b5a 12 // A StepperMotor represents an actual stepper motor. It is used to generate steps that move the actual motor at a given speed
Michael J. Spencer 2:1df0b61d3b5a 13 // TODO : Abstract this into Actuator
Michael J. Spencer 2:1df0b61d3b5a 14
Michael J. Spencer 2:1df0b61d3b5a 15 StepperMotor::StepperMotor(){
Michael J. Spencer 2:1df0b61d3b5a 16 this->moving = false;
Michael J. Spencer 2:1df0b61d3b5a 17 this->paused = false;
Michael J. Spencer 2:1df0b61d3b5a 18 this->fx_counter = 0;
Michael J. Spencer 2:1df0b61d3b5a 19 this->stepped = 0;
Michael J. Spencer 2:1df0b61d3b5a 20 this->fx_ticks_per_step = 0;
Michael J. Spencer 2:1df0b61d3b5a 21 this->steps_to_move = 0;
Michael J. Spencer 2:1df0b61d3b5a 22 this->remove_from_active_list_next_reset = false;
Michael J. Spencer 2:1df0b61d3b5a 23 this->is_move_finished = false;
Michael J. Spencer 2:1df0b61d3b5a 24 this->signal_step = false;
Michael J. Spencer 2:1df0b61d3b5a 25 this->step_signal_hook = new Hook();
Michael J. Spencer 2:1df0b61d3b5a 26
Michael J. Spencer 2:1df0b61d3b5a 27 steps_per_mm = 1.0F;
Michael J. Spencer 2:1df0b61d3b5a 28 max_rate = 50.0F;
Michael J. Spencer 2:1df0b61d3b5a 29
Michael J. Spencer 2:1df0b61d3b5a 30 last_milestone_steps = 0;
Michael J. Spencer 2:1df0b61d3b5a 31 last_milestone_mm = 0.0F;
Michael J. Spencer 2:1df0b61d3b5a 32 }
Michael J. Spencer 2:1df0b61d3b5a 33
Michael J. Spencer 2:1df0b61d3b5a 34 StepperMotor::StepperMotor(Pin& step, Pin& dir, Pin& en) : step_pin(step), dir_pin(dir), en_pin(en) {
Michael J. Spencer 2:1df0b61d3b5a 35 this->moving = false;
Michael J. Spencer 2:1df0b61d3b5a 36 this->paused = false;
Michael J. Spencer 2:1df0b61d3b5a 37 this->fx_counter = 0;
Michael J. Spencer 2:1df0b61d3b5a 38 this->stepped = 0;
Michael J. Spencer 2:1df0b61d3b5a 39 this->fx_ticks_per_step = 0;
Michael J. Spencer 2:1df0b61d3b5a 40 this->steps_to_move = 0;
Michael J. Spencer 2:1df0b61d3b5a 41 this->remove_from_active_list_next_reset = false;
Michael J. Spencer 2:1df0b61d3b5a 42 this->is_move_finished = false;
Michael J. Spencer 2:1df0b61d3b5a 43 this->signal_step = false;
Michael J. Spencer 2:1df0b61d3b5a 44 this->step_signal_hook = new Hook();
Michael J. Spencer 2:1df0b61d3b5a 45
Michael J. Spencer 2:1df0b61d3b5a 46 enable(false);
Michael J. Spencer 2:1df0b61d3b5a 47 set_high_on_debug(en.port_number, en.pin);
Michael J. Spencer 2:1df0b61d3b5a 48
Michael J. Spencer 2:1df0b61d3b5a 49 steps_per_mm = 1.0F;
Michael J. Spencer 2:1df0b61d3b5a 50 max_rate = 50.0F;
Michael J. Spencer 2:1df0b61d3b5a 51
Michael J. Spencer 2:1df0b61d3b5a 52 last_milestone_steps = 0;
Michael J. Spencer 2:1df0b61d3b5a 53 last_milestone_mm = 0.0F;
Michael J. Spencer 2:1df0b61d3b5a 54 }
Michael J. Spencer 2:1df0b61d3b5a 55
Michael J. Spencer 2:1df0b61d3b5a 56 // 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
Michael J. Spencer 2:1df0b61d3b5a 57 // we also here check if the move is finished etc ...
Michael J. Spencer 2:1df0b61d3b5a 58 void StepperMotor::step(){
Michael J. Spencer 2:1df0b61d3b5a 59
Michael J. Spencer 2:1df0b61d3b5a 60 // output to pins 37t
Michael J. Spencer 2:1df0b61d3b5a 61 this->step_pin.set( 1 );
Michael J. Spencer 2:1df0b61d3b5a 62 this->step_ticker->reset_step_pins = true;
Michael J. Spencer 2:1df0b61d3b5a 63
Michael J. Spencer 2:1df0b61d3b5a 64 // move counter back 11t
Michael J. Spencer 2:1df0b61d3b5a 65 this->fx_counter -= this->fx_ticks_per_step;
Michael J. Spencer 2:1df0b61d3b5a 66
Michael J. Spencer 2:1df0b61d3b5a 67 // we have moved a step 9t
Michael J. Spencer 2:1df0b61d3b5a 68 this->stepped++;
Michael J. Spencer 2:1df0b61d3b5a 69
Michael J. Spencer 2:1df0b61d3b5a 70 // Do we need to signal this step
Michael J. Spencer 2:1df0b61d3b5a 71 if( this->stepped == this->signal_step_number && this->signal_step ){
Michael J. Spencer 2:1df0b61d3b5a 72 this->step_signal_hook->call();
Michael J. Spencer 2:1df0b61d3b5a 73 }
Michael J. Spencer 2:1df0b61d3b5a 74
Michael J. Spencer 2:1df0b61d3b5a 75 // Is this move finished ?
Michael J. Spencer 2:1df0b61d3b5a 76 if( this->stepped == this->steps_to_move ){
Michael J. Spencer 2:1df0b61d3b5a 77 // Mark it as finished, then StepTicker will call signal_mode_finished()
Michael J. Spencer 2:1df0b61d3b5a 78 // This is so we don't call that before all the steps have been generated for this tick()
Michael J. Spencer 2:1df0b61d3b5a 79 this->is_move_finished = true;
Michael J. Spencer 2:1df0b61d3b5a 80 this->step_ticker->moves_finished = true;
Michael J. Spencer 2:1df0b61d3b5a 81 }
Michael J. Spencer 2:1df0b61d3b5a 82
Michael J. Spencer 2:1df0b61d3b5a 83 }
Michael J. Spencer 2:1df0b61d3b5a 84
Michael J. Spencer 2:1df0b61d3b5a 85
Michael J. Spencer 2:1df0b61d3b5a 86 // If the move is finished, the StepTicker will call this ( because we asked it to in tick() )
Michael J. Spencer 2:1df0b61d3b5a 87 void StepperMotor::signal_move_finished(){
Michael J. Spencer 2:1df0b61d3b5a 88
Michael J. Spencer 2:1df0b61d3b5a 89 // work is done ! 8t
Michael J. Spencer 2:1df0b61d3b5a 90 this->moving = false;
Michael J. Spencer 2:1df0b61d3b5a 91 this->steps_to_move = 0;
Michael J. Spencer 2:1df0b61d3b5a 92
Michael J. Spencer 2:1df0b61d3b5a 93 // signal it to whatever cares 41t 411t
Michael J. Spencer 2:1df0b61d3b5a 94 this->end_hook->call();
Michael J. Spencer 2:1df0b61d3b5a 95
Michael J. Spencer 2:1df0b61d3b5a 96 // We only need to do this if we were not instructed to move
Michael J. Spencer 2:1df0b61d3b5a 97 if( this->moving == false ){
Michael J. Spencer 2:1df0b61d3b5a 98 this->update_exit_tick();
Michael J. Spencer 2:1df0b61d3b5a 99 }
Michael J. Spencer 2:1df0b61d3b5a 100
Michael J. Spencer 2:1df0b61d3b5a 101 this->is_move_finished = false;
Michael J. Spencer 2:1df0b61d3b5a 102 }
Michael J. Spencer 2:1df0b61d3b5a 103
Michael J. Spencer 2:1df0b61d3b5a 104 // This is just a way not to check for ( !this->moving || this->paused || this->fx_ticks_per_step == 0 ) at every tick()
Michael J. Spencer 2:1df0b61d3b5a 105 inline void StepperMotor::update_exit_tick(){
Michael J. Spencer 2:1df0b61d3b5a 106 if( !this->moving || this->paused || this->steps_to_move == 0 ){
Michael J. Spencer 2:1df0b61d3b5a 107 // We must exit tick() after setting the pins, no bresenham is done
Michael J. Spencer 2:1df0b61d3b5a 108 //this->remove_from_active_list_next_reset = true;
Michael J. Spencer 2:1df0b61d3b5a 109 this->step_ticker->remove_motor_from_active_list(this);
Michael J. Spencer 2:1df0b61d3b5a 110 }else{
Michael J. Spencer 2:1df0b61d3b5a 111 // We must do the bresenham in tick()
Michael J. Spencer 2:1df0b61d3b5a 112 // We have to do this or there could be a bug where the removal still happens when it doesn't need to
Michael J. Spencer 2:1df0b61d3b5a 113 this->step_ticker->add_motor_to_active_list(this);
Michael J. Spencer 2:1df0b61d3b5a 114 }
Michael J. Spencer 2:1df0b61d3b5a 115 }
Michael J. Spencer 2:1df0b61d3b5a 116
Michael J. Spencer 2:1df0b61d3b5a 117
Michael J. Spencer 2:1df0b61d3b5a 118
Michael J. Spencer 2:1df0b61d3b5a 119 // Instruct the StepperMotor to move a certain number of steps
Michael J. Spencer 2:1df0b61d3b5a 120 void StepperMotor::move( bool direction, unsigned int steps ){
Michael J. Spencer 2:1df0b61d3b5a 121 // We do not set the direction directly, we will set the pin just before the step pin on the next tick
Michael J. Spencer 2:1df0b61d3b5a 122 this->dir_pin.set(direction);
Michael J. Spencer 2:1df0b61d3b5a 123 this->direction = direction;
Michael J. Spencer 2:1df0b61d3b5a 124
Michael J. Spencer 2:1df0b61d3b5a 125 // How many steps we have to move until the move is done
Michael J. Spencer 2:1df0b61d3b5a 126 this->steps_to_move = steps;
Michael J. Spencer 2:1df0b61d3b5a 127
Michael J. Spencer 2:1df0b61d3b5a 128 // Zero our tool counters
Michael J. Spencer 2:1df0b61d3b5a 129 this->fx_counter = 0; // Bresenheim counter
Michael J. Spencer 2:1df0b61d3b5a 130 this->stepped = 0;
Michael J. Spencer 2:1df0b61d3b5a 131
Michael J. Spencer 2:1df0b61d3b5a 132 // Do not signal steps until we get instructed to
Michael J. Spencer 2:1df0b61d3b5a 133 this->signal_step = false;
Michael J. Spencer 2:1df0b61d3b5a 134
Michael J. Spencer 2:1df0b61d3b5a 135 // Starting now we are moving
Michael J. Spencer 2:1df0b61d3b5a 136 if( steps > 0 ){
Michael J. Spencer 2:1df0b61d3b5a 137 this->moving = true;
Michael J. Spencer 2:1df0b61d3b5a 138 }else{
Michael J. Spencer 2:1df0b61d3b5a 139 this->moving = false;
Michael J. Spencer 2:1df0b61d3b5a 140 }
Michael J. Spencer 2:1df0b61d3b5a 141 this->update_exit_tick();
Michael J. Spencer 2:1df0b61d3b5a 142
Michael J. Spencer 2:1df0b61d3b5a 143 }
Michael J. Spencer 2:1df0b61d3b5a 144
Michael J. Spencer 2:1df0b61d3b5a 145 // Set the speed at which this steper moves
Michael J. Spencer 2:1df0b61d3b5a 146 void StepperMotor::set_speed( float speed ){
Michael J. Spencer 2:1df0b61d3b5a 147
Michael J. Spencer 2:1df0b61d3b5a 148 if (speed < 20.0)
Michael J. Spencer 2:1df0b61d3b5a 149 speed = 20.0;
Michael J. Spencer 2:1df0b61d3b5a 150
Michael J. Spencer 2:1df0b61d3b5a 151 // How many steps we must output per second
Michael J. Spencer 2:1df0b61d3b5a 152 this->steps_per_second = speed;
Michael J. Spencer 2:1df0b61d3b5a 153
Michael J. Spencer 2:1df0b61d3b5a 154 // How many ticks ( base steps ) between each actual step at this speed, in fixed point 64
Michael J. Spencer 2:1df0b61d3b5a 155 float ticks_per_step = (float)( (float)this->step_ticker->frequency / speed );
Michael J. Spencer 2:1df0b61d3b5a 156 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
Michael J. Spencer 2:1df0b61d3b5a 157 this->fx_ticks_per_step = (uint32_t)( floor(double_fx_ticks_per_step) );
Michael J. Spencer 2:1df0b61d3b5a 158
Michael J. Spencer 2:1df0b61d3b5a 159 }
Michael J. Spencer 2:1df0b61d3b5a 160
Michael J. Spencer 2:1df0b61d3b5a 161 // Pause this stepper motor
Michael J. Spencer 2:1df0b61d3b5a 162 void StepperMotor::pause(){
Michael J. Spencer 2:1df0b61d3b5a 163 this->paused = true;
Michael J. Spencer 2:1df0b61d3b5a 164 this->update_exit_tick();
Michael J. Spencer 2:1df0b61d3b5a 165 }
Michael J. Spencer 2:1df0b61d3b5a 166
Michael J. Spencer 2:1df0b61d3b5a 167 // Unpause this stepper motor
Michael J. Spencer 2:1df0b61d3b5a 168 void StepperMotor::unpause(){
Michael J. Spencer 2:1df0b61d3b5a 169 this->paused = false;
Michael J. Spencer 2:1df0b61d3b5a 170 this->update_exit_tick();
Michael J. Spencer 2:1df0b61d3b5a 171 }
Michael J. Spencer 2:1df0b61d3b5a 172
Michael J. Spencer 2:1df0b61d3b5a 173
Michael J. Spencer 2:1df0b61d3b5a 174 void StepperMotor::change_steps_per_mm(float new_steps)
Michael J. Spencer 2:1df0b61d3b5a 175 {
Michael J. Spencer 2:1df0b61d3b5a 176 steps_per_mm = new_steps;
Michael J. Spencer 2:1df0b61d3b5a 177 last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
Michael J. Spencer 2:1df0b61d3b5a 178 }
Michael J. Spencer 2:1df0b61d3b5a 179
Michael J. Spencer 2:1df0b61d3b5a 180 void StepperMotor::change_last_milestone(float new_milestone)
Michael J. Spencer 2:1df0b61d3b5a 181 {
Michael J. Spencer 2:1df0b61d3b5a 182 last_milestone_mm = new_milestone;
Michael J. Spencer 2:1df0b61d3b5a 183 last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
Michael J. Spencer 2:1df0b61d3b5a 184 }
Michael J. Spencer 2:1df0b61d3b5a 185
Michael J. Spencer 2:1df0b61d3b5a 186 int StepperMotor::steps_to_target(float target)
Michael J. Spencer 2:1df0b61d3b5a 187 {
Michael J. Spencer 2:1df0b61d3b5a 188 int target_steps = lround(target * steps_per_mm);
Michael J. Spencer 2:1df0b61d3b5a 189 return target_steps - last_milestone_steps;
Michael J. Spencer 2:1df0b61d3b5a 190 }