From bc73944518ac0c3709c4adaac36d8c6341dd819d Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Mon, 12 Mar 2018 03:54:12 -0700 Subject: [PATCH] Count half-microsteps, Added plot script, Added reset switch --- src/avr/step-test/plot_velocity.py | 141 +++++++++++++++++++++++++++++ src/avr/step-test/step-test.c | 50 ++++++++-- 2 files changed, 184 insertions(+), 7 deletions(-) create mode 100755 src/avr/step-test/plot_velocity.py diff --git a/src/avr/step-test/plot_velocity.py b/src/avr/step-test/plot_velocity.py new file mode 100755 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 c16dfe1..b529bf0 100644 --- a/src/avr/step-test/step-test.c +++ b/src/avr/step-test/step-test.c @@ -36,6 +36,9 @@ #include +#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(); } -- 2.27.0