From: Joseph Coffland <joseph@cauldrondevelopment.com>
Date: Mon, 5 Sep 2016 11:49:41 +0000 (-0700)
Subject: Working on jog
X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=e122466bd9a65762bdc454f733f6ae4b3590e7fe;p=bbctrl-firmware

Working on jog
---

diff --git a/src/machine.c b/src/machine.c
index ef2f556..22410bd 100644
--- a/src/machine.c
+++ b/src/machine.c
@@ -38,9 +38,9 @@
  * Synchronizing command execution:
  *
  * "Synchronous commands" are commands that affect the runtime need
- * to be synchronized with movement. Examples include G4 dwells,
+ * to be synchronized with movement.  Examples include G4 dwells,
  * program stops and ends, and most M commands.  These are queued
- * into the planner queue and execute from the queue. Synchronous
+ * into the planner queue and execute from the queue.  Synchronous
  * commands work like this:
  *
  *   - Call the mach_xxx_xxx() function which will do any input
@@ -192,17 +192,8 @@ machine_t mach = {
 };
 
 
-// Command execution callbacks from planner queue
-static void _exec_offset(float *value, float *flag);
-static void _exec_change_tool(float *value, float *flag);
-static void _exec_select_tool(float *value, float *flag);
-static void _exec_mist_coolant_control(float *value, float *flag);
-static void _exec_flood_coolant_control(float *value, float *flag);
-static void _exec_absolute_origin(float *value, float *flag);
-
 // Machine State functions
 uint32_t mach_get_line() {return mach.gm.line;}
-
 machMotionMode_t mach_get_motion_mode() {return mach.gm.motion_mode;}
 machCoordSystem_t mach_get_coord_system() {return mach.gm.coord_system;}
 machUnitsMode_t mach_get_units_mode() {return mach.gm.units_mode;}
@@ -676,30 +667,9 @@ void mach_set_coord_offsets(machCoordSystem_t coord_system, float offset[],
 }
 
 
-// Representation functions that affect gcode model and are queued to planner
-// (synchronous)
-
 /// G54-G59
 void mach_set_coord_system(machCoordSystem_t coord_system) {
   mach.gm.coord_system = coord_system;
-
-  // pass coordinate system in value[0] element
-  float value[AXES] = {coord_system};
-  // second vector (flags) is not used, so fake it
-  mp_queue_command(_exec_offset, value, value);
-}
-
-
-static void _exec_offset(float *value, float *flag) {
-  // coordinate system is passed in value[0] element
-  uint8_t coord_system = value[0];
-  float offsets[AXES];
-
-  for (int axis = 0; axis < AXES; axis++)
-    offsets[axis] = mach.offset[coord_system][axis] +
-      mach.origin_offset[axis] * (mach.origin_offset_enable ? 1 : 0);
-
-  mp_runtime_set_work_offsets(offsets);
   mach_set_work_offsets(); // set work offsets in the Gcode model
 }
 
