Count half-microsteps, Added plot script, Added reset switch

diff --git a/src/avr/step-test/plot_velocity.py b/src/avr/step-test/plot_velocity.py
new file mode 100755 (executable)
index 0000000..979098f
--- /dev/null
+++ b/src/avr/step-test/plot_velocity.py
@@ -0,0 +1,141 @@
+#!/usr/bin/env python3
+
+import sys, serial, argparse
+import numpy as np
+import math
+import json
+from time import sleep
+from collections import deque
+from datetime import datetime
+
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+
+
+MM_PER_STEP = 6.35 * 1.8 / 360 / 32
+
+
+class Plot:
+  def __init__(self, port, baud, max_len):
+    self.sp = serial.Serial(port, baud)
+
+    self.series = []
+    for i in range(5):
+      self.series.append(deque([0.0] * max_len))
+
+    self.max_len = max_len
+    self.incoming = ''
+
+    ts = datetime.now().strftime('%Y-%m-%d-%H:%M:%S')
+    self.log = open('velocity-%s.log' % ts, 'w')
+
+    self.last = [None] * 4
+
+
+  def add(self, buf, value):
+    if len(buf) < self.max_len:
+      buf.append(value)
+    else:
+      buf.pop()
+      buf.appendleft(value)
+
+
+  def update_text(self, text, vel, data):
+      text[4].set_text('V {0:8,.2f}'.format(vel))
+
+      for i in range(4):
+        text[i].set_text('{} {:11,}'.format('XYZA'[i], int(data[i])))
+
+
+  def update(self, frame, axes, text):
+    try:
+      data = self.sp.read(self.sp.in_waiting)
+      self.incoming += data.decode('utf-8')
+
+    except Exception as e:
+      print(e)
+      return
+
+    while True:
+      i = self.incoming.find('\n')
+      if i == -1: break
+      line = self.incoming[0:i]
+      self.incoming = self.incoming[i + 1:]
+
+      if line.find('RESET') != -1:
+        self.update_text(text, 0, [0] * 4)
+        self.log.write(line + '\n')
+        self.last = [None] * 4
+        continue
+
+      try:
+        data = [float(value) for value in line.split(',')]
+      except ValueError: continue
+
+      if len(data) != 4: continue
+
+      dist = 0
+      v = [0] * 4
+
+      for i in range(4):
+        if self.last[i] is not None:
+          delta = self.last[i] - data[i]
+          dist += math.pow(delta, 2)
+          v[i] = delta * MM_PER_STEP / 2.0 / 0.01 * 60 # Velocity in mm/min
+
+        self.last[i] = data[i]
+
+      # Compute total velocity
+      vel = math.sqrt(dist) * MM_PER_STEP / 2.0 / 0.01 * 60
+      self.update_text(text, vel, data)
+      if vel == 0: continue
+
+      self.log.write(line + '\n')
+
+      for i in range(4): self.add(self.series[i], v[i])
+      self.add(self.series[4], vel)
+
+      for i in range(5):
+        axes[i].set_data(range(self.max_len), self.series[i])
+
+
+  def close(self):
+    self.sp.flush()
+    self.sp.close()
+
+
+description = "Plot velocity data in real-time"
+
+if __name__ == '__main__':
+  parser = argparse.ArgumentParser(description = description)
+  parser.add_argument('-p', '--port', default = '/dev/ttyUSB0')
+  parser.add_argument('-b', '--baud', default = 115200, type = int)
+  parser.add_argument('-m', '--max-width', default = 2000, type = int)
+  args = parser.parse_args()
+
+  plot = Plot(args.port, args.baud, args.max_width)
+
+  # Set up animation
+  fig = plt.figure()
+  ax = plt.axes(xlim = (0, args.max_width), ylim = (-10000, 10000))
+  axes = []
+  axes_text = []
+
+  for i in range(5):
+    axes.append(ax.plot([], [])[0])
+
+  font = dict(fontsize = 14, family = 'monospace')
+
+  axes_text.append(plt.text(0,    11700, '', **font))
+  axes_text.append(plt.text(800,  11700, '', **font))
+  axes_text.append(plt.text(0,    10500, '', **font))
+  axes_text.append(plt.text(800,  10500, '', **font))
+  axes_text.append(plt.text(1500, 11700, '', **font))
+
+  plot.update_text(axes_text, 0, [0] * 4)
+
+  anim = animation.FuncAnimation(fig, plot.update, fargs = [axes, axes_text],
+                                 interval = 100)
+
+  plt.show()
+  plot.close()

