From c653754f02a92a6b5823ce218e3cb603215e1a97 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Wed, 14 Mar 2018 15:00:40 -0700 Subject: [PATCH] Count only rising edges, Fixed reset switch --- CHANGELOG.md | 3 ++ package.json | 2 +- src/avr/src/config.h | 2 +- src/avr/src/motor.c | 48 +++++++++++----------- src/avr/step-test/plot_velocity.py | 66 +++++++++++++++++++----------- src/avr/step-test/step-test.c | 24 ++++++----- 6 files changed, 85 insertions(+), 60 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 239fbdb..ee79e0b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,6 +1,9 @@ Buildbotics CNC Controller Firmware Change Log ============================================== +## v0.3.20 + - Eliminated drift caused by miscounting half microsteps. + ## v0.3.19 - Fixed stopping problems. #127 - Fixed ``Negative s-curve time`` error. diff --git a/package.json b/package.json index ae4b3c2..2ed64f7 100644 --- a/package.json +++ b/package.json @@ -1,6 +1,6 @@ { "name": "bbctrl", - "version": "0.3.19", + "version": "0.3.20", "homepage": "http://buildbotics.com/", "repository": "https://github.com/buildbotics/bbctrl-firmware", "license": "GPL-3.0+", diff --git a/src/avr/src/config.h b/src/avr/src/config.h index 8bba016..0080d49 100644 --- a/src/avr/src/config.h +++ b/src/avr/src/config.h @@ -204,7 +204,7 @@ enum { // Motor settings. See motor.c #define MOTOR_IDLE_TIMEOUT 0.25 // secs, motor off after this time -#define MIN_HALF_STEP_CORRECTION 4 +#define MIN_STEP_CORRECTION 2 #define MIN_VELOCITY 10 // mm/min #define CURRENT_SENSE_RESISTOR 0.05 // ohms diff --git a/src/avr/src/motor.c b/src/avr/src/motor.c index a6b548e..90d1f9d 100644 --- a/src/avr/src/motor.c +++ b/src/avr/src/motor.c @@ -142,11 +142,13 @@ void motor_init() { _update_config(motor); // IO pins + PINCTRL_PIN(m->step_pin) = PORT_INVEN_bm; // Inverted output DIRSET_PIN(m->step_pin); // Output DIRSET_PIN(m->dir_pin); // Output // Setup motor timer - m->timer->CTRLB = TC_WGMODE_FRQ_gc | TC1_CCAEN_bm; + m->timer->CTRLB = TC_WGMODE_SINGLESLOPE_gc | TC1_CCAEN_bm; + m->timer->CCA = F_CPU * 0.000002 / 2; // Step pulse width, 2uS w/ clk/2 // Setup DMA channel as timer event counter m->dma->ADDRCTRL = DMA_CH_SRCDIR_FIXED_gc | DMA_CH_DESTDIR_FIXED_gc; @@ -180,8 +182,7 @@ int motor_get_axis(int motor) {return motors[motor].axis;} static int32_t _position_to_steps(int motor, float position) { - // We use half steps - return ((int32_t)round(position * motors[motor].steps_per_unit)) << 1; + return (int32_t)round(position * motors[motor].steps_per_unit); } @@ -244,13 +245,13 @@ void motor_end_move(int motor) { while (m->dma->CTRLB & DMA_CH_CHPEND_bm) continue; // Get actual step count from DMA channel - const int24_t half_steps = 0xffff - m->dma->TRFCNT; + const int24_t steps = 0xffff - m->dma->TRFCNT; // Disable DMA step counter m->dma->CTRLA &= ~DMA_CH_ENABLE_bm; // Accumulate encoder & compute error - m->encoder += m->last_negative ? -half_steps : half_steps; + m->encoder += m->last_negative ? -steps : steps; m->error = m->commanded - m->encoder; } @@ -263,8 +264,7 @@ void motor_load_move(int motor) { motor_end_move(motor); // Set direction, compensating for polarity - const bool counterclockwise = m->negative ^ m->reverse; - SET_PIN(m->dir_pin, counterclockwise); + SET_PIN(m->dir_pin, m->negative ^ m->reverse); if (m->timer_period) { // Setup DMA step counter @@ -272,15 +272,14 @@ void motor_load_move(int motor) { m->dma->CTRLA |= DMA_CH_ENABLE_bm; // Set clock and period - m->timer->CCABUF = m->timer_period; // Set next frequency + m->timer->PERBUF = m->timer_period; // Set next frequency if (m->timer_period < m->timer->CNT) m->timer->CNT = m->timer_period - 1; - m->timer->CTRLA = TC_CLKSEL_DIV1_gc; // Start clock + m->timer->CTRLA = TC_CLKSEL_DIV2_gc; // Start clock m->last_negative = m->negative; m->commanded = m->position; } else m->timer->CTRLA = 0; - // Clear move m->prepped = false; } @@ -294,33 +293,36 @@ void motor_prep_move(int motor, float time, float target) { motor_t *m = &motors[motor]; ASSERT(!m->prepped); - // Travel in half steps + // Travel in steps int32_t position = _position_to_steps(motor, target); - int24_t half_steps = position - m->position; + int24_t steps = position - m->position; m->position = position; // Error correction int16_t correction = abs(m->error); - if (MIN_HALF_STEP_CORRECTION <= correction) { + if (MIN_STEP_CORRECTION <= correction) { // Dampen correction oscillation - correction >>= 2; + correction >>= 1; // Make correction - half_steps += m->error < 0 ? -correction : correction; + steps += m->error < 0 ? -correction : correction; } // Positive steps from here on - m->negative = half_steps < 0; - if (m->negative) half_steps = -half_steps; + m->negative = steps < 0; + if (m->negative) steps = -steps; - // Note, clock toggles step line so we need two clocks per step. - float seg_clocks = time * F_CPU * 60; - float ticks_per_step = seg_clocks / half_steps; + // We use clock / 2 + float seg_clocks = time * (F_CPU * 60 / 2); + float ticks_per_step = round(seg_clocks / steps); - if (0xffff <= ticks_per_step) m->timer_period = 0; // Clock off, too slow - else m->timer_period = round(ticks_per_step); + // Disable clock if step rate is too fast or too slow + if (ticks_per_step < m->timer->CCA * 2 || // Too fast + 0xffff <= ticks_per_step) // Too slow + m->timer_period = 0; + else m->timer_period = ticks_per_step; - if (!half_steps) m->timer_period = 0; + if (!steps) m->timer_period = 0; // Power motor switch (m->power_mode) { diff --git a/src/avr/step-test/plot_velocity.py b/src/avr/step-test/plot_velocity.py index ebf7d15..2f278b0 100755 --- a/src/avr/step-test/plot_velocity.py +++ b/src/avr/step-test/plot_velocity.py @@ -12,32 +12,34 @@ import matplotlib.pyplot as plt import matplotlib.animation as animation -MM_PER_STEP = 6.35 * 1.8 / 360 / 32 +MM_PER_STEP = 5 * 1.8 / 360 / 32 +SAMPLES_PER_MIN = 6000 class Plot: def __init__(self, port, baud, max_len): + # Open serial port self.sp = serial.Serial(port, baud) + # Create data series self.series = [] for i in range(5): self.series.append(deque([0.0] * max_len)) + # Init vars self.max_len = max_len self.incoming = '' + self.last = [None] * 4 + # Open velocity log 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) + # Add new series data + def add(self, i, value): + self.series[i].pop() + self.series[i].appendleft(value) def update_text(self, text, vel, data): @@ -48,6 +50,7 @@ class Plot: def update(self, frame, axes, text): + # Read new data try: data = self.sp.read(self.sp.in_waiting) self.incoming += data.decode('utf-8') @@ -57,70 +60,80 @@ class Plot: return while True: + # Parse lines i = self.incoming.find('\n') if i == -1: break line = self.incoming[0:i] self.incoming = self.incoming[i + 1:] + # Handle reset if line.find('RESET') != -1: self.update_text(text, 0, [0] * 4) self.log.write(line + '\n') self.last = [None] * 4 continue + # Parse data try: data = [float(value) for value in line.split(',')] except ValueError: continue if len(data) != 4: continue - dist = 0 - v = [0] * 4 + # Compute axis velocities + v = [] # Axis velocity + totalV = 0 # Tool velocity 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 + v.append(delta * MM_PER_STEP * SAMPLES_PER_MIN) # mm/min + totalV += math.pow(v[i], 2) 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 + # Compute tool velocity + totalV = math.sqrt(totalV) - self.log.write(line + '\n') + # Update position and velocity text + self.update_text(text, totalV, data) + + # Don't update plots when not moving + if totalV == 0: continue - for i in range(4): self.add(self.series[i], v[i]) - self.add(self.series[4], vel) + # Add new data + for i in range(4): self.add(i, v[i]) + self.add(4, totalV) + # Update plots for i in range(5): axes[i].set_data(range(self.max_len), self.series[i]) + self.log.write(line + '\n') + def close(self): self.sp.flush() self.sp.close() -description = "Plot velocity data in real-time" - if __name__ == '__main__': + # Parse command line arguments + description = "Plot velocity data in real-time" 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() + # Create plot 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 = [] + # Setup position and velocity text fields font = dict(fontsize = 14, family = 'monospace') axes_text.append(plt.text(0, 11700, '', **font)) @@ -129,14 +142,19 @@ if __name__ == '__main__': axes_text.append(plt.text(800, 10500, '', **font)) axes_text.append(plt.text(1500, 11700, '', **font)) + # Create axes for i in range(5): axes.append(ax.plot([], [])[0]) + # Set text color to match axis color axes_text[i].set_color(axes[i].get_color()) + # Initial text views plot.update_text(axes_text, 0, [0] * 4) + # Set up animation anim = animation.FuncAnimation(fig, plot.update, fargs = [axes, axes_text], interval = 100) + # Run plt.show() plot.close() diff --git a/src/avr/step-test/step-test.c b/src/avr/step-test/step-test.c index f239ede..2a452e8 100644 --- a/src/avr/step-test/step-test.c +++ b/src/avr/step-test/step-test.c @@ -103,22 +103,24 @@ ISR(TCC1_OVF_vect) { if (reset) reset--; // Report measured steps - int32_t counts[4]; + static int32_t counts[4] = {0, 0, 0, 0}; + bool moving = false; bool zero = true; for (int i = 0; i < 4; i++) { - counts[i] = channel_read(i); - if (counts[i]) zero = false; + int32_t count = channel_read(i); + if (count != counts[i]) moving = true; + if (count) zero = false; + counts[i] = count; } - if (!zero) { - if (reset) { - for (int i = 0; i < 4; i++) channel_reset(i); - printf("RESET\n"); - return; - } + if (reset && !zero) { + for (int i = 0; i < 4; i++) channel_reset(i); + printf("RESET\n"); + return; + } + if (moving) printf("%ld,%ld,%ld,%ld\n", counts[0], counts[1], counts[2], counts[3]); - } } @@ -136,7 +138,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_BOTHEDGES_gc; + PINCTRL_PIN(step_pin) = PORT_SRLEN_bm | PORT_ISC_RISING_gc; PINCTRL_PIN(dir_pin) = PORT_SRLEN_bm | PORT_ISC_BOTHEDGES_gc; // Dir change interrupt -- 2.27.0