Implemented axis zeroing. Start AVR in flushing mode, #10

diff --git a/src/command.c b/src/command.c
index d618d828f412ac56114d1a1ca4162c82fe6f4544..16397d2647ff425ae3fb37ff240c508b91059b1c 100644 (file)
--- a/src/command.c
+++ b/src/command.c
@@ -55,31 +55,37 @@
 static char *_cmd = 0;
 
 
-static void _estop()      {estop_trigger(ESTOP_USER);}
-static void _clear()      {estop_clear();}
-static void _pause()      {mp_request_hold();}
-static void _opt_pause()  {} // TODO
-static void _run()        {mp_request_start();}
-static void _step()       {} // TODO
-static void _flush()      {mp_request_flush();}
-static void _report()     {report_request_full();}
-static void _home()       {} // TODO
 static void _reboot()     {hw_request_hard_reset();}
 
 
+static unsigned _parse_axis(uint8_t axis) {
+  switch (axis) {
+  case 'x': return 0; case 'y': return 1; case 'z': return 2;
+  case 'a': return 3; case 'b': return 4; case 'c': return 5;
+  case 'X': return 0; case 'Y': return 1; case 'Z': return 2;
+  case 'A': return 3; case 'B': return 4; case 'C': return 5;
+  default: return axis;
+  }
+}
+
+
 static void command_i2c_cb(i2c_cmd_t cmd, uint8_t *data, uint8_t length) {
   switch (cmd) {
-  case I2C_NULL:                         break;
-  case I2C_ESTOP:          _estop();     break;
-  case I2C_CLEAR:          _clear();     break;
-  case I2C_PAUSE:          _pause();     break;
-  case I2C_OPTIONAL_PAUSE: _opt_pause(); break;
-  case I2C_RUN:            _run();       break;
-  case I2C_STEP:           _step();      break;
-  case I2C_FLUSH:          _flush();     break;
-  case I2C_REPORT:         _report();    break;
-  case I2C_HOME:           _home();      break;
-  case I2C_REBOOT:         _reboot();    break;
+  case I2C_NULL:                                      break;
+  case I2C_ESTOP:          estop_trigger(ESTOP_USER); break;
+  case I2C_CLEAR:          estop_clear();             break;
+  case I2C_PAUSE:          mp_request_hold();         break;
+  case I2C_OPTIONAL_PAUSE:                            break; // TODO
+  case I2C_RUN:            mp_request_start();        break;
+  case I2C_STEP:                                      break; // TODO
+  case I2C_FLUSH:          mp_request_flush();        break;
+  case I2C_REPORT:         report_request_full();     break;
+  case I2C_HOME:                                      break; // TODO
+  case I2C_REBOOT:         _reboot();                 break;
+  case I2C_ZERO:
+    if (length == 0) mach_zero_all();
+    else if (length == 1) mach_zero_axis(_parse_axis(*data));
+    break;
   }
 }
 

