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.
{
"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+",
// 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
_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;
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);
}
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;
}
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
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;
}
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) {
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):
def update(self, frame, axes, text):
+ # Read new data
try:
data = self.sp.read(self.sp.in_waiting)
self.incoming += data.decode('utf-8')
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))
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()
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]);
- }
}
// 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
projects / bbctrl-firmware / commitdiff