@@ -732,6 +702,16 @@ void mach_set_position(int axis, float position) {
 
 
 // G28.3 functions and support
+static void _exec_absolute_origin(float *value, float *flag) {
+  for (int axis = 0; axis < AXES; axis++)
+    if (fp_TRUE(flag[axis])) {
+      mp_runtime_set_position(axis, value[axis]);
+      mach_set_homed(axis, true);  // G28.3 is not considered homed until here
+    }
+
+  mp_runtime_set_steps_from_position();
+}
+
 
 /* G28.3 - model, planner and queue to runtime
  *
@@ -739,10 +719,10 @@ void mach_set_position(int axis, float position) {
  * applies them to all axes where the corresponding position in the
  * flag vector is true (1).
  *
- * This is a 2 step process. The model and planner contexts are set
+ * This is a 2 step process.  The model and planner contexts are set
  * immediately, the runtime command is queued and synchronized with
- * the planner queue. This includes the runtime position and the step
- * recording done by the encoders. At that point any axis that is set
+ * the planner queue.  This includes the runtime position and the step
+ * recording done by the encoders.  At that point any axis that is set
  * is also marked as homed.
  */
 void mach_set_absolute_origin(float origin[], float flag[]) {
@@ -760,17 +740,6 @@ void mach_set_absolute_origin(float origin[], float flag[]) {
 }
 
 
-static void _exec_absolute_origin(float *value, float *flag) {
-  for (int axis = 0; axis < AXES; axis++)
-    if (fp_TRUE(flag[axis])) {
-      mp_runtime_set_position(axis, value[axis]);
-      mach_set_homed(axis, true);  // G28.3 is not considered homed until here
-    }
-
-  mp_runtime_set_steps_from_position();
-}
-
-
 /* G92's behave according to NIST 3.5.18 & LinuxCNC G92
  * http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G92-G92.1-G92.2-G92.3
  */
@@ -784,11 +753,7 @@ void mach_set_origin_offsets(float offset[], float flag[]) {
       mach.origin_offset[axis] = mach.position[axis] -
         mach.offset[mach.gm.coord_system][axis] - TO_MILLIMETERS(offset[axis]);
 
-  // now pass the offset to the callback - setting the coordinate system also
-  // applies the offsets
-  // pass coordinate system in value[0] element
-  float value[AXES] = {mach.gm.coord_system};
-  mp_queue_command(_exec_offset, value, value); // second vector is not used
+  mach_set_work_offsets(); // set work offsets in the Gcode model
 }
 
 
@@ -798,24 +763,21 @@ void mach_reset_origin_offsets() {
   for (int axis = 0; axis < AXES; axis++)
     mach.origin_offset[axis] = 0;
 
-  float value[AXES] = {mach.gm.coord_system};
-  mp_queue_command(_exec_offset, value, value);
+  mach_set_work_offsets(); // set work offsets in the Gcode model
 }
 
 
 /// G92.2
 void mach_suspend_origin_offsets() {
   mach.origin_offset_enable = false;
-  float value[AXES] = {mach.gm.coord_system};
-  mp_queue_command(_exec_offset, value, value);
+  mach_set_work_offsets(); // set work offsets in the Gcode model
 }
 
 
 /// G92.3
 void mach_resume_origin_offsets() {
   mach.origin_offset_enable = true;
-  float value[AXES] = {mach.gm.coord_system};
-  mp_queue_command(_exec_offset, value, value);
+  mach_set_work_offsets(); // set work offsets in the Gcode model
 }
 
 
@@ -833,10 +795,10 @@ stat_t mach_straight_traverse(float target[], float flags[]) {
   // prep and plan the move
   mach_set_work_offsets();         // capture fully resolved offsets to state
   mach.ms.line = mach.gm.line;     // copy line number
-  mp_aline(&mach.ms);              // send the move to the planner
+  status = mp_aline(&mach.ms);     // send the move to the planner
   mach_finalize_move();
 
-  return STAT_OK;
+  return status;
 }
 
 
@@ -909,7 +871,7 @@ void mach_set_path_control(machPathControlMode_t mode) {
 
 /// G4, P parameter (seconds)
 stat_t mach_dwell(float seconds) {
-  return mp_dwell(seconds);
+  return mp_dwell(seconds, mach.gm.line);
 }
 
 
@@ -938,64 +900,44 @@ stat_t mach_straight_feed(float target[], float flags[]) {
 
 // Spindle Functions (4.3.7) see spindle.c, spindle.h
 
-/* Tool Functions (4.3.8)
- *
- * Note: These functions don't actually do anything for now, and there's a bug
- *       where T and M in different blocks don't work correctly
- */
+// Tool Functions (4.3.8)
 
 /// T parameter
-void mach_select_tool(uint8_t tool_select) {
-  float value[AXES] = {tool_select};
-  mp_queue_command(_exec_select_tool, value, value);
-}
-
-
-static void _exec_select_tool(float *value, float *flag) {
-  mach.gm.tool_select = value[0];
-}
+void mach_select_tool(uint8_t tool_select) {mach.gm.tool_select = tool_select;}
 
 
 /// M6 This might become a complete tool change cycle
-void mach_change_tool(uint8_t tool_change) {
-  float value[AXES] = {mach.gm.tool_select};
-  mp_queue_command(_exec_change_tool, value, value);
-}
+void mach_change_tool(uint8_t tool) {mach.gm.tool = tool;}
 
 
-static void _exec_change_tool(float *value, float *flag) {
-  mach.gm.tool = (uint8_t)value[0];
+// Miscellaneous Functions (4.3.9)
+static void _exec_mist_coolant_control(float *value, float *flag) {
+  coolant_set_mist(value[0]);
 }
 
 
-// Miscellaneous Functions (4.3.9)
 /// M7
 void mach_mist_coolant_control(bool mist_coolant) {
+  mach.gm.mist_coolant = mist_coolant;
   float value[AXES] = {mist_coolant};
   mp_queue_command(_exec_mist_coolant_control, value, value);
 }
 
 
-static void _exec_mist_coolant_control(float *value, float *flag) {
-  coolant_set_mist(mach.gm.mist_coolant = value[0]);
+static void _exec_flood_coolant_control(float *value, float *flag) {
+  coolant_set_flood(value[0]);
+  if (!value[0]) coolant_set_mist(false); // M9 special function
 }
 
 
 /// M8, M9
 void mach_flood_coolant_control(bool flood_coolant) {
+  mach.gm.flood_coolant = flood_coolant;
   float value[AXES] = {flood_coolant};
   mp_queue_command(_exec_flood_coolant_control, value, value);
 }
 
 
-static void _exec_flood_coolant_control(float *value, float *flag) {
-  mach.gm.flood_coolant = value[0];
-
-  coolant_set_flood(value[0]);
-  if (!value[0]) coolant_set_mist(false); // M9 special function
-}
-
-
 /* Override enables are kind of a mess in Gcode. This is an attempt to sort
  * them out.  See
  * http://www.linuxcnc.org/docs/2.4/html/gcode_main.html#sec:M50:-Feed-Override
@@ -1065,7 +1007,7 @@ void mach_message(const char *message) {
  * It is extended beyond the NIST spec to handle various situations.
  *
  * mach_program_stop and mach_optional_program_stop are synchronous Gcode
- * commands that are received through the interpreter. They cause all motion
+ * commands that are received through the interpreter.  They cause all motion
  * to stop at the end of the current command, including spindle motion.
  *
  * Note that the stop occurs at the end of the immediately preceding command
@@ -1075,7 +1017,35 @@ void mach_message(const char *message) {
  */
 
 
-/*** _exec_program_end() implements M2 and M30.  End behaviors are defined by
+static void _exec_program_stop(float *value, float *flag) {
+  // Machine should be stopped at this point.  Go into hold so that a start is
+  // needed before executing further instructions.
+  mp_state_holding();
+}
+
+
+/// M0 Queue a program stop
+void mach_program_stop() {
+  float value[AXES] = {0};
+  mp_queue_command(_exec_program_stop, value, value);
+}
+
+
+/// M1
+void mach_optional_program_stop() {
+  // TODO Check for user stop signal
+  mach_program_stop();
+}
+
+
+/// M60
+void mach_pallet_change_stop() {
+  // TODO Emit pallet change signal
+  mach_program_stop();
+}
+
+
+/*** mach_program_end() implements M2 and M30.  End behaviors are defined by
  * NIST 3.6.1 are:
  *
  *    1. Axis offsets are set to zero (like G92.2) and origin offsets are set
@@ -1089,7 +1059,7 @@ void mach_message(const char *message) {
  *    8. The current motion mode is set to G_1 (like G1)
  *    9. Coolant is turned off (like M9)
  *
- * _exec_program_end() implements things slightly differently:
+ * mach_program_end() implements things slightly differently:
  *
  *    1. Axis offsets are set to G92.1 CANCEL offsets
  *       (instead of using G92.2 SUSPEND Offsets)
@@ -1104,7 +1074,10 @@ void mach_message(const char *message) {
  *    9. Coolant is turned off (like M9)
  *    +  Default INCHES or MM units mode is restored
  */
-static void _exec_program_end(float *value, float *flag) {
+
+
+/// M2, M30
+void mach_program_end() {
   mach_reset_origin_offsets();      // G92.1 - we do G91.1 instead of G92.2
   mach_set_coord_system(GCODE_DEFAULT_COORD_SYSTEM);
   mach_set_plane(GCODE_DEFAULT_PLANE);
@@ -1116,40 +1089,6 @@ static void _exec_program_end(float *value, float *flag) {
 }
 
 
-static void _exec_program_stop(float *value, float *flag) {
-  // This is ok because we should be already stopped at this point
-  mp_state_holding();
-}
-
-
-/// M0 Queue a program stop
-void mach_program_stop() {
-  float value[AXES] = {0};
-  mp_queue_command(_exec_program_stop, value, value);
-}
-
-
-/// M1
-void mach_optional_program_stop() {
-  // TODO Check for user stop signal
-  mach_program_stop();
-}
-
-
-/// M60
-void mach_pallet_change_stop() {
-  // TODO Emit pallet change signal
-  mach_program_stop();
-}
-
-
-/// M2, M30
-void mach_program_end() {
-  float value[AXES] = {0};
-  mp_queue_command(_exec_program_end, value, value);
-}
-
-
 /// return ASCII char for axis given the axis number
 char mach_get_axis_char(int8_t axis) {
   char axis_char[] = "XYZABC";
diff --git a/src/plan/dwell.c b/src/plan/dwell.c
index 7c5be81..385f002 100644
--- a/src/plan/dwell.c
+++ b/src/plan/dwell.c
@@ -49,7 +49,7 @@ static stat_t _exec_dwell(mpBuf_t *bf) {
 
 
 /// Queue a dwell
-stat_t mp_dwell(float seconds) {
+stat_t mp_dwell(float seconds, int32_t line) {
   mpBuf_t *bf = mp_get_write_buffer();
 
   // never supposed to fail
@@ -60,7 +60,7 @@ stat_t mp_dwell(float seconds) {
   bf->move_state = MOVE_NEW;
 
   // must be final operation before exit
-  mp_commit_write_buffer(mach_get_line(), MOVE_TYPE_DWELL);
+  mp_commit_write_buffer(line, MOVE_TYPE_DWELL);
 
   return STAT_OK;
 }
diff --git a/src/plan/dwell.h b/src/plan/dwell.h
index 8be10df..9d94555 100644
--- a/src/plan/dwell.h
+++ b/src/plan/dwell.h
@@ -29,4 +29,4 @@
 
 #include "status.h"
 
-stat_t mp_dwell(float seconds);
+stat_t mp_dwell(float seconds, int32_t line);
diff --git a/src/plan/exec.c b/src/plan/exec.c
index d360579..180c905 100644
--- a/src/plan/exec.c
+++ b/src/plan/exec.c
@@ -427,7 +427,6 @@ static stat_t _exec_aline_init(mpBuf_t *bf) {
 
   // Update runtime
   mp_runtime_set_busy(true);
-  mp_runtime_set_line(bf->ms.line);
   mp_runtime_set_work_offsets(bf->ms.work_offset);
 
   // Generate waypoints for position correction at section ends.  This helps
@@ -565,5 +564,8 @@ stat_t mp_exec_move() {
   if (!bf) return STAT_NOOP; // nothing running
   if (!bf->bf_func) return CM_ALARM(STAT_INTERNAL_ERROR);
 
+  // Update runtime
+  mp_runtime_set_line(bf->ms.line);
+
   return bf->bf_func(bf); // move callback
 }
diff --git a/src/plan/jog.c b/src/plan/jog.c
index fa3b4b4..57fbfc6 100644
--- a/src/plan/jog.c
+++ b/src/plan/jog.c
@@ -29,10 +29,8 @@
 
 #include "planner.h"
 #include "buffer.h"
-#include "stepper.h"
-#include "motor.h"
+#include "runtime.h"
 #include "machine.h"
-#include "motor.h"
 #include "state.h"
 #include "config.h"
 
@@ -64,12 +62,13 @@ static stat_t _exec_jog(mpBuf_t *bf) {
       jr.target_velocity[axis] = jr.next_velocity[axis];
 
   // Compute next line segment
-  float travel[AXES]; // In mm
+  float target[AXES];
   const float time = MIN_SEGMENT_TIME; // In minutes
   const float maxDeltaV = JOG_ACCELERATION * time;
+  float velocitySqr = 0;
   bool done = true;
 
-  // Compute new velocities and travel
+  // Compute new axis velocities and target
   for (int axis = 0; axis < AXES; axis++) {
     float targetV = jr.target_velocity[axis] * mach.a[axis].velocity_max;
     float deltaV = targetV - jr.current_velocity[axis];
@@ -78,20 +77,21 @@ static stat_t _exec_jog(mpBuf_t *bf) {
     if (maxDeltaV < fabs(deltaV)) jr.current_velocity[axis] += maxDeltaV * sign;
     else jr.current_velocity[axis] = targetV;
 
-    // Compute travel from velocity
-    travel[axis] = time * jr.current_velocity[axis];
+    if (!fp_ZERO(jr.current_velocity[axis])) done = false;
 
-    if (!fp_ZERO(travel[axis])) done = false;
+    // Compute target from axis velocity
+    target[axis] =
+      mp_runtime_get_position(axis) + time * jr.current_velocity[axis];
+
+    // Accumulate velocities squared
+    velocitySqr += square(jr.current_velocity[axis]);
   }
 
   // Check if we are done
   if (done) {
     // Update machine position
-    for (int motor = 0; motor < MOTORS; motor++) {
-      int axis = motor_get_axis(motor);
-      float steps = motor_get_encoder(axis);
-      mach_set_position(axis, steps / motor_get_steps_per_unit(motor));
-    }
+    for (int axis = 0; axis < AXES; axis++)
+      mach_set_position(axis, mp_runtime_get_work_position(axis));
 
     // Release buffer
     mp_free_run_buffer();
@@ -101,15 +101,7 @@ static stat_t _exec_jog(mpBuf_t *bf) {
     return STAT_NOOP;
   }
 
-  // Convert to steps
-  float steps[MOTORS] = {0};
-  mp_kinematics(travel, steps);
-
-  // Queue segment
-  float error[MOTORS] = {0};
-  st_prep_line(steps, error, time);
-
-  return STAT_OK;
+  return mp_runtime_move_to_target(target, sqrt(velocitySqr), time);
 }
 
 
@@ -128,7 +120,7 @@ uint8_t command_jog(int argc, char *argv[]) {
     else velocity[axis] = 0;
 
   // Reset
-  if (mp_jog_busy()) memset(&jr, 0, sizeof(jr));
+  if (!mp_jog_busy()) memset(&jr, 0, sizeof(jr));
 
   jr.writing = true;
   for (int axis = 0; axis < AXES; axis++)
diff --git a/src/plan/runtime.h b/src/plan/runtime.h
index 66275af..6a00dfa 100644
--- a/src/plan/runtime.h
+++ b/src/plan/runtime.h
@@ -34,13 +34,17 @@
 
 bool mp_runtime_is_busy();
 void mp_runtime_set_busy(bool busy);
+
 int32_t mp_runtime_get_line();
 void mp_runtime_set_line(int32_t line);
+
 float mp_runtime_get_velocity();
+
 void mp_runtime_set_steps_from_position();
 void mp_runtime_set_position(uint8_t axis, const float position);
 float mp_runtime_get_position(uint8_t axis);
 float *mp_runtime_get_position_vector();
 float mp_runtime_get_work_position(uint8_t axis);
 void mp_runtime_set_work_offsets(float offset[]);
+
 stat_t mp_runtime_move_to_target(float target[], float velocity, float time);