// Serial settings
-#define SERIAL_BAUD USART_BAUD_230400
+#define SERIAL_BAUD USART_BAUD_115200
#define SERIAL_PORT USARTC0
#define SERIAL_DRE_vect USARTC0_DRE_vect
#define SERIAL_RXC_vect USARTC0_RXC_vect
typedef struct {
- float offset;
+ float time;
float speed;
} speed_t;
float jerk;
float lV; // Last velocity
+ float lineT;
speed_t speed;
} l;
}
-static void _set_sync_speeds(float d) {
- float speed = FLT_MAX;
+static void _load_sync_speeds(float startT, float endT) {
+ // Convert from mins to ms
+ startT *= 60000;
+ endT *= 60000;
while (true) {
// Load new sync speed if needed and available
- if (l.speed.offset < 0 && command_peek() == COMMAND_sync_speed)
+ if (l.speed.time < 0 && command_peek() == COMMAND_sync_speed)
l.speed = *(speed_t *)(command_next() + 1);
// Exit if we don't have a speed or it's not ready to be set
- if (l.speed.offset < 0 || d < l.speed.offset) break;
+ if (l.speed.time < 0 || endT < l.speed.time) break;
- // Set speed
- speed = l.speed.speed;
- l.speed.offset = -1; // Mark done
+ // Queue speed
+ spindle_queue_speed(round(l.speed.time - startT), l.speed.speed);
+ l.speed.time = -1; // Mark done
}
+}
+
- if (speed != FLT_MAX) spindle_set_speed(speed);
+static stat_t _exec_segment(float time, const float target[], float vel,
+ float accel) {
+ return exec_segment(time, target, vel, accel, l.line.max_vel,
+ l.line.max_accel, l.line.max_jerk);
}
t = section_time;
}
+ // Handle synchronous speeds
+ _load_sync_speeds(l.lineT + t - seg_time, l.lineT + t);
+
// Compute distance and velocity
float d = _segment_distance(t);
float v = _segment_velocity(t);
// Don't allow overshoot
if (l.line.length < d) d = l.line.length;
- // Handle syncronous speeds
- _set_sync_speeds(d);
-
// Check if section complete
if (t == section_time) {
if (_section_next()) {
l.seg = 0;
l.iD = d;
l.iV = v;
+ l.lineT += t;
} else {
+ spindle_new_segment();
exec_set_cb(0);
// Last segment of last section
// Use exact target values to correct for floating-point errors
- return exec_segment(seg_time, l.line.target, l.line.target_vel, a,
- l.line.max_vel, l.line.max_accel, l.line.max_jerk);
+ return _exec_segment(seg_time, l.line.target, l.line.target_vel, a);
}
}
_segment_target(target, d);
// Segment move
- return exec_segment
- (seg_time, target, v, a, l.line.max_vel, l.line.max_accel, l.line.max_jerk);
+ return _exec_segment(seg_time, target, v, a);
}
void command_line_exec(void *data) {
l.line = *(line_t *)data;
- l.speed.offset = -1;
- _set_sync_speeds(0);
+ l.lineT = 0;
+ l.speed.time = -1;
// Setup first section
l.seg = 0;
cmd++; // Skip command code
// Get target velocity
- if (!decode_float(&cmd, &s.offset)) return STAT_BAD_FLOAT;
+ if (!decode_float(&cmd, &s.time)) return STAT_BAD_FLOAT;
if (!decode_float(&cmd, &s.speed)) return STAT_BAD_FLOAT;
// Queue
void command_sync_speed_exec(void *data) {
speed_t s = *(speed_t *)data;
- spindle_set_speed(s.speed);
+ spindle_queue_speed(0, s.speed);
}
#include "analog.h"
#include "exec.h"
#include "state.h"
+#include "spindle.h"
#include <avr/wdt.h>
vars_init(); // configuration variables
estop_init(); // emergency stop handler
command_init(); // command queue
+ spindle_init(); // spindle
sei(); // enable interrupts
// Disable
if (!speed || estop_triggered()) {
+ spindle.duty = 0;
TIMER_PWM.CTRLB = 0; // Disable clock control of pin
OUTCLR_PIN(SPIN_PWM_PIN);
_set_enable(false);
// 100% duty
if (speed == 1 && spindle.max_duty == 1) {
+ spindle.duty = 1;
TIMER_PWM.CTRLB = 0; // Disable clock control of pin
OUTSET_PIN(SPIN_PWM_PIN);
return;
#define RING_BUF_INC(x) (((x) + 1) & RING_BUF_MASK)
+#define RING_BUF_DEC(x) ((x) ? x - 1 : (RING_BUF_SIZE - 1))
#ifdef RING_BUF_ATOMIC_COPY
}
+RING_BUF_FUNC RING_BUF_TYPE *CONCAT(RING_BUF_NAME, _front)() {
+ return &RING_BUF.buf[RING_BUF_READ_INDEX(head)];
+}
+
+
+RING_BUF_FUNC RING_BUF_TYPE *CONCAT(RING_BUF_NAME, _back)() {
+ return &RING_BUF.buf[RING_BUF_DEC(RING_BUF_READ_INDEX(tail))];
+}
+
+
RING_BUF_FUNC RING_BUF_TYPE CONCAT(RING_BUF_NAME, _get)(int offset) {
return RING_BUF.buf[(RING_BUF_READ_INDEX(head) + offset) & RING_BUF_MASK];
}
#include <math.h>
+#define SPEED_QUEUE_SIZE 32
+
+typedef struct {
+ int8_t time;
+ float speed;
+} speed_t;
+
+
+#define RING_BUF_NAME speed_q
+#define RING_BUF_TYPE speed_t
+#define RING_BUF_INDEX_TYPE volatile uint8_t
+#define RING_BUF_SIZE SPEED_QUEUE_SIZE
+#include "ringbuf.def"
+
+
static struct {
spindle_type_t type;
float override;
float min_rpm;
float max_rpm;
+ int8_t time;
spindle_type_t next_type;
-} spindle = {SPINDLE_TYPE_DISABLED, 1};
+
+} spindle = {
+ .type = SPINDLE_TYPE_DISABLED,
+ .override = 1
+};
+
+
+void spindle_init() {
+ speed_q_init();
+ spindle_new_segment();
+}
spindle_type_t spindle_get_type() {return spindle.type;}
-void spindle_set_speed(float speed) {
+void _set_speed(float speed) {
spindle.speed = speed;
speed *= spindle.override;
}
-float spindle_get_speed() {
+float _get_speed() {
float speed = 0;
switch (spindle.type) {
}
-void spindle_stop() {spindle_set_speed(0);}
+void spindle_stop() {_set_speed(0);}
bool spindle_is_reversed() {return spindle.reversed;}
-static void _update_speed() {spindle_set_speed(spindle.speed);}
-// Var callbacks
-uint8_t get_tool_type() {return spindle.type;}
+void spindle_update() {
+ while (!speed_q_empty()) {
+ speed_t s = speed_q_peek();
+ if (s.time == -1 || spindle.time < s.time) break;
+ speed_q_pop();
+ _set_speed(s.speed);
+ }
+
+ spindle.time++;
+}
+
+
+void spindle_next_segment() {
+ while (!speed_q_empty()) {
+ speed_t s = speed_q_next();
+ if (s.time == -1) break;
+ _set_speed(s.speed);
+ }
+
+ spindle.time = 0;
+ spindle_update();
+}
+
+
+void spindle_new_segment() {
+ speed_t s = {-1, 0};
+ if (!speed_q_full()) speed_q_push(s);
+}
+
+
+void spindle_queue_speed(int8_t time, float speed) {
+ if (speed_q_empty()) spindle_new_segment();
+
+ speed_t s = {time, speed};
+
+#if 1
+ if (!speed_q_full()) speed_q_push(s);
+
+#else
+ speed_t *last = speed_q_back();
+
+ if ((speed_q_empty() || last->time != time) && !speed_q_full())
+ speed_q_push(s);
+ else if (last->time == time) last->speed = speed;
+#endif
+}
+
+
+static void _update_speed() {_set_speed(spindle.speed);}
static void _deinit_cb() {
default: vfd_spindle_init(); break;
}
- spindle_set_speed(spindle.speed);
+ _set_speed(spindle.speed);
}
+// Var callbacks
+uint8_t get_tool_type() {return spindle.type;}
+
+
void set_tool_type(uint8_t value) {
if (value == spindle.type) return;
}
-void set_speed(float speed) {spindle_set_speed(speed);}
-float get_speed() {return spindle_get_speed();}
+float get_speed() {return _get_speed();}
+void set_speed(float speed) {spindle_queue_speed(0, speed);}
bool get_tool_reversed() {return spindle.reversed;}
#pragma once
#include <stdbool.h>
+#include <stdint.h>
typedef enum {
typedef void (*deinit_cb_t)();
+void spindle_init();
spindle_type_t spindle_get_type();
-void spindle_set_speed(float speed);
-float spindle_get_speed();
void spindle_stop();
bool spindle_is_reversed();
+void spindle_update();
+void spindle_next_segment();
+void spindle_new_segment();
+void spindle_queue_speed(int8_t time, float speed);
#include "util.h"
#include "cpp_magic.h"
#include "exec.h"
+#include "spindle.h"
#include "drv8711.h"
#include <string.h>
bool move_queued; // prepped move queued
move_type_t move_type;
float prep_dwell;
- uint16_t clock_period;
uint32_t underflow;
} stepper_t;
TIMER_STEP.INTCTRLA = STEP_TIMER_INTLVL; // interrupt mode
TIMER_STEP.PER = STEP_TIMER_POLL; // timer rate
TIMER_STEP.CTRLA = STEP_TIMER_ENABLE; // start step timer
-
- st.clock_period = STEP_TIMER_POLL;
}
/// Dwell or dequeue and load next move.
static void _load_move() {
- // New clock period
- TIMER_STEP.PER = st.clock_period;
+ static uint8_t tick = 0;
+
+ spindle_update();
// Dwell
if (0 < st.dwell) {
return;
} else st.dwell = 0;
+ if (tick++ & 3) return;
+
// If the next move is not ready try to load it
if (!st.move_ready) {
if (exec_get_velocity()) st.underflow++;
_request_exec_move();
_end_move();
+ tick = 0;
return;
}
+ spindle_next_segment();
+
// Start move
if (st.move_type == MOVE_TYPE_LINE)
for (int motor = 0; motor < MOTORS; motor++)
st.move_type = MOVE_TYPE_NULL;
st.prep_dwell = 0; // clear dwell
st.move_ready = false; // flip the flag back
- st.clock_period = STEP_TIMER_POLL;
// Request next move if not currently in a dwell. Requesting the next move
// may power up motors and the motors should not be powered up during a dwell.
// Setup segment parameters
st.move_type = MOVE_TYPE_LINE;
- st.clock_period = SEGMENT_TIME * 60 * STEP_TIMER_FREQ;
// Prepare motor moves
for (int motor = 0; motor < MOTORS; motor++)
def speed(speed): return set_float('s', speed)
-def sync_speed(offset, speed):
- return SYNC_SPEED + encode_float(offset) + encode_float(speed)
+def sync_speed(time, speed):
+ return SYNC_SPEED + encode_float(time) + encode_float(speed)
def input(port, mode, timeout):
help = 'HTTP address to bind')
parser.add_argument('-s', '--serial', default = '/dev/ttyAMA0',
help = 'Serial device')
- parser.add_argument('-b', '--baud', default = 230400, type = int,
+ parser.add_argument('-b', '--baud', default = 115200, type = int,
help = 'Serial baud rate')
parser.add_argument('--i2c-port', default = 1, type = int,
help = 'I2C port')
projects / bbctrl-firmware / commitdiff