From 36cbee4312960642619c664398ba728a2010b87a Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Fri, 2 Nov 2018 01:57:47 -0700 Subject: [PATCH] Working on PWM speed sync --- src/avr/src/config.h | 2 +- src/avr/src/line.c | 48 ++++++++++++-------- src/avr/src/main.c | 2 + src/avr/src/pwm_spindle.c | 2 + src/avr/src/ringbuf.def | 11 +++++ src/avr/src/spindle.c | 96 +++++++++++++++++++++++++++++++++++---- src/avr/src/spindle.h | 8 +++- src/avr/src/stepper.c | 16 ++++--- src/py/bbctrl/Cmd.py | 4 +- src/py/bbctrl/__init__.py | 2 +- 10 files changed, 148 insertions(+), 43 deletions(-) diff --git a/src/avr/src/config.h b/src/avr/src/config.h index 15cf514..d8bb92d 100644 --- a/src/avr/src/config.h +++ b/src/avr/src/config.h @@ -191,7 +191,7 @@ enum { // 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 diff --git a/src/avr/src/line.c b/src/avr/src/line.c index 8ced036..7722a24 100644 --- a/src/avr/src/line.c +++ b/src/avr/src/line.c @@ -53,7 +53,7 @@ typedef struct { typedef struct { - float offset; + float time; float speed; } speed_t; @@ -70,6 +70,7 @@ static struct { float jerk; float lV; // Last velocity + float lineT; speed_t speed; } l; @@ -121,23 +122,30 @@ static bool _section_next() { } -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); } @@ -153,6 +161,9 @@ static stat_t _line_exec() { 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); @@ -161,9 +172,6 @@ static stat_t _line_exec() { // 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()) { @@ -171,14 +179,15 @@ static stat_t _line_exec() { 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); } } @@ -187,8 +196,7 @@ static stat_t _line_exec() { _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); } @@ -271,8 +279,8 @@ unsigned command_line_size() {return sizeof(line_t);} 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; @@ -311,7 +319,7 @@ stat_t command_sync_speed(char *cmd) { 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 @@ -326,5 +334,5 @@ unsigned command_sync_speed_size() {return sizeof(speed_t);} void command_sync_speed_exec(void *data) { speed_t s = *(speed_t *)data; - spindle_set_speed(s.speed); + spindle_queue_speed(0, s.speed); } diff --git a/src/avr/src/main.c b/src/avr/src/main.c index f93bcf5..a6b3470 100644 --- a/src/avr/src/main.c +++ b/src/avr/src/main.c @@ -42,6 +42,7 @@ #include "analog.h" #include "exec.h" #include "state.h" +#include "spindle.h" #include @@ -68,6 +69,7 @@ int main() { vars_init(); // configuration variables estop_init(); // emergency stop handler command_init(); // command queue + spindle_init(); // spindle sei(); // enable interrupts diff --git a/src/avr/src/pwm_spindle.c b/src/avr/src/pwm_spindle.c index 027eb71..a7c36f4 100644 --- a/src/avr/src/pwm_spindle.c +++ b/src/avr/src/pwm_spindle.c @@ -61,6 +61,7 @@ static void _update_pwm() { // 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); @@ -70,6 +71,7 @@ static void _update_pwm() { // 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; diff --git a/src/avr/src/ringbuf.def b/src/avr/src/ringbuf.def index 96d1db1..1f444a0 100644 --- a/src/avr/src/ringbuf.def +++ b/src/avr/src/ringbuf.def @@ -101,6 +101,7 @@ RING_BUF_FUNC void CONCAT(RING_BUF_NAME, _init)() { #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 @@ -166,6 +167,16 @@ RING_BUF_FUNC RING_BUF_TYPE CONCAT(RING_BUF_NAME, _peek)() { } +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]; } diff --git a/src/avr/src/spindle.c b/src/avr/src/spindle.c index e0d5e04..ca733cd 100644 --- a/src/avr/src/spindle.c +++ b/src/avr/src/spindle.c @@ -35,6 +35,21 @@ #include +#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; @@ -43,14 +58,25 @@ static struct { 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; @@ -71,7 +97,7 @@ void spindle_set_speed(float speed) { } -float spindle_get_speed() { +float _get_speed() { float speed = 0; switch (spindle.type) { @@ -85,15 +111,61 @@ float spindle_get_speed() { } -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() { @@ -107,10 +179,14 @@ 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; @@ -127,8 +203,8 @@ void set_tool_type(uint8_t value) { } -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;} diff --git a/src/avr/src/spindle.h b/src/avr/src/spindle.h index 83079fb..1947aa6 100644 --- a/src/avr/src/spindle.h +++ b/src/avr/src/spindle.h @@ -28,6 +28,7 @@ #pragma once #include +#include typedef enum { @@ -45,8 +46,11 @@ 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); diff --git a/src/avr/src/stepper.c b/src/avr/src/stepper.c index f59866e..89397c1 100644 --- a/src/avr/src/stepper.c +++ b/src/avr/src/stepper.c @@ -34,6 +34,7 @@ #include "util.h" #include "cpp_magic.h" #include "exec.h" +#include "spindle.h" #include "drv8711.h" #include @@ -58,7 +59,6 @@ typedef struct { bool move_queued; // prepped move queued move_type_t move_type; float prep_dwell; - uint16_t clock_period; uint32_t underflow; } stepper_t; @@ -77,8 +77,6 @@ void stepper_init() { 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; } @@ -146,8 +144,9 @@ static void _request_exec_move() { /// 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) { @@ -155,14 +154,19 @@ static void _load_move() { 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++) @@ -180,7 +184,6 @@ static void _load_move() { 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. @@ -198,7 +201,6 @@ void st_prep_line(const float target[]) { // 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++) diff --git a/src/py/bbctrl/Cmd.py b/src/py/bbctrl/Cmd.py index f82fea4..4700471 100644 --- a/src/py/bbctrl/Cmd.py +++ b/src/py/bbctrl/Cmd.py @@ -124,8 +124,8 @@ def line(target, exitVel, maxAccel, maxJerk, times, speeds): 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): diff --git a/src/py/bbctrl/__init__.py b/src/py/bbctrl/__init__.py index d18c7fa..098a80f 100644 --- a/src/py/bbctrl/__init__.py +++ b/src/py/bbctrl/__init__.py @@ -91,7 +91,7 @@ def parse_args(): 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') -- 2.27.0