From: Joseph Coffland <joseph@cauldrondevelopment.com>
Date: Sat, 6 Jan 2018 01:33:48 +0000 (-0800)
Subject: Working pause/unpause/stop
X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=033798bba24f7c6d57def2cb5a3f77d6af83a7e0;p=bbctrl-firmware

Working pause/unpause/stop
---

diff --git a/src/avr/src/command.def b/src/avr/src/command.def
index b4ecdb9..6a674b5 100644
--- a/src/avr/src/command.def
+++ b/src/avr/src/command.def
@@ -34,6 +34,7 @@ CMD('d', dwell,     1, "[seconds]")
 CMD('o', out,       1, "Output")
 CMD('p', opt_pause, 1, "Set an optional pause")
 CMD('P', pause,     0, "[optional]")
+CMD('U', unpause,   0, "Unpause")
 CMD('j', jog,       0, "[axes]")
 CMD('h', help,      0, "Print this help screen")
 CMD('r', report,    0, "<0|1>[var] Enable or disable var reporting")
diff --git a/src/avr/src/command.h b/src/avr/src/command.h
index 613eb1a..4a93d1f 100644
--- a/src/avr/src/command.h
+++ b/src/avr/src/command.h
@@ -27,6 +27,7 @@
 
 #pragma once
 
+#include "config.h"
 #include "status.h"
 
 #include <stdbool.h>
