Move most handling of steps from arm_solution,Robot,Planner to StepperMotor

[clinton/Smoothieware.git] / src / libs / StepperMotor.cpp

diff --git a/src/libs/StepperMotor.cpp b/src/libs/StepperMotor.cpp
index 316311f..9fb4140 100644 (file)
--- a/src/libs/StepperMotor.cpp
+++ b/src/libs/StepperMotor.cpp
@@ -7,6 +7,10 @@
 #include "mri.h"
 #include "libs/Kernel.h"
 #include "StepperMotor.h"
+#include "MRI_Hooks.h"
+
+// A StepperMotor represents an actual stepper motor. It is used to generate steps that move the actual motor at a given speed
+// TODO : Abstract this into Actuator
 
 StepperMotor::StepperMotor(){
     this->moving = false;
@@ -15,11 +19,13 @@ StepperMotor::StepperMotor(){
     this->stepped = 0;
     this->fx_ticks_per_step = 0;
     this->steps_to_move = 0;
-    //this->direction_bit = 0;
     this->remove_from_active_list_next_reset = false;
     this->is_move_finished = false;
     this->signal_step = false;
     this->step_signal_hook = new Hook();
+
+    last_milestone_steps = 0;
+    last_milestone_mm    = 0.0F;
 }
 
 StepperMotor::StepperMotor(Pin* step, Pin* dir, Pin* en) : step_pin(step), dir_pin(dir), en_pin(en) {
@@ -29,47 +35,47 @@ StepperMotor::StepperMotor(Pin* step, Pin* dir, Pin* en) : step_pin(step), dir_p
     this->stepped = 0;
     this->fx_ticks_per_step = 0;
     this->steps_to_move = 0;
-    //this->direction_bit = 0;
     this->remove_from_active_list_next_reset = false;
     this->is_move_finished = false;
     this->signal_step = false;
     this->step_signal_hook = new Hook();
-}
-
-// Called a great many times per second, to step if we have to now
-void StepperMotor::tick(){
 
-    // increase the ( fixed point ) counter by one tick 11t
-    this->fx_counter += (uint64_t)((uint64_t)1<<32);
+    set_high_on_debug(en->port_number, en->pin);
 
-    // if we are to step now 10t
-    if( this->fx_counter >= this->fx_ticks_per_step ){
+    last_milestone_steps = 0;
+    last_milestone_mm    = 0.0F;
+}
 
-        // output to pins 37t
-        this->step_pin->set( 1                   );
-        this->step_ticker->reset_step_pins = true;
+// 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
+// we also here check if the move is finished etc ...
+void StepperMotor::step(){
 
-        // move counter back 11t
-        this->fx_counter -= this->fx_ticks_per_step;
+    // output to pins 37t
+    this->step_pin->set( 1                   );
+    this->step_ticker->reset_step_pins = true;
 
-        // we have moved a step 9t
-        this->stepped++;
+    // move counter back 11t
+    this->fx_counter -= this->fx_ticks_per_step;
 
-        // Do we need to signal this step
-        if( this->stepped == this->signal_step_number && this->signal_step ){
-            this->step_signal_hook->call();
-        }
+    // we have moved a step 9t
+    this->stepped++;
 
-        // is this move finished ? 11t
-        if( this->stepped == this->steps_to_move ){
-            this->is_move_finished = true;
-            this->step_ticker->moves_finished = true;
-        }
+    // Do we need to signal this step
+    if( this->stepped == this->signal_step_number && this->signal_step ){
+        this->step_signal_hook->call();
+    }
 
+    // Is this move finished ?
+    if( this->stepped == this->steps_to_move ){
+        // Mark it as finished, then StepTicker will call signal_mode_finished() 
+        // This is so we don't call that before all the steps have been generated for this tick()
+        this->is_move_finished = true;
+        this->step_ticker->moves_finished = true;
     }
 
 }
 
+
 // If the move is finished, the StepTicker will call this ( because we asked it to in tick() )
 void StepperMotor::signal_move_finished(){
 
@@ -128,44 +134,49 @@ void StepperMotor::move( bool direction, unsigned int steps ){
 
 }
 
-//#pragma GCC push_options
-//#pragma GCC optimize ("O0")
-
 // Set the speed at which this steper moves
-void StepperMotor::set_speed( double speed ){
-//     if( speed <= 1.0 ){
-//         this->steps_per_second = 0;
-//         this->fx_ticks_per_step = 1ULL<<63;
-//         return;
-//     }
+void StepperMotor::set_speed( float speed ){
 
-    //if( speed < this->steps_per_second ){
-        LPC_GPIO1->FIOSET = 1<<19;
-    //}
-
-    if (speed < 1.0)
-        speed = 1.0;
+    if (speed < 20.0)
+        speed = 20.0;
 
     // How many steps we must output per second
     this->steps_per_second = speed;
 
     // How many ticks ( base steps ) between each actual step at this speed, in fixed point 64
-    double ticks_per_step = (double)( (double)this->step_ticker->frequency / speed );
-    double double_fx_ticks_per_step = (double)(1<<16) * ( (double)(1<<16) * ticks_per_step );
-    this->fx_ticks_per_step = (uint64_t)( floor(double_fx_ticks_per_step) );
+    float ticks_per_step = (float)( (float)this->step_ticker->frequency / speed );
+    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
+    this->fx_ticks_per_step = (uint32_t)( floor(double_fx_ticks_per_step) );
 
 }
 
-//#pragma GCC pop_options
-
+// Pause this stepper motor
 void StepperMotor::pause(){
     this->paused = true;
     this->update_exit_tick();
 }
 
+// Unpause this stepper motor
 void StepperMotor::unpause(){
     this->paused = false;
     this->update_exit_tick();
 }
 
 
+void StepperMotor::change_steps_per_mm(float new_steps)
+{
+    steps_per_mm = new_steps;
+    last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
+}
+
+void StepperMotor::change_last_milestone(float new_milestone)
+{
+    last_milestone_mm = new_milestone;
+    last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
+}
+
+int  StepperMotor::steps_to_target(float target)
+{
+    int target_steps = target * steps_per_mm;
+    return target_steps - last_milestone_steps;
+}