diff --git a/src/hardware.c b/src/hardware.c
index d31a382888fdb09653ebe9e17cc2fca815f15493..65c5c8fbefe704af369612c1d94f7719c6d369e1 100644 (file)
--- a/src/hardware.c
+++ b/src/hardware.c
@@ -161,6 +161,7 @@ void hw_reset_handler() {
   if (hw.hard_reset) {
     while (huanyang_stopping() || !usart_tx_empty() || !eeprom_is_ready())
       continue;
+
     hw_hard_reset();
   }
 

diff --git a/src/homing.c b/src/homing.c
index 48eb2dc6337416b7d28e032ed7a266f4e8f824fc..218d1fc48fd618d0378ded2c8e04037c48c8b24b 100644 (file)
--- a/src/homing.c
+++ b/src/homing.c
@@ -229,11 +229,11 @@ static void _homing_abort(int8_t axis) {
 /// set zero and finish up
 static void _homing_axis_set_zero(int8_t axis) {
   if (hm.set_coordinates) {
-    mach_set_position(axis, 0);
+    mach_set_axis_position(axis, 0);
     hm.homed[axis] = true;
 
   } else // do not set axis if in G28.4 cycle
-    mach_set_position(axis, mp_runtime_get_work_position(axis));
+    mach_set_axis_position(axis, mp_runtime_get_work_position(axis));
 
   mach_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value
 

diff --git a/src/i2c.h b/src/i2c.h
index 9287249efeab2af45eeb5d54ab1cfe24d9eae547..7ded350e8a3e6faaec081101bfc35ce7bcb072e7 100644 (file)
--- a/src/i2c.h
+++ b/src/i2c.h
@@ -45,6 +45,7 @@ typedef enum {
   I2C_REPORT,
   I2C_HOME,
   I2C_REBOOT,
+  I2C_ZERO,
 } i2c_cmd_t;
 
 

diff --git a/src/machine.c b/src/machine.c
index b13fe3a16b8e4e3e3783f7768d1cb27b65a1ef63..939bc9d203f7e7f26cc6e40a9c4df3ac8ff8e309 100644 (file)
--- a/src/machine.c
+++ b/src/machine.c
@@ -688,16 +688,38 @@ void mach_set_coord_system(coord_system_t coord_system) {
  * (such as homing cycles) when you know there are no more moves in
  * the planner and that all motion has stopped.
  */
-void mach_set_position(int axis, float position) {
-  //if (!mp_is_quiescent()) CM_ALARM(STAT_INTERNAL_ERROR);
+void mach_set_axis_position(unsigned axis, float position) {
+  //if (!mp_is_quiescent()) CM_ALARM(STAT_MACH_NOT_QUIESCENT);
+  if (AXES <= axis) return;
+
   mach.position[axis] = position;
   mach.ms.target[axis] = position;
-  mp_set_planner_position(axis, position);
+  mp_set_axis_position(axis, position);
   mp_runtime_set_axis_position(axis, position);
   mp_runtime_set_steps_from_position();
 }
 
 
+stat_t mach_zero_all() {
+  for (unsigned axis = 0; axis < AXES; axis++) {
+    stat_t status = mach_zero_axis(axis);
+    if (status != STAT_OK) return status;
+  }
+
+  return STAT_OK;
+}
+
+
+stat_t mach_zero_axis(unsigned axis) {
+  if (!mp_is_quiescent()) return STAT_MACH_NOT_QUIESCENT;
+  if (AXES <= axis) return STAT_INVALID_AXIS;
+
+  mach_set_axis_position(axis, 0);
+
+  return STAT_OK;
+}
+
+
 // G28.3 functions and support
 static void _exec_absolute_origin(float *value, float *flag) {
   for (int axis = 0; axis < AXES; axis++)
@@ -730,7 +752,7 @@ void mach_set_absolute_origin(float origin[], float flag[]) {
       value[axis] = TO_MILLIMETERS(origin[axis]);
       mach.position[axis] = value[axis];           // set model position
       mach.ms.target[axis] = value[axis];          // reset model target
-      mp_set_planner_position(axis, value[axis]);  // set mm position
+      mp_set_axis_position(axis, value[axis]);  // set mm position
     }
 
   mp_queue_command(_exec_absolute_origin, value, flag);

diff --git a/src/machine.h b/src/machine.h
index 50378c734d7c9e006edf40e90593274f1994a269..4814b8df7d121232228f890fe6eec3990221383c 100644 (file)
--- a/src/machine.h
+++ b/src/machine.h
@@ -377,9 +377,12 @@ void mach_set_distance_mode(distance_mode_t mode);
 void mach_set_coord_offsets(coord_system_t coord_system, float offset[],
                             float flag[]);
 
-void mach_set_position(int axis, float position);
+void mach_set_axis_position(unsigned axis, float position);
 void mach_set_absolute_origin(float origin[], float flag[]);
 
+stat_t mach_zero_all();
+stat_t mach_zero_axis(unsigned axis);
+
 void mach_set_coord_system(coord_system_t coord_system);
 void mach_set_origin_offsets(float offset[], float flag[]);
 void mach_reset_origin_offsets();

diff --git a/src/messages.def b/src/messages.def
index 686de5b5436856375e8a23ee7f586b1403cb0ab6..d48da3bc58aa47174b8b80e47b735f5aac99a65f 100644 (file)
--- a/src/messages.def
+++ b/src/messages.def
@@ -37,6 +37,7 @@ STAT_MSG(BUFFER_FULL_FATAL, "Buffer full - fatal")
 STAT_MSG(EEPROM_DATA_INVALID, "EEPROM data invalid")
 STAT_MSG(MOTOR_ERROR, "Motor error")
 STAT_MSG(STEP_CHECK_FAILED, "Step check failed")
+STAT_MSG(MACH_NOT_QUIESCENT, "Cannot perform action while machine is active")
 STAT_MSG(INTERNAL_ERROR, "Internal error")
 
 STAT_MSG(PREP_LINE_MOVE_TIME_IS_INFINITE, "Move time is infinite")
@@ -72,6 +73,7 @@ STAT_MSG(MINIMUM_TIME_MOVE, "Move less than minimum time")
 STAT_MSG(MACHINE_ALARMED, "Machine is alarmed - Command not processed")
 STAT_MSG(LIMIT_SWITCH_HIT, "Limit switch hit - Shutdown occurred")
 STAT_MSG(SOFT_LIMIT_EXCEEDED, "Soft limit exceeded")
+STAT_MSG(INVALID_AXIS, "Invalid axis")
 
 // Homing
 STAT_MSG(HOMING_CYCLE_FAILED, "Homing cycle failed")

diff --git a/src/plan/exec.c b/src/plan/exec.c
index 7f330538102b88f36a7edfdba96dec2517c87e82..1f6d1123ca79ddc393cf5d7b7d26cf46e20caf49 100644 (file)
--- a/src/plan/exec.c
+++ b/src/plan/exec.c
@@ -518,8 +518,8 @@ stat_t mp_exec_aline(mp_buffer_t *bf) {
 
 /// Dequeues buffer and executes move callback
 stat_t mp_exec_move() {
-  if (mp_get_state() == STATE_ESTOPPED) return STAT_MACHINE_ALARMED;
-  if (mp_get_state() == STATE_HOLDING) return STAT_NOOP;
+  if (mp_get_state() == STATE_ESTOPPED ||
+      mp_get_state() == STATE_HOLDING) return STAT_NOOP;
 
   mp_buffer_t *bf = mp_get_run_buffer();
   if (!bf) return STAT_NOOP; // Nothing running

diff --git a/src/plan/jog.c b/src/plan/jog.c
index d652bbffdfe2efd2aff91bd2b5c8445896d152d5..2d25c94a5e69b48073fc64513c2c82fc5f2f6fd7 100644 (file)
--- a/src/plan/jog.c
+++ b/src/plan/jog.c
@@ -89,7 +89,7 @@ static stat_t _exec_jog(mp_buffer_t *bf) {
   if (done) {
     // Update machine position
     for (int axis = 0; axis < AXES; axis++)
-      mach_set_position(axis, mp_runtime_get_work_position(axis));
+      mach_set_axis_position(axis, mp_runtime_get_work_position(axis));
 
     return STAT_NOOP; // Done
   }

diff --git a/src/plan/line.c b/src/plan/line.c
index 0718e6d541fdc316b2b2d96a2395a66e6309a1ea..4376a04967ae9d01fa529ec6f23ddc153709c906 100644 (file)
--- a/src/plan/line.c
+++ b/src/plan/line.c
@@ -56,7 +56,7 @@ move_master_t mm = {{0}}; // context for line planning
 
 
 /// Set planner position for a single axis
-void mp_set_planner_position(int axis, const float position) {
+void mp_set_axis_position(int axis, float position) {
   mm.position[axis] = position;
 }
 

diff --git a/src/plan/line.h b/src/plan/line.h
index b49d44668dfa5ab562f7f41ca61c40cd5f405c6a..75ce7d535abce4bc5cfe12a16cd4ffeaa2193eb6 100644 (file)
--- a/src/plan/line.h
+++ b/src/plan/line.h
@@ -29,5 +29,5 @@
 
 #include "machine.h"
 
-void mp_set_planner_position(int axis, const float position);
+void mp_set_axis_position(int axis, float position);
 stat_t mp_aline(move_state_t *ms);

diff --git a/src/plan/planner.c b/src/plan/planner.c
index 1fc625a8f8f60c6de434ef0e87c3a9c9ed222d1f..49eb6da17e9e5dd1f98d152610f1daf8d6461fe1 100644 (file)
--- a/src/plan/planner.c
+++ b/src/plan/planner.c
@@ -596,8 +596,7 @@ void mp_replan_blocks() {
  * [2] Cannot assume Vf >= Vi due to rounding errors and use of
  *     PLANNER_VELOCITY_TOLERANCE necessitating the introduction of fabs()
  */
-float mp_get_target_length(const float Vi, const float Vf,
-                           const mp_buffer_t *bf) {
+float mp_get_target_length(float Vi, float Vf, const mp_buffer_t *bf) {
   return fabs(Vi - Vf) * sqrt(fabs(Vi - Vf) * bf->recip_jerk);
 }
 
@@ -668,8 +667,7 @@ float mp_get_target_length(const float Vi, const float Vf,
  *
  *   J'(x) = (2 * Vi * x - Vi^2 + 3 * x^2) / L^2
  */
-float mp_get_target_velocity(const float Vi, const float L,
-                             const mp_buffer_t *bf) {
+float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf) {
   // 0 iterations (a reasonable estimate)
   float x = pow(L, 0.66666666) * bf->cbrt_jerk + Vi; // First estimate
 

diff --git a/src/plan/planner.h b/src/plan/planner.h
index e5b620e547444ef7c20e3e1dcc90e7e9d1035734..db1417f62f4f9e71f8bf32ddc82478f93293e6e2 100644 (file)
--- a/src/plan/planner.h
+++ b/src/plan/planner.h
@@ -78,7 +78,5 @@ void mp_flush_planner();
 void mp_kinematics(const float travel[], float steps[]);
 void mp_plan_block_list(mp_buffer_t *bf);
 void mp_replan_blocks();
-float mp_get_target_length(const float Vi, const float Vf,
-                           const mp_buffer_t *bf);
-float mp_get_target_velocity(const float Vi, const float L,
-                             const mp_buffer_t *bf);
+float mp_get_target_length(float Vi, float Vf, const mp_buffer_t *bf);
+float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf);

diff --git a/src/plan/state.c b/src/plan/state.c
index 310cd2c94ecd2e389667c14f2f79325241af2c3c..09c99fe6733dda0d5873669504c225d973d4251a 100644 (file)
--- a/src/plan/state.c
+++ b/src/plan/state.c
@@ -51,7 +51,9 @@ typedef struct {
 } planner_state_t;
 
 
-static planner_state_t ps = {0};
+static planner_state_t ps = {
+  .flush_requested = true // Start out flushing
+};
 
 
 mp_state_t mp_get_state() {return ps.state;}
@@ -186,7 +188,7 @@ void mp_state_callback() {
       // Reset to actual machine position.  Otherwise machine is set to the
       // position of the last queued move.
       for (int axis = 0; axis < AXES; axis++)
-        mach_set_position(axis, mp_runtime_get_axis_position(axis));
+        mach_set_axis_position(axis, mp_runtime_get_axis_position(axis));
     }
 
     // Resume

diff --git a/src/probing.c b/src/probing.c
index c9e2bee7db01bd7b6651c0b98c1be7247a16d1a8..989ab430562f1b2ba8f7457b044adfd86e3c44ad 100644 (file)
--- a/src/probing.c
+++ b/src/probing.c
@@ -114,7 +114,7 @@ static void _probing_finish() {
 
   for (int axis = 0; axis < AXES; axis++) {
     // if we got here because of a feed hold keep the model position correct
-    mach_set_position(axis, mp_runtime_get_work_position(axis));
+    mach_set_axis_position(axis, mp_runtime_get_work_position(axis));
 
     // store the probe results
     pb.results[axis] = mach_get_absolute_position(axis);