@@ -41,11 +42,13 @@ typedef enum {
 
 
 void command_init();
-bool command_is_busy();
 bool command_is_active();
 unsigned command_get_count();
 void command_print_help();
 void command_flush_queue();
 void command_push(char code, void *data);
 bool command_callback();
+void command_set_position(const float position[AXES]);
+void command_get_position(float position[AXES]);
+void command_reset_position();
 bool command_exec();
diff --git a/src/avr/src/commands.c b/src/avr/src/commands.c
index cceec0e..c9b11e2 100644
--- a/src/avr/src/commands.c
+++ b/src/avr/src/commands.c
@@ -85,24 +85,6 @@ stat_t command_reboot(char *cmd) {
 }
 
 
-stat_t command_resume(char *cmd) {
-  state_request_resume();
-  return STAT_OK;
-}
-
-
-stat_t command_step(char *cmd) {
-  state_request_step();
-  return STAT_OK;
-}
-
-
-stat_t command_flush(char *cmd) {
-  state_request_flush();
-  return STAT_OK;
-}
-
-
 stat_t command_dump(char *cmd) {
   report_request_full();
   return STAT_OK;
diff --git a/src/avr/src/hardware.c b/src/avr/src/hardware.c
index 36b77d3..b4d937c 100644
--- a/src/avr/src/hardware.c
+++ b/src/avr/src/hardware.c
@@ -43,7 +43,6 @@
 typedef struct {
   char id[26];
   bool hard_reset;         // flag to perform a hard reset
-  bool bootloader;         // flag to enter the bootloader
 } hw_t;
 
 static hw_t hw = {{0}};
@@ -146,18 +145,9 @@ void hw_reset_handler() {
 
     hw_hard_reset();
   }
-
-  if (hw.bootloader) {
-    // TODO enable bootloader interrupt vectors and jump to BOOT_SECTION_START
-    hw.bootloader = false;
-  }
 }
 
 
-/// Executes a software reset using CCPWrite
-void hw_request_bootloader() {hw.bootloader = true;}
-
-
 uint8_t hw_disable_watchdog() {
   uint8_t state = WDT.CTRL;
   wdt_disable();
diff --git a/src/avr/src/hardware.h b/src/avr/src/hardware.h
index 4f1acf6..8dbd928 100644
--- a/src/avr/src/hardware.h
+++ b/src/avr/src/hardware.h
@@ -37,7 +37,5 @@ void hw_request_hard_reset();
 void hw_hard_reset();
 void hw_reset_handler();
 
-void hw_request_bootloader();
-
 uint8_t hw_disable_watchdog();
 void hw_restore_watchdog(uint8_t state);
diff --git a/src/avr/src/jog.c b/src/avr/src/jog.c
index bcb8faf..eaeed05 100644
--- a/src/avr/src/jog.c
+++ b/src/avr/src/jog.c
@@ -220,7 +220,9 @@ static float _compute_axis_velocity(int axis) {
   // Compute next accel
   a->accel = scurve_next_accel(SEGMENT_TIME, V, Vt, a->accel, jerk);
 
-  // TODO limit acceleration
+  // Limit acceleration
+  if (axis_get_accel_max(axis) < fabs(a->accel))
+    a->accel = (a->accel < 0 ? -1 : 1) * axis_get_accel_max(axis);
 
   return V + a->accel * SEGMENT_TIME;
 }
diff --git a/src/avr/src/line.c b/src/avr/src/line.c
index 37ab3a1..69ff58b 100644
--- a/src/avr/src/line.c
+++ b/src/avr/src/line.c
@@ -131,16 +131,6 @@ static bool _segment_next() {
 }
 
 
-static stat_t _hold() {
-  if (state_get() != STATE_HOLDING) {
-    exec_set_cb(0);
-    return STAT_AGAIN;
-  }
-
-  return STAT_NOP;
-}
-
-
 static stat_t _pause() {
   float t = SEGMENT_TIME - l.current_t;
   float v = exec_get_velocity();
@@ -152,13 +142,17 @@ static stat_t _pause() {
     exec_set_acceleration(0);
     exec_set_jerk(0);
     state_holding();
-    exec_set_cb(_hold);
-    return _hold();
+    exec_set_cb(0);
+
+    return STAT_NOP;
   }
 
   // Compute new velocity and acceleration
   a = scurve_next_accel(t, v, 0, a, j);
-  if (l.line.max_accel < a) a = l.line.max_accel;
+  if (l.line.max_accel < fabs(a)) {
+    a = (a < 0 ? -1 : 1) * l.line.max_accel;
+    j = 0;
+  } else if (0 < a) j = -j;
   v += a * t;
 
   // Compute distance that will be traveled
@@ -259,13 +253,12 @@ void _print_vector(const char *name, float v[AXES]) {
 
 
 stat_t command_line(char *cmd) {
-  static float next_start[AXES];
   line_t line = {};
 
   cmd++; // Skip command code
 
   // Get start position
-  copy_vector(line.start, next_start);
+  command_get_position(line.start);
 
   // Get target velocity
   if (!decode_float(&cmd, &line.target_vel)) return STAT_BAD_FLOAT;
@@ -309,7 +302,7 @@ stat_t command_line(char *cmd) {
   if (*cmd) return STAT_INVALID_ARGUMENTS;
 
   // Set next start position
-  copy_vector(next_start, line.target);
+  command_set_position(line.target);
 
   // Compute direction vector
   for (int i = 0; i < AXES; i++)
diff --git a/src/avr/src/motor.c b/src/avr/src/motor.c
index 4a8142b..543230f 100644
--- a/src/avr/src/motor.c
+++ b/src/avr/src/motor.c
@@ -200,7 +200,7 @@ static void _update_power(int motor) {
     // Fall through
 
   case MOTOR_ALWAYS_POWERED:
-    // TODO is ~5ms enough time to enable the motor?
+    // NOTE, we have ~5ms to enable the motor
     drv8711_set_state(motor, DRV8711_ACTIVE);
     break;
 
diff --git a/src/avr/src/state.c b/src/avr/src/state.c
index 4130349..9f6a222 100644
--- a/src/avr/src/state.c
+++ b/src/avr/src/state.c
@@ -33,7 +33,7 @@
 #include "spindle.h"
 #include "report.h"
 
-#include <stdbool.h>
+#include <stdio.h>
 
 
 static struct {
@@ -66,11 +66,12 @@ PGM_P state_get_pgmstr(state_t state) {
 
 PGM_P state_get_hold_reason_pgmstr(hold_reason_t reason) {
   switch (reason) {
-  case HOLD_REASON_USER_PAUSE:    return PSTR("USER");
-  case HOLD_REASON_PROGRAM_PAUSE: return PSTR("PROGRAM");
-  case HOLD_REASON_PROGRAM_END:   return PSTR("END");
-  case HOLD_REASON_PALLET_CHANGE: return PSTR("PALLET");
-  case HOLD_REASON_TOOL_CHANGE:   return PSTR("TOOL");
+  case HOLD_REASON_USER_PAUSE:    return PSTR("User paused");
+  case HOLD_REASON_PROGRAM_PAUSE: return PSTR("Program paused");
+  case HOLD_REASON_PROGRAM_END:   return PSTR("Program end");
+  case HOLD_REASON_PALLET_CHANGE: return PSTR("Pallet change");
+  case HOLD_REASON_TOOL_CHANGE:   return PSTR("Tool change");
+  case HOLD_REASON_STEPPING:      return PSTR("Stepping");
   }
 
   return PSTR("INVALID");
@@ -102,17 +103,11 @@ bool state_is_resuming() {return s.resume_requested;}
 
 bool state_is_quiescent() {
   return (state_get() == STATE_READY || state_get() == STATE_HOLDING) &&
-    !st_is_busy() && !command_is_busy();
+    !st_is_busy();
 }
 
 
-static void _set_plan_steps(bool plan_steps) {} // TODO
-
-
-void state_holding() {
-  _set_state(STATE_HOLDING);
-  _set_plan_steps(false);
-}
+void state_holding() {_set_state(STATE_HOLDING);}
 
 
 void state_optional_pause() {
@@ -130,17 +125,6 @@ void state_running() {
 
 void state_idle() {if (state_get() == STATE_RUNNING) _set_state(STATE_READY);}
 void state_estop() {_set_state(STATE_ESTOPPED);}
-void state_request_pause() {s.pause_requested = true;}
-void state_request_start() {s.start_requested = true;}
-void state_request_flush() {s.flush_requested = true;}
-void state_request_resume() {if (s.flush_requested) s.resume_requested = true;}
-void state_request_optional_pause() {s.optional_pause_requested = true;}
-
-
-void state_request_step() {
-  _set_plan_steps(true);
-  s.start_requested = true;
-}
 
 
 /*** Pauses, queue flushes and starts are all related.  Request functions
@@ -172,11 +156,12 @@ void state_callback() {
     if (state_get() == STATE_RUNNING) _set_state(STATE_STOPPING);
   }
 
-  // Only flush queue when idle or holding.
+  // Only flush queue when idle or holding
   if (s.flush_requested && state_is_quiescent()) {
     command_flush_queue();
 
     // Stop spindle
+    // TODO Spindle should not be stopped when pausing
     spindle_stop();
 
     // Resume
@@ -208,7 +193,31 @@ PGM_P get_hold_reason() {return state_get_hold_reason_pgmstr(s.hold_reason);}
 
 // Command callbacks
 stat_t command_pause(char *cmd) {
-  if (cmd[1] == '1') state_request_optional_pause();
-  else state_request_pause();
+  if (cmd[1] == '1') s.optional_pause_requested = true;
+  else s.pause_requested = true;
+  return STAT_OK;
+}
+
+
+stat_t command_unpause(char *cmd) {
+  s.start_requested = true;
+  return STAT_OK;
+}
+
+
+stat_t command_resume(char *cmd) {
+  if (s.flush_requested) s.resume_requested = true;
+  return STAT_OK;
+}
+
+
+stat_t command_step(char *cmd) {
+  // TODO
+  return STAT_OK;
+}
+
+
+stat_t command_flush(char *cmd) {
+  s.flush_requested = true;
   return STAT_OK;
 }
diff --git a/src/avr/src/state.h b/src/avr/src/state.h
index e6c658b..7e496e2 100644
--- a/src/avr/src/state.h
+++ b/src/avr/src/state.h
@@ -47,6 +47,7 @@ typedef enum {
   HOLD_REASON_PROGRAM_END,
   HOLD_REASON_PALLET_CHANGE,
   HOLD_REASON_TOOL_CHANGE,
+  HOLD_REASON_STEPPING,
 } hold_reason_t;
 
 
@@ -66,11 +67,4 @@ void state_running();
 void state_idle();
 void state_estop();
 
-void state_request_pause();
-void state_request_start();
-void state_request_flush();
-void state_request_resume();
-void state_request_optional_pause();
-void state_request_step();
-
 void state_callback();
diff --git a/src/py/bbctrl/AVR.py b/src/py/bbctrl/AVR.py
index 2a9e3c7..318dffc 100644
--- a/src/py/bbctrl/AVR.py
+++ b/src/py/bbctrl/AVR.py
@@ -116,7 +116,7 @@ class AVR():
                 if isinstance(value, str): value = '"' + value + '"'
                 if isinstance(value, bool): value = int(value)
 
-                self.queue_command('${}={}'.format(var, value))
+                self.set('', var, value)
 
         self.queue_command('$$') # Refresh all vars, must come after above
 
@@ -142,7 +142,7 @@ class AVR():
             if self.ctrl.ioloop.READ & events: self.serial_read()
             if self.ctrl.ioloop.WRITE & events: self.serial_write()
         except Exception as e:
-            log.error('Serial handler error:', e)
+            log.error('Serial handler error: %s', e)
 
 
     def serial_write(self):
@@ -166,10 +166,7 @@ class AVR():
         elif self.stream is not None:
             cmd = self.stream.next()
 
-            if cmd is None:
-                self.set_write(False)
-                self.stream = None
-
+            if cmd is None: self.set_write(False)
             else: self.load_next_command(cmd)
 
         # Else stop writing
@@ -206,6 +203,7 @@ class AVR():
                 update.update(msg)
                 log.debug(line)
 
+                # Don't overwrite duplicate `msg`
                 if 'msg' in msg: break
 
         if update:
@@ -218,6 +216,11 @@ class AVR():
 
             self.vars.update(update)
 
+            if self.stream is not None:
+                self.stream.update(update)
+                if not self.stream.is_running():
+                    self.stream = None
+
             try:
                 self._update_lcd(update)
 
@@ -295,6 +298,7 @@ class AVR():
 
     def home(self, axis, position = None):
         if self.stream is not None: raise Exception('Busy, cannot home')
+        raise Exception('NYI') # TODO
 
         if position is not None:
             self.queue_command('G28.3 %c%f' % (axis, position))
@@ -317,10 +321,7 @@ class AVR():
 
     def start(self, path):
         if self.stream is not None: raise Exception('Busy, cannot start file')
-
-        if path:
-            self._start_sending_gcode(path)
-            self._i2c_command(Cmd.RUN)
+        if path: self._start_sending_gcode(path)
 
 
     def step(self, path):
@@ -333,16 +334,28 @@ class AVR():
         self._i2c_command(Cmd.FLUSH)
         self._stop_sending_gcode()
         # Resume processing once current queue of GCode commands has flushed
-        self.queue_command('c')
+        self.queue_command(Cmd.RESUME)
 
 
     def pause(self): self._i2c_command(Cmd.PAUSE, byte = 0)
-    def unpause(self): self._i2c_command(Cmd.RUN)
+
+
+    def unpause(self):
+        if self.vars.get('x', '') != 'HOLDING' or self.stream is None: return
+
+        self._i2c_command(Cmd.FLUSH)
+        self.queue_command(Cmd.RESUME)
+        self.stream.restart()
+        self.set_write(True)
+        self._i2c_command(Cmd.UNPAUSE)
+
+
     def optional_pause(self): self._i2c_command(Cmd.PAUSE, byte = 1)
 
 
     def set_position(self, axis, position):
         if self.stream is not None: raise Exception('Busy, cannot set position')
         if self._is_axis_homed('%c' % axis):
+            raise Exception('NYI') # TODO
             self.queue_command('G92 %c%f' % (axis, position))
         else: self.queue_command('$%cp=%f' % (axis, position))
diff --git a/src/py/bbctrl/Cmd.py b/src/py/bbctrl/Cmd.py
index 32107ba..ca1ac3a 100644
--- a/src/py/bbctrl/Cmd.py
+++ b/src/py/bbctrl/Cmd.py
@@ -5,13 +5,14 @@ import logging
 log = logging.getLogger('Cmd')
 
 # TODO, sync this up with AVR code
-REPORT = 'r'
-PAUSE  = 'P'
-ESTOP  = 'E'
-CLEAR  = 'C'
-FLUSH  = 'F'
-STEP   = 'S'
-RUN    = 'p'
+REPORT  = 'r'
+PAUSE   = 'P'
+UNPAUSE = 'U'
+ESTOP   = 'E'
+CLEAR   = 'C'
+FLUSH   = 'F'
+STEP    = 'S'
+RESUME  = 'c'
 
 
 def encode_float(x):
diff --git a/src/py/bbctrl/Planner.py b/src/py/bbctrl/Planner.py
index f396ec9..6d71b7a 100644
--- a/src/py/bbctrl/Planner.py
+++ b/src/py/bbctrl/Planner.py
@@ -11,6 +11,8 @@ class Planner():
     def __init__(self, ctrl, path):
         self.ctrl = ctrl
         self.path = path
+        self.lastID = -1
+        self.done = False
 
         vars = ctrl.avr.vars
 
@@ -51,9 +53,32 @@ class Planner():
         self.reset()
 
 
+    def is_running(self): return self.planner.is_running()
+
+
+    def update(self, update):
+        if 'id' in update:
+            id = update['id']
+            if id: self.planner.release(id - 1)
+
+
+    def restart(self):
+        vars = self.ctrl.avr.vars
+        id = vars['id']
+
+        position = {}
+        for axis in 'xyzabc':
+            if (axis + 'p') in vars:
+                position[axis] = vars[axis + 'p']
+
+        self.planner.restart(id, position)
+        self.done = False
+
+
     def reset(self):
         self.planner = gplan.Planner(self.config)
         self.planner.load('upload' + self.path)
+        self.done = False
 
 
     def encode(self, block):
@@ -75,4 +100,13 @@ class Planner():
 
     def next(self):
         if self.planner.has_more():
-            return self.encode(self.planner.next())
+            cmd = self.planner.next()
+            self.lastID = cmd['id']
+            return self.encode(cmd)
+
+        if not self.done:
+            self.done = True
+
+            # Cause last cmd to flush when complete
+            if 0 <= self.lastID:
+                return '#id=%d' % (self.lastID + 1)