SlowTicker: implement G4 pause

diff --git a/src/libs/SlowTicker.cpp b/src/libs/SlowTicker.cpp
index 1461aef..168c508 100644 (file)
--- a/src/libs/SlowTicker.cpp
+++ b/src/libs/SlowTicker.cpp
@@ -18,7 +18,7 @@ using namespace std;
 SlowTicker* global_slow_ticker;
 
 SlowTicker::SlowTicker(){
-    this->max_frequency = 0;
+    max_frequency = 0;
     global_slow_ticker = this;
     LPC_SC->PCONP |= (1 << 22);     // Power Ticker ON
     LPC_TIM2->MR0 = 10000;        // Initial dummy value for Match Register
@@ -30,15 +30,19 @@ SlowTicker::SlowTicker(){
 
     flag_1s_flag = 0;
     flag_1s_count = SystemCoreClock;
+
+    g4_ticks = 0;
+    g4_pause = false;
 }
 
 void SlowTicker::on_module_loaded()
 {
     register_for_event(ON_IDLE);
+    register_for_event(ON_GCODE_EXECUTE);
 }
 
 void SlowTicker::set_frequency( int frequency ){
-    this->interval = int(floor((SystemCoreClock >> 2)/frequency));   // SystemCoreClock/4 = Timer increments in a second
+    this->interval = (SystemCoreClock >> 2) / frequency;   // SystemCoreClock/4 = Timer increments in a second
     LPC_TIM2->MR0 = this->interval;
     LPC_TIM2->TCR = 3;  // Reset
     LPC_TIM2->TCR = 1;  // Reset
@@ -51,7 +55,7 @@ void SlowTicker::tick()
     LPC_GPIO1->FIODIR |= 1<<20;
     LPC_GPIO1->FIOSET = 1<<20;
 
-    for (unsigned int i=0; i<this->hooks.size(); i++){
+    for (uint32_t i=0; i<this->hooks.size(); i++){
         Hook* hook = this->hooks.at(i);
         hook->countdown -= this->interval;
         if (hook->countdown < 0)
@@ -68,6 +72,14 @@ void SlowTicker::tick()
         flag_1s_flag++;
     }
 
+    if (g4_ticks > 0)
+    {
+        if (g4_ticks > interval)
+            g4_ticks -= interval;
+        else
+            g4_ticks = 0;
+    }
+
     LPC_GPIO1->FIOCLR = 1<<20;
 
     if (ispbtn.get() == 0)
@@ -92,6 +104,40 @@ void SlowTicker::on_idle(void*)
 {
     if (flag_1s())
         kernel->call_event(ON_SECOND_TICK);
+
+    // if G4 has finished, release our pause
+    if (g4_pause && (g4_ticks == 0))
+    {
+        g4_pause = false;
+        kernel->pauser->release();
+    }
+}
+
+void SlowTicker::on_gcode_execute(void* argument)
+{
+    Gcode* gcode = static_cast<Gcode*>(argument);
+
+    if (gcode->has_g)
+    {
+        if (gcode->g == 4)
+        {
+            if (gcode->has_letter('P'))
+            {
+                // G4 Pnn should pause for nn milliseconds
+                // at 120MHz core clock, the longest possible delay is (2^32 / (120MHz / 4)) = 143 seconds
+                if (g4_pause)
+                {
+                    g4_ticks += gcode->get_int('P') * ((SystemCoreClock >> 2) / 1000UL);
+                }
+                else
+                {
+                    g4_ticks = gcode->get_int('P') * ((SystemCoreClock >> 2) / 1000UL);
+                    g4_pause = true;
+                    kernel->pauser->take();
+                }
+            }
+        }
+    }
 }
 
 extern "C" void TIMER2_IRQHandler (void){

diff --git a/src/libs/SlowTicker.h b/src/libs/SlowTicker.h
index a288864..cde1ad2 100644 (file)
--- a/src/libs/SlowTicker.h
+++ b/src/libs/SlowTicker.h
@@ -28,11 +28,12 @@ class SlowTicker : public Module{
 
         void on_module_loaded(void);
         void on_idle(void*);
+        void on_gcode_execute(void*);
 
         void set_frequency( int frequency );
         void tick();
         // For some reason this can't go in the .cpp, see :  http://mbed.org/forum/mbed/topic/2774/?page=1#comment-14221
-        template<typename T> Hook* attach( int frequency, T *optr, uint32_t ( T::*fptr )( uint32_t ) ){
+        template<typename T> Hook* attach( uint32_t frequency, T *optr, uint32_t ( T::*fptr )( uint32_t ) ){
             Hook* hook = new Hook();
             hook->interval = int(floor((SystemCoreClock/4)/frequency));
             hook->attach(optr, fptr);
@@ -48,8 +49,11 @@ class SlowTicker : public Module{
         bool flag_1s();
 
         vector<Hook*> hooks;
-        int max_frequency;
-        int interval;
+        uint32_t max_frequency;
+        uint32_t interval;
+
+        uint32_t g4_ticks;
+        bool     g4_pause;
 
         Pin ispbtn;
 protected: