From 18ca10a33a1ecb389a234eceea3631f2751aea6d Mon Sep 17 00:00:00 2001
From: Jim Morris <morris@wolfman.com>
Date: Sun, 30 Sep 2018 15:19:20 +0100
Subject: [PATCH] add $J for instant jog of one axis.   $J X0.01 F0.5   moves
 0.01 mm in X at 0.5 of the max actuator speed for the X axis

---
 src/modules/robot/Conveyor.h                  |  1 +
 src/modules/utils/simpleshell/SimpleShell.cpp | 57 ++++++++++++++++++-
 src/modules/utils/simpleshell/SimpleShell.h   |  3 +
 3 files changed, 60 insertions(+), 1 deletion(-)

diff --git a/src/modules/robot/Conveyor.h b/src/modules/robot/Conveyor.h
index 9bfbb54f..14181889 100644
--- a/src/modules/robot/Conveyor.h
+++ b/src/modules/robot/Conveyor.h
@@ -34,6 +34,7 @@ public:
     void dump_queue(void);
     void flush_queue(void);
     float get_current_feedrate() const { return current_feedrate; }
+    void force_queue() { check_queue(true); }
 
     friend class Planner; // for queue
 
diff --git a/src/modules/utils/simpleshell/SimpleShell.cpp b/src/modules/utils/simpleshell/SimpleShell.cpp
index 3821d14f..e05942ab 100644
--- a/src/modules/utils/simpleshell/SimpleShell.cpp
+++ b/src/modules/utils/simpleshell/SimpleShell.cpp
@@ -12,7 +12,7 @@
 #include "libs/utils.h"
 #include "libs/SerialMessage.h"
 #include "libs/StreamOutput.h"
-#include "modules/robot/Conveyor.h"
+#include "Conveyor.h"
 #include "DirHandle.h"
 #include "mri.h"
 #include "version.h"
@@ -244,6 +244,11 @@ void SimpleShell::on_console_line_received( void *argument )
                 new_message.stream->printf("ok\n");
                 break;
 
+            case 'J':
+                // instant jog command
+                jog(possible_command, new_message.stream);
+                break;
+
             default:
                 new_message.stream->printf("error:Invalid statement\n");
                 break;
@@ -1131,6 +1136,56 @@ void SimpleShell::test_command( string parameters, StreamOutput *stream)
     }
 }
 
+void SimpleShell::jog(string parameters, StreamOutput *stream)
+{
+    // $J X0.1 F0.5
+    int n_motors= THEROBOT->get_number_registered_motors();
+
+    // get axis to move and amount (X0.1)
+    // for now always 1 axis
+    size_t npos= parameters.find_first_of("XYZABC");
+    if(npos == string::npos) {
+        stream->printf("usage: $J X|Y|Z|A|B|C 0.01 [F0.5]\n");
+        return;
+    }
+
+    string s = parameters.substr(npos);
+    if(s.empty() || s.size() < 2) {
+        stream->printf("usage: $J X0.01 [F0.5]\n");
+        return;
+    }
+    char ax= toupper(s[0]);
+    uint8_t a= ax >= 'X' ? ax - 'X' : ax - 'A' + 3;
+    if(a >= n_motors) {
+        stream->printf("error:bad axis\n");
+        return;
+    }
+
+    float d= strtof(s.substr(1).c_str(), NULL);
+
+    float delta[n_motors];
+    for (int i = 0; i < n_motors; ++i) {
+        delta[i]= 0;
+    }
+    delta[a]= d;
+
+    // get speed scale
+    float scale= 1.0F;
+    npos= parameters.find_first_of("F");
+    if(npos != string::npos && npos+1 < parameters.size()) {
+        scale= strtof(parameters.substr(npos+1).c_str(), NULL);
+    }
+
+    THEROBOT->push_state();
+    float rate_mm_s= THEROBOT->actuators[a]->get_max_rate() * scale;
+    THEROBOT->delta_move(delta, rate_mm_s, n_motors);
+
+    // turn off queue delay and run it now
+    THECONVEYOR->force_queue();
+    THEROBOT->pop_state();
+    stream->printf("Jog: %c%f F%f\n", ax, d, scale);
+}
+
 void SimpleShell::help_command( string parameters, StreamOutput *stream )
 {
     stream->printf("Commands:\r\n");
diff --git a/src/modules/utils/simpleshell/SimpleShell.h b/src/modules/utils/simpleshell/SimpleShell.h
index a1d3be30..5f853463 100644
--- a/src/modules/utils/simpleshell/SimpleShell.h
+++ b/src/modules/utils/simpleshell/SimpleShell.h
@@ -29,6 +29,9 @@ public:
     static void version_command(string parameters, StreamOutput *stream );
 
 private:
+
+    void jog(string params, StreamOutput *stream);
+
     static void ls_command(string parameters, StreamOutput *stream );
     static void cd_command(string parameters, StreamOutput *stream );
     static void delete_file_command(string parameters, StreamOutput *stream );
-- 
2.20.1