refactor G4 out of slowtickerinto robot,and to not use pauser. Just drain queue and...

diff --git a/src/libs/SlowTicker.cpp b/src/libs/SlowTicker.cpp
index 14a0288..e943878 100644 (file)
--- a/src/libs/SlowTicker.cpp
+++ b/src/libs/SlowTicker.cpp
@@ -36,9 +36,6 @@ SlowTicker::SlowTicker(){
     flag_1s_flag = 0;
     flag_1s_count = SystemCoreClock>>2;
 
-    g4_ticks = 0;
-    g4_pause = false;
-
     // Configure the actual timer after setup to avoid race conditions
     LPC_SC->PCONP |= (1 << 22);     // Power Ticker ON
     LPC_TIM2->MR0 = 10000;          // Initial dummy value for Match Register
@@ -49,8 +46,6 @@ SlowTicker::SlowTicker(){
 
 void SlowTicker::on_module_loaded(){
     register_for_event(ON_IDLE);
-    register_for_event(ON_GCODE_RECEIVED);
-    register_for_event(ON_GCODE_EXECUTE);
 }
 
 // Set the base frequency we use for all sub-frequencies
@@ -86,16 +81,6 @@ void SlowTicker::tick(){
         flag_1s_flag++;
     }
 
-    // if we're counting down a pause
-    if (g4_ticks > 0)
-    {
-        // deduct tick time from timeout
-        if (g4_ticks > interval)
-            g4_ticks -= interval;
-        else
-            g4_ticks = 0;
-    }
-
     // Enter MRI mode if the ISP button is pressed
     // TODO: This should have it's own module
     if (ispbtn.get() == 0)
@@ -136,52 +121,6 @@ void SlowTicker::on_idle(void*)
     if (flag_1s())
         // fire the on_second_tick event
         THEKERNEL->call_event(ON_SECOND_TICK);
-
-    // if G4 has finished, release our pause
-    if (g4_pause && (g4_ticks == 0))
-    {
-        g4_pause = false;
-        THEKERNEL->pauser->release();
-    }
-}
-
-// When a G4-type gcode is received, add it to the queue so we can execute it in time
-void SlowTicker::on_gcode_received(void* argument){
-    Gcode* gcode = static_cast<Gcode*>(argument);
-    // Add the gcode to the queue ourselves if we need it
-    if( gcode->has_g && gcode->g == 4 ){
-        THEKERNEL->conveyor->append_gcode(gcode);
-        // ensure that no subsequent gcodes get executed along with our G4
-        THEKERNEL->conveyor->queue_head_block();
-    }
-}
-
-// When a G4-type gcode is executed, start the pause
-void SlowTicker::on_gcode_execute(void* argument){
-    Gcode* gcode = static_cast<Gcode*>(argument);
-
-    if (gcode->has_g){
-        if (gcode->g == 4){
-            gcode->mark_as_taken();
-            bool updated = false;
-            if (gcode->has_letter('P')) {
-                updated = true;
-                g4_ticks += gcode->get_int('P') * ((SystemCoreClock >> 2) / 1000UL);
-            }
-            if (gcode->has_letter('S')) {
-                updated = true;
-                g4_ticks += gcode->get_int('S') * (SystemCoreClock >> 2);
-            }
-            if (updated){
-                // G4 Smm Pnn should pause for mm seconds + nn milliseconds
-                // at 120MHz core clock, the longest possible delay is (2^32 / (120MHz / 4)) = 143 seconds
-                if (!g4_pause){
-                    g4_pause = true;
-                    THEKERNEL->pauser->take();
-                }
-            }
-        }
-    }
 }
 
 extern "C" void TIMER2_IRQHandler (void){

diff --git a/src/libs/SlowTicker.h b/src/libs/SlowTicker.h
index 242273b..091f931 100644 (file)
--- a/src/libs/SlowTicker.h
+++ b/src/libs/SlowTicker.h
@@ -27,8 +27,6 @@ class SlowTicker : public Module{
 
         void on_module_loaded(void);
         void on_idle(void*);
-        void on_gcode_received(void*);
-        void on_gcode_execute(void*);
 
         void set_frequency( int frequency );
         void tick();
@@ -58,9 +56,6 @@ class SlowTicker : public Module{
         uint32_t max_frequency;
         uint32_t interval;
 
-        uint32_t g4_ticks;
-        bool     g4_pause;
-
         Pin ispbtn;
 protected:
     int flag_1s_count;

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index 293c78c..9fddde5 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -8,6 +8,8 @@
 #include "libs/Module.h"
 #include "libs/Kernel.h"
 
+#include "mbed.h" // for us_ticker_read()
+
 #include <math.h>
 #include <string>
 using std::string;
@@ -335,6 +337,25 @@ void Robot::on_gcode_received(void *argument)
             case 1:  this->motion_mode = MOTION_MODE_LINEAR; gcode->mark_as_taken();  break;
             case 2:  this->motion_mode = MOTION_MODE_CW_ARC; gcode->mark_as_taken();  break;
             case 3:  this->motion_mode = MOTION_MODE_CCW_ARC; gcode->mark_as_taken();  break;
+            case 4: {
+                uint32_t delay_ms= 0;
+                if (gcode->has_letter('P')) {
+                    delay_ms= gcode->get_int('P');
+                }
+                if (gcode->has_letter('S')) {
+                    delay_ms += gcode->get_int('S') * 1000;
+                }
+                if (delay_ms > 0){
+                    uint32_t start= us_ticker_read(); // mbed call
+                    // drain queue
+                    THEKERNEL->conveyor->wait_for_empty_queue();
+                    // wait for specified time
+                    while ((us_ticker_read() - start) < delay_ms*1000) {
+                        THEKERNEL->call_event(ON_IDLE, this);
+                    }
+                }
+                gcode->mark_as_taken();
+            }
             case 17: this->select_plane(X_AXIS, Y_AXIS, Z_AXIS); gcode->mark_as_taken();  break;
             case 18: this->select_plane(X_AXIS, Z_AXIS, Y_AXIS); gcode->mark_as_taken();  break;
             case 19: this->select_plane(Y_AXIS, Z_AXIS, X_AXIS); gcode->mark_as_taken();  break;