From 9ef27108d2c877630edaa792d8b2c6b69953ddce Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sun, 27 Mar 2016 06:24:47 -0700 Subject: [PATCH] DMA channels to count motor pulses, more reorg, much faster motor SPI --- src/clock.c | 81 ----------------------- src/clock.h | 35 ---------- src/config.h | 32 +++++----- src/hardware.c | 64 ++++++++++++++++--- src/main.c | 47 +++++--------- src/motor.c | 57 +++++++---------- src/motor.h | 4 +- src/pwm.c | 90 ++++++++------------------ src/pwm.h | 37 ++--------- src/spindle.c | 116 ++++++++++++++++++++------------- src/spindle.h | 2 +- src/stepper.c | 27 +++----- src/switch.c | 4 +- src/tmc2660.c | 170 ++++++++++++++++++++++++------------------------- src/usart.c | 4 +- src/usart.h | 8 +-- 16 files changed, 317 insertions(+), 461 deletions(-) delete mode 100644 src/clock.c delete mode 100644 src/clock.h diff --git a/src/clock.c b/src/clock.c deleted file mode 100644 index 8c50d0b..0000000 --- a/src/clock.c +++ /dev/null @@ -1,81 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "clock.h" - -#include "hardware.h" - -#include -#include - - -/// This routine is lifted and modified from Boston Android and from -/// http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&p=711659 -void clock_init() { - // external 8 Mhx Xtal with 4x PLL = 32 Mhz -#ifdef __CLOCK_EXTERNAL_8MHZ - OSC.XOSCCTRL = 0x4B; // 2-9 MHz crystal; 0.4-16 MHz XTAL w/16K CLK startup - OSC.CTRL = 0x08; // enable external crystal oscillator - while (!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready - - OSC.PLLCTRL = 0xC4; // XOSC is PLL Source; 4x Factor (32 MHz sys clock) - OSC.CTRL = 0x18; // Enable PLL & External Oscillator - while (!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready - - CCP = CCP_IOREG_gc; - CLK.CTRL = CLK_SCLKSEL_PLL_gc; // switch to PLL clock - - OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock -#endif - - // external 16 Mhx Xtal with 2x PLL = 32 Mhz -#ifdef __CLOCK_EXTERNAL_16MHZ - OSC.XOSCCTRL = 0xCB; // 12-16 MHz crystal; 0.4-16 MHz XTAL w/16K CLK startup - OSC.CTRL = 0x08; // enable external crystal oscillator - while (!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready - - OSC.PLLCTRL = 0xC2; // XOSC is PLL Source; 2x Factor (32 MHz sys clock) - OSC.CTRL = 0x18; // Enable PLL & External Oscillator - while(!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready - - CCP = CCP_IOREG_gc; - CLK.CTRL = CLK_SCLKSEL_PLL_gc; // switch to PLL clock - - OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock -#endif - - // 32 MHz internal clock (Boston Android code) -#ifdef __CLOCK_INTERNAL_32MHZ - CCP = CCP_IOREG_gc; // Security Signature to modify clk - OSC.CTRL = OSC_RC32MEN_bm; // enable internal 32MHz oscillator - while (!(OSC.STATUS & OSC_RC32MRDY_bm)); // wait for oscillator ready - - CCP = CCP_IOREG_gc; // Security Signature to modify clk - CLK.CTRL = 0x01; // select sysclock 32MHz osc -#endif -} diff --git a/src/clock.h b/src/clock.h deleted file mode 100644 index 02fd34d..0000000 --- a/src/clock.h +++ /dev/null @@ -1,35 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - - -#include - -void clock_init(); -void CCPWrite(volatile uint8_t *address, uint8_t value); diff --git a/src/config.h b/src/config.h index ed9ac40..374462b 100644 --- a/src/config.h +++ b/src/config.h @@ -35,7 +35,7 @@ //#define __JERK_EXEC // Use computed jerk (vs. forward difference) //#define __KAHAN // Use Kahan summation in aline exec functions #define __CLOCK_EXTERNAL_16MHZ // uses PLL to provide 32 MHz system clock - +//#define __CLOCK_INTERNAL_32MHZ #define AXES 6 // number of axes #define MOTORS 4 // number of motors on the board @@ -211,17 +211,17 @@ typedef enum { #define C_ZERO_BACKOFF 2 -// PWM settings -#define P1_PWM_FREQUENCY 100 // in Hz -#define P1_CW_SPEED_LO 1000 // in RPM (arbitrary units) -#define P1_CW_SPEED_HI 2000 -#define P1_CW_PHASE_LO 0.125 // phase [0..1] -#define P1_CW_PHASE_HI 0.2 -#define P1_CCW_SPEED_LO 1000 -#define P1_CCW_SPEED_HI 2000 -#define P1_CCW_PHASE_LO 0.125 -#define P1_CCW_PHASE_HI 0.2 -#define P1_PWM_PHASE_OFF 0.1 +// Spindle settings +#define SPINDLE_PWM_FREQUENCY 100 // in Hz +#define SPINDLE_CW_SPEED_LO 1000 // in RPM (arbitrary units) +#define SPINDLE_CW_SPEED_HI 2000 +#define SPINDLE_CW_PHASE_LO 0.125 // phase [0..1] +#define SPINDLE_CW_PHASE_HI 0.2 +#define SPINDLE_CCW_SPEED_LO 1000 +#define SPINDLE_CCW_SPEED_HI 2000 +#define SPINDLE_CCW_PHASE_LO 0.125 +#define SPINDLE_CCW_PHASE_HI 0.2 +#define SPINDLE_PWM_PHASE_OFF 0.1 // Gcode defaults @@ -351,14 +351,16 @@ typedef enum { #define TMC2660_SPI_MISO_PIN 6 #define TMC2660_SPI_MOSI_PIN 7 #define TMC2660_TIMER TCC1 -#define TMC2660_POLL_RATE 0.01 // sec. Must be in (0, 1] -#define TMC2660_STABILIZE_TIME 0.01 // sec. Must be at least 1ms +#define TMC2660_TIMER_ENABLE TC_CLKSEL_DIV64_gc +#define TMC2660_POLL_RATE 0.001 // sec. Must be in (0, 1] +#define TMC2660_STABILIZE_TIME 0.001 // sec. Must be at least 1ms // PWM settings +#define PWM_MAX_FREQ (F_CPU / 256) // with 8-bits duty cycle precision +#define PWM_MIN_FREQ (F_CPU / 64 / 65536) // min frequency for prescaling #define PWM1_CTRLB (3 | TC1_CCBEN_bm) // single slope PWM channel B #define PWM1_ISR_vect TCD1_CCB_vect -#define PWM2_CTRLA_CLKSEL TC_CLKSEL_DIV1_gc #define PWM2_CTRLB 3 // single slope PWM no output #define PWM2_ISR_vect TCE1_CCB_vect diff --git a/src/hardware.c b/src/hardware.c index 86223ae..95f3155 100644 --- a/src/hardware.c +++ b/src/hardware.c @@ -29,7 +29,7 @@ #include "hardware.h" #include "rtc.h" #include "usart.h" -#include "clock.h" +#include "config.h" #include #include @@ -41,8 +41,8 @@ typedef struct { char id[26]; - bool hard_reset_requested; // flag to perform a hard reset - bool bootloader_requested; // flag to enter the bootloader + bool hard_reset; // flag to perform a hard reset + bool bootloader; // flag to enter the bootloader } hw_t; static hw_t hw = {}; @@ -52,6 +52,52 @@ static hw_t hw = {}; #define HEXNIB(x) "0123456789abcdef"[(x) & 0xf] +/// This routine is lifted and modified from Boston Android and from +/// http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&p=711659 +static void _init_clock() { +#if defined(__CLOCK_EXTERNAL_8MHZ) // external 8 Mhx Xtal w/ 4x PLL = 32 Mhz + // 2-9 MHz crystal; 0.4-16 MHz XTAL w/ 16K CLK startup + OSC.XOSCCTRL = OSC_FRQRANGE_2TO9_gc | OSC_XOSCSEL_XTAL_16KCLK_gc; + OSC.CTRL = OSC_XOSCEN_bm; // enable external crystal oscillator + while (!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready + + OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | 4; // PLL source, 4x (32 MHz sys clock) + OSC.CTRL = OSC_PLLEN_bm | OSC_XOSCEN_bm; // Enable PLL & External Oscillator + while (!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready + + CCP = CCP_IOREG_gc; + CLK.CTRL = CLK_SCLKSEL_PLL_gc; // switch to PLL clock + + OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock + +#elif defined(__CLOCK_EXTERNAL_16MHZ) // external 16Mhz Xtal w/ 2x PLL = 32 Mhz + // 12-16 MHz crystal; 0.4-16 MHz XTAL w/ 16K CLK startup + OSC.XOSCCTRL = OSC_FRQRANGE_12TO16_gc | OSC_XOSCSEL_XTAL_16KCLK_gc; + OSC.CTRL = OSC_XOSCEN_bm; // enable external crystal oscillator + while (!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready + + OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | 2; // PLL source, 2x (32 MHz sys clock) + OSC.CTRL = OSC_PLLEN_bm | OSC_XOSCEN_bm; // Enable PLL & External Oscillator + while (!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready + + CCP = CCP_IOREG_gc; + CLK.CTRL = CLK_SCLKSEL_PLL_gc; // switch to PLL clock + + OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock + +#elif defined(__CLOCK_INTERNAL_32MHZ) // 32 MHz internal clock + OSC.CTRL = OSC_RC32MEN_bm; // enable internal 32MHz oscillator + while (!(OSC.STATUS & OSC_RC32MRDY_bm)); // wait for oscillator ready + + CCP = CCP_IOREG_gc; // Security Signature to modify clk + CLK.CTRL = CLK_SCLKSEL_RC32M_gc; // select sysclock 32MHz osc + +#else +#error No clock defined +#endif +} + + static void _load_hw_id_byte(int i, register8_t *reg) { NVM.CMD = NVM_CMD_READ_CALIB_ROW_gc; uint8_t byte = pgm_read_byte(reg); @@ -84,7 +130,7 @@ static void _read_hw_id() { /// Lowest level hardware init void hardware_init() { - clock_init(); // set system clock + _init_clock(); // set system clock rtc_init(); // real time counter _read_hw_id(); @@ -95,7 +141,7 @@ void hardware_init() { } -void hw_request_hard_reset() {hw.hard_reset_requested = true;} +void hw_request_hard_reset() {hw.hard_reset = true;} /// Hard reset using watchdog timer @@ -110,11 +156,11 @@ void hw_hard_reset() { /// Controller's rest handler stat_t hw_reset_handler() { - if (hw.hard_reset_requested) hw_hard_reset(); + if (hw.hard_reset) hw_hard_reset(); - if (hw.bootloader_requested) { + if (hw.bootloader) { // TODO enable bootloader interrupt vectors and jump to BOOT_SECTION_START - hw.bootloader_requested = false; + hw.bootloader = false; } return STAT_NOOP; @@ -122,7 +168,7 @@ stat_t hw_reset_handler() { /// Executes a software reset using CCPWrite -void hw_request_bootloader() {hw.bootloader_requested = true;} +void hw_request_bootloader() {hw.bootloader = true;} uint8_t hw_disable_watchdog() { diff --git a/src/main.c b/src/main.c index 58357bc..c406b1d 100644 --- a/src/main.c +++ b/src/main.c @@ -50,6 +50,7 @@ #include #include +#include static stat_t _shutdown_idler() { @@ -102,57 +103,41 @@ static stat_t _limit_switch_handler() { void _run() { #define DISPATCH(func) if (func == STAT_EAGAIN) return; - // Interrupt Service Routines are the highest priority - // See hardware.h for a list of ISRs and their priorities. - - // Kernel level ISR handlers, flags are set in ISRs, order is important DISPATCH(hw_reset_handler()); // handle hard reset requests DISPATCH(_shutdown_idler()); // idle in shutdown state DISPATCH(_limit_switch_handler()); // limit switch thrown - DISPATCH(cm_feedhold_sequencing_callback()); // feedhold state machine - DISPATCH(mp_plan_hold_callback()); // plan a feedhold DISPATCH(tmc2660_sync()); // synchronize driver config DISPATCH(motor_power_callback()); // stepper motor power sequencing - // Planner hierarchy for gcode and cycles + DISPATCH(cm_feedhold_sequencing_callback()); // feedhold state machine + DISPATCH(mp_plan_hold_callback()); // plan a feedhold DISPATCH(cm_arc_callback()); // arc generation runs DISPATCH(cm_homing_callback()); // G28.2 continuation DISPATCH(cm_probe_callback()); // G38.2 continuation - // Command readers and parsers - // ensure at least one free buffer in planning queue - DISPATCH(_sync_to_planner()); - // sync with TX buffer (pseudo-blocking) - DISPATCH(_sync_to_tx_buffer()); - - DISPATCH(report_callback()); + DISPATCH(_sync_to_planner()); // ensure a free planning buffer + DISPATCH(_sync_to_tx_buffer()); // sync with TX buffer + DISPATCH(report_callback()); // report changes DISPATCH(command_dispatch()); // read and execute next command } static void _init() { - // There are a lot of dependencies in the order of these inits. - // Don't change the ordering unless you understand this. - - cli(); // disable global interrupts + cli(); // disable interrupts - // do these first - hardware_init(); // system hardware setup - must be first + hardware_init(); // hardware setup - must be first usart_init(); // serial port - - // do these next tmc2660_init(); // motor drivers - stepper_init(); // stepper subsystem - motor_init(); + stepper_init(); // steppers + motor_init(); // motors switch_init(); // switches - pwm_init(); // pulse width modulation drivers - - planner_init(); // motion planning subsystem - canonical_machine_init(); // must follow config_init() - vars_init(); + pwm_init(); // PWM drivers + planner_init(); // motion planning + canonical_machine_init(); // gcode machine + vars_init(); // configuration variables - sei(); // enable global interrupts + sei(); // enable interrupts fprintf_P(stderr, PSTR("\nBuildbotics firmware\n")); } @@ -164,7 +149,7 @@ int main() { _init(); // main loop - while (1) { + while (true) { _run(); // single pass wdt_reset(); } diff --git a/src/motor.c b/src/motor.c index c56c454..6d94380 100644 --- a/src/motor.c +++ b/src/motor.c @@ -143,21 +143,25 @@ ISR(TCE1_CCA_vect) { ISR(DMA_CH0_vect) { M1_TIMER.CTRLA = 0; // Top motor clock + M1_DMA_CH.CTRLB |= DMA_CH_TRNIF_bm; // Clear interrupt flag } ISR(DMA_CH1_vect) { M2_TIMER.CTRLA = 0; // Top motor clock + M2_DMA_CH.CTRLB |= DMA_CH_TRNIF_bm; // Clear interrupt flag } ISR(DMA_CH2_vect) { M3_TIMER.CTRLA = 0; // Top motor clock + M3_DMA_CH.CTRLB |= DMA_CH_TRNIF_bm; // Clear interrupt flag } ISR(DMA_CH3_vect) { M4_TIMER.CTRLA = 0; // Top motor clock + M4_DMA_CH.CTRLB |= DMA_CH_TRNIF_bm; // Clear interrupt flag } @@ -208,9 +212,9 @@ int motor_get_axis(int motor) { } -int motor_get_steps_per_unit(int motor) { - return (360 * motors[motor].microsteps) / - (motors[motor].travel_rev * motors[motor].step_angle); +float motor_get_steps_per_unit(int motor) { + return 360 * motors[motor].microsteps / motors[motor].travel_rev / + motors[motor].step_angle; } @@ -306,9 +310,19 @@ void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps, float error) { motor_t *m = &motors[motor]; - if (fp_ZERO(travel_steps)) { - m->timer_clock = 0; // Motor clock off - return; + // Power motor + switch (motors[motor].power_mode) { + case MOTOR_DISABLED: return; + + case MOTOR_POWERED_ONLY_WHEN_MOVING: + if (fp_ZERO(travel_steps)) return; // Not moving + // Fall through + + case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE: + _energize(motor); + break; + + case MOTOR_POWER_MODE_MAX_VALUE: break; // Shouldn't get here } #ifdef __STEP_CORRECTION @@ -333,9 +347,9 @@ void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps, // Compute motor timer clock and period. Rounding is performed to eliminate // a negative bias in the uint32_t conversion that results in long-term // negative drift. - uint16_t steps = round(fabs(travel_steps)); + uint16_t steps = round(fabs(travel_steps / 2)); // TODO why do we need to multiply by 2 here? - uint32_t ticks_per_step = seg_clocks / steps * 2; + uint32_t ticks_per_step = seg_clocks / steps; // Find the clock rate that will fit the required number of steps if (ticks_per_step & 0xffff0000UL) { @@ -350,14 +364,14 @@ void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps, ticks_per_step /= 2; seg_clocks /= 2; - if (ticks_per_step & 0xffff0000UL) m->timer_clock = 0; // Off + if (ticks_per_step & 0xffff0000UL) m->timer_clock = 0; // Off, too slow else m->timer_clock = TC_CLKSEL_DIV8_gc; } else m->timer_clock = TC_CLKSEL_DIV4_gc; } else m->timer_clock = TC_CLKSEL_DIV2_gc; } else m->timer_clock = TC_CLKSEL_DIV1_gc; m->timer_period = ticks_per_step; - m->steps = seg_clocks / ticks_per_step; // Compute actual steps + m->steps = steps; // Setup the direction, compensating for polarity. if (0 <= travel_steps) m->direction = DIRECTION_CW ^ m->polarity; @@ -368,23 +382,6 @@ void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps, } -void motor_begin_move(int motor) { - switch (motors[motor].power_mode) { - case MOTOR_DISABLED: return; - - case MOTOR_POWERED_ONLY_WHEN_MOVING: - if (!motors[motor].timer_clock) return; // Not moving - // Fall through - - case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE: - _energize(motor); - break; - - case MOTOR_POWER_MODE_MAX_VALUE: break; // Shouldn't get here - } -} - - void motor_load_move(int motor) { motor_t *m = &motors[motor]; @@ -407,12 +404,6 @@ void motor_load_move(int motor) { } -void motor_end_move(int motor) { - // Disable motor clock - //motors[motor].timer->CTRLA = 0; -} - - // Var callbacks float get_step_angle(int motor) { return motors[motor].step_angle; diff --git a/src/motor.h b/src/motor.h index 5482be2..bbf149a 100644 --- a/src/motor.h +++ b/src/motor.h @@ -66,7 +66,7 @@ void motor_init(); void motor_shutdown(int motor); int motor_get_axis(int motor); -int motor_get_steps_per_unit(int motor); +float motor_get_steps_per_unit(int motor); int32_t motor_get_encoder(int motor); void motor_set_encoder(int motor, float encoder); @@ -78,6 +78,4 @@ void motor_error_callback(int motor, cmMotorFlags_t errors); void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps, float error); -void motor_begin_move(int motor); void motor_load_move(int motor); -void motor_end_move(int motor); diff --git a/src/pwm.c b/src/pwm.c index 58ca111..8814435 100644 --- a/src/pwm.c +++ b/src/pwm.c @@ -34,27 +34,16 @@ #include -pwmSingleton_t pwm; +typedef struct { + TC1_t *timer; // assumes TC1 flavor timers used for PWM channels +} pwm_t; -// defines common to all PWM channels -#define PWM_TIMER_t TC1_t // PWM uses TC1's -#define PWM_TIMER_DISABLE 0 // turn timer off (clock = 0 Hz) -#define PWM_MAX_FREQ (F_CPU / 256) // with 8-bits duty cycle precision -#define PWM_MIN_FREQ (F_CPU / 64 / 65536) // min frequency for prescaling - -/* CLKSEL is used to configure default PWM clock operating ranges - * These can be changed by pwm_freq() depending on the PWM frequency selected - * - * The useful ranges (assuming a 32 Mhz system clock) are: - * TC_CLKSEL_DIV1_gc - good for about 500 Hz to 125 Khz practical upper limit - * TC_CLKSEL_DIV2_gc - good for about 250 Hz to 62 KHz - * TC_CLKSEL_DIV4_gc - good for about 125 Hz to 31 KHz - * TC_CLKSEL_DIV8_gc - good for about 62 Hz to 16 KHz - * TC_CLKSEL_DIV64_gc - good for about 8 Hz to 2 Khz - */ -#define PWM1_CTRLA_CLKSEL TC_CLKSEL_DIV1_gc // starting clock select value +pwm_t pwm[PWMS] = { + {.timer = &TIMER_PWM1}, + {.timer = &TIMER_PWM2} +}; /* Initialize pwm channels @@ -69,34 +58,8 @@ void pwm_init() { gpio_set_bit_off(SPINDLE_PWM); - // setup PWM channel 1 - // clear parent structure - memset(&pwm.p[PWM_1], 0, sizeof(pwmChannel_t)); - - // bind timer struct to PWM struct array - pwm.p[PWM_1].timer = &TIMER_PWM1; - // initialize starting clock operating range - pwm.p[PWM_1].ctrla = PWM1_CTRLA_CLKSEL; - pwm.p[PWM_1].timer->CTRLB = PWM1_CTRLB; - - // setup PWM channel 2 - // clear all values, pointers and status - memset(&pwm.p[PWM_2], 0, sizeof(pwmChannel_t)); - - pwm.p[PWM_2].timer = &TIMER_PWM2; - pwm.p[PWM_2].ctrla = PWM2_CTRLA_CLKSEL; - pwm.p[PWM_2].timer->CTRLB = PWM2_CTRLB; - - pwm.c[PWM_1].frequency = P1_PWM_FREQUENCY; - pwm.c[PWM_1].cw_speed_lo = P1_CW_SPEED_LO; - pwm.c[PWM_1].cw_speed_hi = P1_CW_SPEED_HI; - pwm.c[PWM_1].cw_phase_lo = P1_CW_PHASE_LO; - pwm.c[PWM_1].cw_phase_hi = P1_CW_PHASE_HI; - pwm.c[PWM_1].ccw_speed_lo = P1_CCW_SPEED_LO; - pwm.c[PWM_1].ccw_speed_hi = P1_CCW_SPEED_HI; - pwm.c[PWM_1].ccw_phase_lo = P1_CCW_PHASE_LO; - pwm.c[PWM_1].ccw_phase_hi = P1_CCW_PHASE_HI; - pwm.c[PWM_1].phase_off = P1_PWM_PHASE_OFF; + pwm[PWM_1].timer->CTRLB = PWM1_CTRLB; + pwm[PWM_2].timer->CTRLB = PWM2_CTRLB; } @@ -109,32 +72,31 @@ void pwm_init() { * Doesn't turn time on until duty cycle is set */ stat_t pwm_set_freq(uint8_t chan, float freq) { - if (chan > PWMS) return STAT_NO_SUCH_DEVICE; - if (freq > PWM_MAX_FREQ) return STAT_INPUT_EXCEEDS_MAX_VALUE; + if (PWMS <= chan) return STAT_NO_SUCH_DEVICE; + if (PWM_MAX_FREQ < freq) return STAT_INPUT_EXCEEDS_MAX_VALUE; if (freq < PWM_MIN_FREQ) return STAT_INPUT_LESS_THAN_MIN_VALUE; - // Set the period and the prescaler - // optimal non-integer prescaler value + // Set period and optimal prescaler value float prescale = F_CPU / 65536 / freq; if (prescale <= 1) { - pwm.p[chan].timer->PER = F_CPU / freq; - pwm.p[chan].timer->CTRLA = TC_CLKSEL_DIV1_gc; + pwm[chan].timer->PER = F_CPU / freq; + pwm[chan].timer->CTRLA = TC_CLKSEL_DIV1_gc; } else if (prescale <= 2) { - pwm.p[chan].timer->PER = F_CPU / 2 / freq; - pwm.p[chan].timer->CTRLA = TC_CLKSEL_DIV2_gc; + pwm[chan].timer->PER = F_CPU / 2 / freq; + pwm[chan].timer->CTRLA = TC_CLKSEL_DIV2_gc; } else if (prescale <= 4) { - pwm.p[chan].timer->PER = F_CPU / 4 / freq; - pwm.p[chan].timer->CTRLA = TC_CLKSEL_DIV4_gc; + pwm[chan].timer->PER = F_CPU / 4 / freq; + pwm[chan].timer->CTRLA = TC_CLKSEL_DIV4_gc; } else if (prescale <= 8) { - pwm.p[chan].timer->PER = F_CPU / 8 / freq; - pwm.p[chan].timer->CTRLA = TC_CLKSEL_DIV8_gc; + pwm[chan].timer->PER = F_CPU / 8 / freq; + pwm[chan].timer->CTRLA = TC_CLKSEL_DIV8_gc; } else { - pwm.p[chan].timer->PER = F_CPU / 64 / freq; - pwm.p[chan].timer->CTRLA = TC_CLKSEL_DIV64_gc; + pwm[chan].timer->PER = F_CPU / 64 / freq; + pwm[chan].timer->CTRLA = TC_CLKSEL_DIV64_gc; } return STAT_OK; @@ -154,13 +116,13 @@ stat_t pwm_set_freq(uint8_t chan, float freq) { * The frequency must have been set previously */ stat_t pwm_set_duty(uint8_t chan, float duty) { - if (duty < 0.0) return STAT_INPUT_LESS_THAN_MIN_VALUE; - if (duty > 1.0) return STAT_INPUT_EXCEEDS_MAX_VALUE; + if (duty < 0) return STAT_INPUT_LESS_THAN_MIN_VALUE; + if (duty > 1) return STAT_INPUT_EXCEEDS_MAX_VALUE; // Ffrq = Fper / 2N(CCA + 1) // Fpwm = Fper / N(PER + 1) - float period_scalar = pwm.p[chan].timer->PER; - pwm.p[chan].timer->CCB = (uint16_t)(period_scalar * duty) + 1; + float period_scalar = pwm[chan].timer->PER; + pwm[chan].timer->CCB = period_scalar * duty + 1; return STAT_OK; } diff --git a/src/pwm.h b/src/pwm.h index 00119b3..58b8612 100644 --- a/src/pwm.h +++ b/src/pwm.h @@ -29,42 +29,13 @@ #pragma once -#include "config.h" #include "status.h" -#include - -#define PWM_1 0 -#define PWM_2 1 - - -typedef struct pwmConfigChannel { - float frequency; // base frequency for PWM driver, in Hz - float cw_speed_lo; // minimum clockwise spindle speed [0..N] - float cw_speed_hi; // maximum clockwise spindle speed - float cw_phase_lo; // pwm phase at minimum CW spindle speed, clamped [0..1] - float cw_phase_hi; // pwm phase at maximum CW spindle speed, clamped [0..1] - float ccw_speed_lo; // minimum counter-clockwise spindle speed [0..N] - float ccw_speed_hi; // maximum counter-clockwise spindle speed - float ccw_phase_lo; // pwm phase at minimum CCW spindle speed, clamped [0..1] - float ccw_phase_hi; // pwm phase at maximum CCW spindle speed, clamped - float phase_off; // pwm phase when spindle is disabled -} pwmConfigChannel_t; - - -typedef struct pwmChannel { - uint8_t ctrla; // byte needed to active CTRLA (it's dynamic - rest are static) - TC1_t *timer; // assumes TC1 flavor timers used for PWM channels -} pwmChannel_t; - - -typedef struct pwmSingleton { - pwmConfigChannel_t c[PWMS]; // array of channel configs - pwmChannel_t p[PWMS]; // array of PWM channels -} pwmSingleton_t; - -extern pwmSingleton_t pwm; +enum { + PWM_1, + PWM_2, +}; void pwm_init(); diff --git a/src/spindle.c b/src/spindle.c index 7cb4626..332dbf5 100644 --- a/src/spindle.c +++ b/src/spindle.c @@ -36,33 +36,87 @@ #include "plan/command.h" -static void _exec_spindle_control(float *value, float *flag); -static void _exec_spindle_speed(float *value, float *flag); +typedef struct { + float frequency; // base frequency for PWM driver, in Hz + float cw_speed_lo; // minimum clockwise spindle speed [0..N] + float cw_speed_hi; // maximum clockwise spindle speed + float cw_phase_lo; // pwm phase at minimum CW spindle speed, clamped [0..1] + float cw_phase_hi; // pwm phase at maximum CW spindle speed, clamped [0..1] + float ccw_speed_lo; // minimum counter-clockwise spindle speed [0..N] + float ccw_speed_hi; // maximum counter-clockwise spindle speed + float ccw_phase_lo; // pwm phase at minimum CCW spindle speed, clamped [0..1] + float ccw_phase_hi; // pwm phase at maximum CCW spindle speed, clamped + float phase_off; // pwm phase when spindle is disabled +} spindle_t; + + +static spindle_t spindle = { + .frequency = SPINDLE_PWM_FREQUENCY, + .cw_speed_lo = SPINDLE_CW_SPEED_LO, + .cw_speed_hi = SPINDLE_CW_SPEED_HI, + .cw_phase_lo = SPINDLE_CW_PHASE_LO, + .cw_phase_hi = SPINDLE_CW_PHASE_HI, + .ccw_speed_lo = SPINDLE_CCW_SPEED_LO, + .ccw_speed_hi = SPINDLE_CCW_SPEED_HI, + .ccw_phase_lo = SPINDLE_CCW_PHASE_LO, + .ccw_phase_hi = SPINDLE_CCW_PHASE_HI, + .phase_off = SPINDLE_PWM_PHASE_OFF, +}; + + +/// execute the spindle command (called from planner) +static void _exec_spindle_control(float *value, float *flag) { + uint8_t spindle_mode = (uint8_t)value[0]; + + cm_set_spindle_mode(MODEL, spindle_mode); + + if (spindle_mode == SPINDLE_CW) { + gpio_set_bit_on(SPINDLE_BIT); + gpio_set_bit_off(SPINDLE_DIR); + + } else if (spindle_mode == SPINDLE_CCW) { + gpio_set_bit_on(SPINDLE_BIT); + gpio_set_bit_on(SPINDLE_DIR); + + } else gpio_set_bit_off(SPINDLE_BIT); // failsafe: any error causes stop + + // PWM spindle control + pwm_set_duty(PWM_1, cm_get_spindle_pwm(spindle_mode) ); +} + + +/// Spindle speed callback from planner queue +static void _exec_spindle_speed(float *value, float *flag) { + cm_set_spindle_speed_parameter(MODEL, value[0]); + // update spindle speed if we're running + pwm_set_duty(PWM_1, cm_get_spindle_pwm(cm.gm.spindle_mode)); +} void cm_spindle_init() { - if( pwm.c[PWM_1].frequency < 0 ) - pwm.c[PWM_1].frequency = 0; + if( spindle.frequency < 0 ) + spindle.frequency = 0; - pwm_set_freq(PWM_1, pwm.c[PWM_1].frequency); - pwm_set_duty(PWM_1, pwm.c[PWM_1].phase_off); + pwm_set_freq(PWM_1, spindle.frequency); + pwm_set_duty(PWM_1, spindle.phase_off); } /// return PWM phase (duty cycle) for dir and speed -float cm_get_spindle_pwm( uint8_t spindle_mode ) { - float speed_lo=0, speed_hi=0, phase_lo=0, phase_hi=0; +float cm_get_spindle_pwm(uint8_t spindle_mode) { + float speed_lo = 0, speed_hi = 0, phase_lo = 0, phase_hi = 0; + if (spindle_mode == SPINDLE_CW) { - speed_lo = pwm.c[PWM_1].cw_speed_lo; - speed_hi = pwm.c[PWM_1].cw_speed_hi; - phase_lo = pwm.c[PWM_1].cw_phase_lo; - phase_hi = pwm.c[PWM_1].cw_phase_hi; + speed_lo = spindle.cw_speed_lo; + speed_hi = spindle.cw_speed_hi; + phase_lo = spindle.cw_phase_lo; + phase_hi = spindle.cw_phase_hi; } else if (spindle_mode == SPINDLE_CCW) { - speed_lo = pwm.c[PWM_1].ccw_speed_lo; - speed_hi = pwm.c[PWM_1].ccw_speed_hi; - phase_lo = pwm.c[PWM_1].ccw_phase_lo; - phase_hi = pwm.c[PWM_1].ccw_phase_hi; + speed_lo = spindle.ccw_speed_lo; + speed_hi = spindle.ccw_speed_hi; + phase_lo = spindle.ccw_phase_lo; + phase_hi = spindle.ccw_phase_hi; } if (spindle_mode == SPINDLE_CW || spindle_mode == SPINDLE_CCW) { @@ -74,7 +128,7 @@ float cm_get_spindle_pwm( uint8_t spindle_mode ) { float speed = (cm.gm.spindle_speed - speed_lo) / (speed_hi - speed_lo); return speed * (phase_hi - phase_lo) + phase_lo; - } else return pwm.c[PWM_1].phase_off; + } else return spindle.phase_off; } @@ -87,26 +141,6 @@ stat_t cm_spindle_control(uint8_t spindle_mode) { return STAT_OK; } -/// execute the spindle command (called from planner) -static void _exec_spindle_control(float *value, float *flag) { - uint8_t spindle_mode = (uint8_t)value[0]; - - cm_set_spindle_mode(MODEL, spindle_mode); - - if (spindle_mode == SPINDLE_CW) { - gpio_set_bit_on(SPINDLE_BIT); - gpio_set_bit_off(SPINDLE_DIR); - - } else if (spindle_mode == SPINDLE_CCW) { - gpio_set_bit_on(SPINDLE_BIT); - gpio_set_bit_on(SPINDLE_DIR); - - } else gpio_set_bit_off(SPINDLE_BIT); // failsafe: any error causes stop - - // PWM spindle control - pwm_set_duty(PWM_1, cm_get_spindle_pwm(spindle_mode) ); -} - /// Queue the S parameter to the planner buffer stat_t cm_set_spindle_speed(float speed) { float value[AXES] = { speed, 0,0,0,0,0 }; @@ -121,14 +155,6 @@ void cm_exec_spindle_speed(float speed) { } -/// Spindle speed callback from planner queue -static void _exec_spindle_speed(float *value, float *flag) { - cm_set_spindle_speed_parameter(MODEL, value[0]); - // update spindle speed if we're running - pwm_set_duty(PWM_1, cm_get_spindle_pwm(cm.gm.spindle_mode)); -} - - float get_max_spin(int index) { return 0; } diff --git a/src/spindle.h b/src/spindle.h index 188b34c..22bdc49 100644 --- a/src/spindle.h +++ b/src/spindle.h @@ -35,6 +35,6 @@ void cm_spindle_init(); stat_t cm_set_spindle_speed(float speed); // S parameter void cm_exec_spindle_speed(float speed); // callback for above - +float cm_get_spindle_pwm(uint8_t spindle_mode); stat_t cm_spindle_control(uint8_t spindle_mode); // M3, M4, M5 void cm_exec_spindle_control(uint8_t spindle_mode); // callback for above diff --git a/src/stepper.c b/src/stepper.c index c3321a5..91f3bb3 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -99,23 +99,16 @@ ISR(STEP_TIMER_ISR) { // End last move st.busy = false; + TIMER_STEP.PER = STEP_TIMER_POLL; // If there are no more moves try to load one if (!st.move_ready) { - TIMER_STEP.PER = STEP_TIMER_POLL; mp_exec_move(); return; } - // Power up motors if needed - for (int motor = 0; motor < MOTORS; motor++) - motor_begin_move(motor); - - // Wait until motors have energized - if (motor_energizing()) { - TIMER_STEP.PER = STEP_TIMER_POLL; - return; - } + // Wait until all motors have energized + if (motor_energizing()) return; // Start move if (st.seg_period) { @@ -124,13 +117,12 @@ ISR(STEP_TIMER_ISR) { TIMER_STEP.PER = st.seg_period; st.busy = true; - } - // Start dwell - st.dwell = st.prep_dwell; + // Start dwell + st.dwell = st.prep_dwell; - // Execute command - if (st.move_type == MOVE_TYPE_COMMAND) mp_runtime_command(st.bf); + } else if (st.move_type == MOVE_TYPE_COMMAND) + mp_runtime_command(st.bf); // Execute command // We are done with this move st.move_type = MOVE_TYPE_NULL; @@ -138,8 +130,9 @@ ISR(STEP_TIMER_ISR) { st.prep_dwell = 0; // clear dwell st.move_ready = false; // flip the flag back - // Request next move - _request_exec_move(); + // 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. + if (!st.dwell) _request_exec_move(); } diff --git a/src/switch.c b/src/switch.c index 9bf5dd0..72dad42 100644 --- a/src/switch.c +++ b/src/switch.c @@ -188,8 +188,8 @@ void switch_init() { if (s->mode == SW_MODE_DISABLED) continue; - port->DIRCLR = bm; // See 13.14.14 - port->INT0MASK |= bm; // Enable INT0 + port->DIRCLR = bm; // See 13.14.14 + port->INT0MASK |= bm; // Enable INT0 port->INTCTRL |= SWITCH_INTLVL; // Set interrupt level if (s->min) port->PIN6CTRL = PORT_OPC_PULLUP_gc | PORT_ISC_BOTHEDGES_gc; diff --git a/src/tmc2660.c b/src/tmc2660.c index 0623587..ae0ec00 100644 --- a/src/tmc2660.c +++ b/src/tmc2660.c @@ -44,24 +44,19 @@ void set_power_level(int driver, float value); typedef enum { + TMC2660_STATE_START, TMC2660_STATE_CONFIG, TMC2660_STATE_MONITOR, + TMC2660_STATE_WAIT, TMC2660_STATE_RECONFIGURE, } tmc2660_state_t; -typedef enum { - SPI_STATE_SELECT, - SPI_STATE_WRITE, - SPI_STATE_READ, - SPI_STATE_QUIT, -} spi_state_t; - typedef struct { tmc2660_state_t state; bool reconfigure; + bool configured; uint8_t reg; uint32_t stabilizing; - bool callback; uint16_t sguard; uint8_t flags; @@ -90,7 +85,7 @@ static tmc2660_driver_t drivers[MOTORS] = { typedef struct { volatile uint8_t driver; - volatile spi_state_t state; + volatile bool read; volatile uint8_t byte; volatile uint32_t out; volatile uint32_t in; @@ -121,12 +116,14 @@ static void _report_error_flags(int driver) { } -static void spi_cs(int driver, int enable) { +static void spi_cs(int driver, bool enable) { if (enable) drivers[driver].port->OUTCLR = CHIP_SELECT_BIT_bm; else drivers[driver].port->OUTSET = CHIP_SELECT_BIT_bm; } +static void spi_next(); + static void spi_send() { // Flush any status errors (TODO check for errors) @@ -139,102 +136,103 @@ static void spi_send() { // Write if (spi.byte < 3) SPIC.DATA = 0xff & (spi.out >> ((2 - spi.byte++) * 8)); - else spi.byte = 0; + else { + // SPI transfer complete + spi.byte = 0; + spi_next(); + } } -void spi_next() { +static void _driver_write(int driver) { tmc2660_driver_t *drv = &drivers[spi.driver]; - uint16_t spi_delay = 4; - switch (spi.state) { - case SPI_STATE_SELECT: - // Select driver - spi_cs(spi.driver, 1); + switch (drv->state) { + case TMC2660_STATE_START: return; // Should not get here + case TMC2660_STATE_WAIT: return; - // Next state - spi_delay = 4; - spi.state = SPI_STATE_WRITE; + case TMC2660_STATE_CONFIG: + spi.out = reg_addrs[drv->reg] | drv->regs[drv->reg]; break; - case SPI_STATE_WRITE: - switch (drv->state) { - case TMC2660_STATE_CONFIG: - spi.out = reg_addrs[drv->reg] | drv->regs[drv->reg]; - break; + case TMC2660_STATE_MONITOR: + spi.out = TMC2660_DRVCTRL_ADDR | drv->regs[TMC2660_DRVCTRL]; + break; - case TMC2660_STATE_MONITOR: - spi.out = TMC2660_DRVCTRL_ADDR | drv->regs[TMC2660_DRVCTRL]; - break; + case TMC2660_STATE_RECONFIGURE: + spi.out = TMC2660_CHOPCONF_ADDR | (drv->regs[TMC2660_CHOPCONF] & 0xffff0); + break; + } - case TMC2660_STATE_RECONFIGURE: - spi.out = TMC2660_CHOPCONF_ADDR | (drv->regs[TMC2660_CHOPCONF] & 0xffff0); - break; - } + spi_send(); // Start transfer +} - spi_send(); - // Next state - spi_delay = 8; - spi.state = SPI_STATE_READ; - break; +static void _driver_read(int driver) { + tmc2660_driver_t *drv = &drivers[spi.driver]; - case SPI_STATE_READ: - // Deselect driver - spi_cs(spi.driver, 0); - spi.state = SPI_STATE_SELECT; - - switch (drv->state) { - case TMC2660_STATE_CONFIG: - if (++drv->reg == 5) { - drv->state = TMC2660_STATE_MONITOR; - drv->reg = 0; - drv->stabilizing = rtc_get_time() + TMC2660_STABILIZE_TIME * 1000; - drv->callback = true; - drv->port->OUTCLR = MOTOR_ENABLE_BIT_bm; // Enable - } - break; + switch (drv->state) { + case TMC2660_STATE_START: + drv->state = TMC2660_STATE_CONFIG; + break; - case TMC2660_STATE_MONITOR: - // Read response (in bits [23, 4]) - drv->sguard = (uint16_t)((spi.in >> 14) & 0x3ff); - drv->flags = spi.in >> 4; + case TMC2660_STATE_CONFIG: + if (++drv->reg == 5) { + drv->reg = 0; + drv->stabilizing = rtc_get_time() + TMC2660_STABILIZE_TIME * 1000; + drv->port->OUTCLR = MOTOR_ENABLE_BIT_bm; // Enable + drv->state = TMC2660_STATE_MONITOR; + } + break; - if (spi.driver == 0) { - DACB.STATUS = DAC_CH0DRE_bm; - DACB.CH0DATA = drv->sguard << 2; - } + case TMC2660_STATE_MONITOR: + // Read response (in bits [23, 4]) + drv->sguard = (uint16_t)((spi.in >> 14) & 0x3ff); + drv->flags = spi.in >> 4; - if (drv->callback && drv->stabilizing < rtc_get_time()) { - motor_driver_callback(spi.driver); - drv->callback = false; - } + // Write driver 0 stallguard to DAC + if (spi.driver == 0) { + DACB.STATUS = DAC_CH0DRE_bm; + DACB.CH0DATA = drv->sguard << 2; + } - _report_error_flags(spi.driver); + if (!drv->configured && drv->stabilizing < rtc_get_time()) { + motor_driver_callback(spi.driver); + drv->configured = true; + } - if (drv->reconfigure) { - drv->state = TMC2660_STATE_RECONFIGURE; - drv->reconfigure = false; + _report_error_flags(spi.driver); - } else if (++spi.driver == MOTORS) { - spi.driver = 0; - spi_delay = F_CPU / 1024 * TMC2660_POLL_RATE; - break; - } - break; + if (drv->reconfigure) { + drv->state = TMC2660_STATE_RECONFIGURE; + drv->reconfigure = false; + drv->configured = false; - case TMC2660_STATE_RECONFIGURE: - drv->state = TMC2660_STATE_CONFIG; + } else if (++spi.driver == MOTORS) { + spi.driver = 0; + TMC2660_TIMER.CTRLA = TMC2660_TIMER_ENABLE; + drv->state = TMC2660_STATE_WAIT; break; } + break; - // Next state (delay set above) + case TMC2660_STATE_WAIT: + drv->state = TMC2660_STATE_MONITOR; break; - case SPI_STATE_QUIT: break; + case TMC2660_STATE_RECONFIGURE: + drv->state = TMC2660_STATE_CONFIG; + break; } +} + + +static void spi_next() { + spi_cs(spi.driver, false); // Deselect driver + _driver_read(spi.driver); // Read - TMC2660_TIMER.PER = spi_delay; + spi_cs(spi.driver, true); // Select driver + _driver_write(spi.driver); // Write } @@ -244,6 +242,7 @@ ISR(SPIC_INT_vect) { ISR(TCC1_OVF_vect) { + TMC2660_TIMER.CTRLA = 0; // Disable clock spi_next(); } @@ -263,7 +262,7 @@ ISR(PORT_4_FAULT_ISR_vect) {_fault_isr(3);} void tmc2660_init() { // Configure motors for (int i = 0; i < MOTORS; i++) { - drivers[i].state = TMC2660_STATE_CONFIG; + drivers[i].state = i ? TMC2660_STATE_CONFIG : TMC2660_STATE_START; drivers[i].reg = 0; uint32_t mstep = 0; @@ -325,16 +324,16 @@ void tmc2660_init() { // Configure SPI PR.PRPC &= ~PR_SPI_bm; // Disable power reduction - SPIC.CTRL = SPI_ENABLE_bm | SPI_MASTER_bm | SPI_MODE_3_gc | - SPI_PRESCALER_DIV128_gc; // enable, big endian, master, mode 3, clock/128 + SPIC.CTRL = SPI_ENABLE_bm | SPI_MASTER_bm | SPI_MODE_3_gc | SPI_CLK2X_bm | + SPI_PRESCALER_DIV16_gc; // enable, big endian, master, mode 3, clock/8 PORTC.REMAP = PORT_SPI_bm; // Swap SCK and MOSI SPIC.INTCTRL = SPI_INTLVL_LO_gc; // interupt level // Configure timer PR.PRPC &= ~PR_TC1_bm; // Disable power reduction - TMC2660_TIMER.PER = 1; // initial period not important + TMC2660_TIMER.PER = F_CPU / 64 * TMC2660_POLL_RATE; TMC2660_TIMER.INTCTRLA = TC_OVFINTLVL_LO_gc; // overflow interupt level - TMC2660_TIMER.CTRLA = TC_CLKSEL_DIV1024_gc; // enable, clock/1024 + TMC2660_TIMER.CTRLA = TMC2660_TIMER_ENABLE; // Configure DAC channel 0 for output DACB.CTRLB = DAC_CHSEL_SINGLE_gc; @@ -354,8 +353,7 @@ void tmc2660_reconfigure(int motor) { bool tmc2660_ready(int motor) { - return drivers[motor].state == TMC2660_STATE_MONITOR && - !drivers[motor].reconfigure; + return drivers[motor].configured && !drivers[motor].reconfigure; } diff --git a/src/usart.c b/src/usart.c index 1d42620..cff9bdc 100644 --- a/src/usart.c +++ b/src/usart.c @@ -44,7 +44,7 @@ #define RING_BUF_SIZE USART_RX_RING_BUF_SIZE #include "ringbuf.def" -int usart_flags = USART_CRLF | USART_ECHO; +static int usart_flags = USART_CRLF | USART_ECHO; static void set_dre_interrupt(int enable) { @@ -164,7 +164,7 @@ static void usart_sleep() { void usart_putc(char c) { - while (tx_buf_full() | (usart_flags & USART_FLUSH)) usart_sleep(); + while (tx_buf_full() || (usart_flags & USART_FLUSH)) usart_sleep(); tx_buf_push(c); diff --git a/src/usart.h b/src/usart.h index cd5640b..babe2d0 100644 --- a/src/usart.h +++ b/src/usart.h @@ -48,10 +48,10 @@ enum { }; enum { - USART_CRLF = (1 << 0), - USART_ECHO = (1 << 1), - USART_XOFF = (1 << 2), - USART_FLUSH = (1 << 3), + USART_CRLF = 1 << 0, + USART_ECHO = 1 << 1, + USART_XOFF = 1 << 2, + USART_FLUSH = 1 << 3, }; void usart_init(); -- 2.27.0