diff --git a/src/avr/step-test/step-test.c b/src/avr/step-test/step-test.c
index c16dfe103f024b63f27ac279d6a5e176637c3587..b529bf0019380960174bb3c240487eabd102c172 100644 (file)
--- a/src/avr/step-test/step-test.c
+++ b/src/avr/step-test/step-test.c
@@ -36,6 +36,9 @@
 #include <stdio.h>
 
 
+#define RESET_PIN SPI_MOSI_PIN
+
+
 void rtc_init() {}
 
 
@@ -53,6 +56,12 @@ static struct {
 };
 
 
+static int reset = 0;
+
+
+void channel_reset(int i) {channel[i].timer->CNT = channel[i].high = 0;}
+
+
 #define EVSYS_CHMUX(CH) (&EVSYS_CH0MUX)[CH]
 #define EVSYS_CHCTRL(CH) (&EVSYS_CH0CTRL)[CH]
 
@@ -75,9 +84,7 @@ void channel_update_dir(int i) {
 }
 
 
-ISR(PORTE_INT0_vect) {
-  for (int i = 0; i < 4; i++) channel_update_dir(i);
-}
+ISR(PORTE_INT0_vect) {for (int i = 0; i < 4; i++) channel_update_dir(i);}
 
 
 int32_t channel_read(int i) {
@@ -89,6 +96,9 @@ int32_t channel_read(int i) {
 }
 
 
+ISR(PORTC_INT0_vect) {reset = 32;}
+
+
 void channel_init(int i) {
   uint8_t step_pin = channel[i].step_pin;
   uint8_t dir_pin = channel[i].dir_pin;
@@ -96,7 +106,7 @@ void channel_init(int i) {
   // Configure I/O
   DIRCLR_PIN(step_pin);
   DIRCLR_PIN(dir_pin);
-  PINCTRL_PIN(step_pin) = PORT_SRLEN_bm | PORT_ISC_RISING_gc;
+  PINCTRL_PIN(step_pin) = PORT_SRLEN_bm | PORT_ISC_BOTHEDGES_gc;
   PINCTRL_PIN(dir_pin)  = PORT_SRLEN_bm | PORT_ISC_BOTHEDGES_gc;
 
   // Dir change interrupt
@@ -117,8 +127,25 @@ void channel_init(int i) {
 
 
 ISR(TCC1_OVF_vect) {
-  printf("%ld,%ld,%ld,%ld\n",
-         channel_read(0), channel_read(1), channel_read(2), channel_read(3));
+  if (reset) reset--;
+
+  // Report measured steps
+  int32_t counts[4];
+  bool zero = true;
+  for (int i = 0; i < 4; i++) {
+    counts[i] = channel_read(i);
+    if (counts[i]) zero = false;
+  }
+
+  if (!zero) {
+    if (reset) {
+      for (int i = 0; i < 4; i++) channel_reset(i);
+      printf("RESET\n");
+      return;
+    }
+
+    printf("%ld,%ld,%ld,%ld\n", counts[0], counts[1], counts[2], counts[3]);
+  }
 }
 
 
@@ -136,11 +163,20 @@ static void init() {
   usart_init();
   for (int i = 0; i < 4; i++) channel_init(i);
 
-  // Configure clock
+  // Configure report clock
   TCC1.INTCTRLA = TC_OVFINTLVL_LO_gc;
   TCC1.PER = F_CPU / 256 * 0.01; // 10ms
   TCC1.CTRLA = TC_CLKSEL_DIV256_gc;
 
+  // Reset switch
+  DIRCLR_PIN(RESET_PIN);
+  PINCTRL_PIN(RESET_PIN) =
+    PORT_SRLEN_bm | PORT_ISC_RISING_gc | PORT_OPC_PULLUP_gc;
+  PIN_PORT(RESET_PIN)->INTCTRL  |= PORT_INT0LVL_LO_gc;
+  PIN_PORT(RESET_PIN)->INT0MASK |= PIN_BM(RESET_PIN);
+
+  printf("RESET\n");
+
   sei();
 }