From: Joseph Coffland Date: Wed, 25 May 2016 06:02:50 +0000 (-0700) Subject: motor calibration, haunyang spindle control X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=faa6dc2f39200389c55e94425f2f0de30af2130d;p=bbctrl-firmware motor calibration, haunyang spindle control --- diff --git a/Makefile b/Makefile index a01e1a5..b6ec3f6 100644 --- a/Makefile +++ b/Makefile @@ -12,7 +12,8 @@ CPP = avr-g++ COMMON = -mmcu=$(MCU) CFLAGS += $(COMMON) -CFLAGS += -gdwarf-2 -std=gnu99 -Wall -Werror -DF_CPU=$(CLOCK)UL -O3 +CFLAGS += -Wall -Werror # -Wno-error=unused-function +CFLAGS += -gdwarf-2 -std=gnu99 -DF_CPU=$(CLOCK)UL -O3 CFLAGS += -funsigned-bitfields -fpack-struct -fshort-enums -funsigned-char CFLAGS += -MD -MP -MT $@ -MF build/dep/$(@F).d CFLAGS += -Isrc @@ -27,14 +28,15 @@ EEFLAGS += --set-section-flags=.eeprom="alloc,load" EEFLAGS += --change-section-lma .eeprom=0 --no-change-warnings # Programming flags -PROGRAMMER = avrispmkII +#PROGRAMMER = avrispmkII +PROGRAMMER = jtag3pdi PDEV = usb AVRDUDE_OPTS = -c $(PROGRAMMER) -p $(MCU) -P $(PDEV) FUSE0=0xff FUSE1=0x00 FUSE2=0xfe -FUSE4=0xfe +FUSE4=0xff FUSE5=0xeb # SRC @@ -100,7 +102,7 @@ usersig: # Clean tidy: - rm -f $(shell find -name *~ -o -name \#*) + rm -f $(shell find -name \*~ -o -name \#\*) clean: tidy rm -rf $(PROJECT).elf $(PROJECT).hex $(PROJECT).eep $(PROJECT).lss \ diff --git a/src/config.h b/src/config.h index 3140681..d39ba99 100644 --- a/src/config.h +++ b/src/config.h @@ -212,6 +212,7 @@ typedef enum { // Spindle settings +#define SPINDLE_TYPE SPINDLE_TYPE_HUANYANG #define SPINDLE_PWM_FREQUENCY 100 // in Hz #define SPINDLE_CW_SPEED_LO 1000 // in RPM (arbitrary units) #define SPINDLE_CW_SPEED_HI 2000 @@ -272,8 +273,8 @@ typedef enum { #define SPINDLE_BIT 0 // spindle on/off #define SPINDLE_DIR 1 // spindle direction, 1=CW, 0=CCW #define SPINDLE_PWM 2 // spindle PWMs output bit -#define MIST_COOLANT_BIT 3 // coolant on/off (same as flood) -#define FLOOD_COOLANT_BIT 3 // coolant on/off (same as mist) +#define MIST_COOLANT_BIT 2 // coolant on/off +#define FLOOD_COOLANT_BIT 1 // coolant on/off /* Interrupt usage: * @@ -365,6 +366,11 @@ typedef enum { #define PWM2_ISR_vect TCE1_CCB_vect +// Huanyang settings +#define HUANYANG_TIMEOUT 50 // ms. response timeout +#define HUANYANG_RETRIES 4 // Number of retries before failure +#define HUANYANG_ID 1 // Default ID + // Input #define INPUT_BUFFER_LEN 255 // text buffer size (255 max) diff --git a/src/gcode_parser.c b/src/gcode_parser.c index e5d7750..ea69740 100644 --- a/src/gcode_parser.c +++ b/src/gcode_parser.c @@ -480,7 +480,7 @@ static stat_t _parse_gcode_block(char *buf) { case 6: SET_NON_MODAL(tool_change, true); case 7: SET_MODAL(MODAL_GROUP_M8, mist_coolant, true); case 8: SET_MODAL(MODAL_GROUP_M8, flood_coolant, true); - case 9: SET_MODAL(MODAL_GROUP_M8, flood_coolant, false); + case 9: SET_MODAL(MODAL_GROUP_M8, flood_coolant, false); // Also mist case 48: SET_MODAL(MODAL_GROUP_M9, override_enables, true); case 49: SET_MODAL(MODAL_GROUP_M9, override_enables, false); case 50: SET_MODAL(MODAL_GROUP_M9, feed_rate_override_enable, true); diff --git a/src/huanyang.c b/src/huanyang.c new file mode 100644 index 0000000..0523185 --- /dev/null +++ b/src/huanyang.c @@ -0,0 +1,595 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + 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 "huanyang.h" +#include "config.h" +#include "rtc.h" + +#include +#include +#include +#include + +#include +#include +#include + + +enum { + HUANYANG_FUNC_READ = 1, + HUANYANG_FUNC_WRITE, + HUANYANG_CTRL_WRITE, + HUANYANG_CTRL_READ, + HUANYANG_FREQ_WRITE, + HUANYANG_RESERVED_1, + HUANYANG_RESERVED_2, + HUANYANG_LOOP_TEST +}; + + +enum { + HUANYANG_BASE_FREQ = 4, + HUANYANG_MAX_FREQ = 5, + HUANYANG_MIN_FREQ = 11, + HUANYANG_RATED_MOTOR_VOLTAGE = 141, + HUANYANG_RATED_MOTOR_CURRENT = 142, + HUANYANG_MOTOR_POLE = 143, + HUANYANG_RATED_RPM = 144, +}; + + +enum { + HUANYANG_TARGET_FREQ, + HUANYANG_ACTUAL_FREQ, + HUANYANG_ACTUAL_CURRENT, + HUANYANG_ACTUAL_RPM, + HUANYANG_DC_VOLTAGE, + HUANYANG_AC_VOLTAGE, + HUANYANG_CONT, + HUANYANG_TEMPERATURE, +}; + + +enum { + HUANYANG_FORWARD = 1, + HUANYANG_STOP = 8, + HUANYANG_REVERSE = 17, +}; + + +enum { + HUANYANG_RUN = 1 << 0, + HUANYANG_JOG = 1 << 1, + HUANYANG_COMMAND_RF = 1 << 2, + HUANYANG_RUNNING = 1 << 3, + HUANYANG_JOGGING = 1 << 4, + HUANYANG_RUNNING_RF = 1 << 5, + HUANYANG_BRACKING = 1 << 6, + HUANYANG_TRACK_START = 1 << 7, +}; + + +typedef bool (*next_command_cb_t)(int index); + + +typedef struct { + uint8_t id; + bool debug; + + next_command_cb_t next_command_cb; + uint8_t command_index; + uint8_t current_offset; + uint8_t command[8]; + uint8_t command_length; + uint8_t response_length; + uint8_t response[10]; + uint32_t last; + uint8_t retry; + + bool connected; + bool changed; + cmSpindleMode_t mode; + float speed; + + float actual_freq; + float actual_current; + uint16_t actual_rpm; + uint16_t dc_voltage; + uint16_t ac_voltage; + uint16_t temperature; + + float max_freq; + float min_freq; + uint16_t rated_rpm; + + uint8_t status; +} huanyang_t; + + +static huanyang_t ha = {}; + + +#define CTRL_STATUS_RESPONSE(R) ((uint16_t)R[4] << 8 | R[5]) +#define FUNC_RESPONSE(R) (R[2] == 2 ? R[4] : ((uint16_t)R[4] << 8 | R[5])) + + +static uint16_t _crc16(const uint8_t *buffer, unsigned length) { + uint16_t crc = 0xffff; + + for (unsigned i = 0; i < length; i++) + crc = _crc16_update(crc, buffer[i]); + + return crc; +} + + +static void _set_baud(uint16_t bsel, uint8_t bscale) { + USARTD1.BAUDCTRLB = (uint8_t)((bscale << 4) | (bsel >> 8)); + USARTD1.BAUDCTRLA = bsel; +} + + +static void _set_write(bool x) { + if (x) { + PORTB.OUTSET = 1 << 4; // High + PORTB.OUTSET = 1 << 5; // High + + } else { + PORTB.OUTCLR = 1 << 4; // Low + PORTB.OUTCLR = 1 << 5; // Low + } +} + + +static void _set_dre_interrupt(bool enable) { + if (enable) USARTD1.CTRLA |= USART_DREINTLVL_MED_gc; + else USARTD1.CTRLA &= ~USART_DREINTLVL_MED_gc; +} + + +static void _set_txc_interrupt(bool enable) { + if (enable) USARTD1.CTRLA |= USART_TXCINTLVL_MED_gc; + else USARTD1.CTRLA &= ~USART_TXCINTLVL_MED_gc; +} + + +static void _set_rxc_interrupt(bool enable) { + if (enable) USARTD1.CTRLA |= USART_RXCINTLVL_MED_gc; + else USARTD1.CTRLA &= ~USART_RXCINTLVL_MED_gc; +} + + +static void _set_command1(int code, uint8_t arg0) { + ha.command_length = 4; + ha.command[1] = code; + ha.command[2] = 1; + ha.command[3] = arg0; +} + + +static void _set_command2(int code, uint8_t arg0, uint8_t arg1) { + ha.command_length = 5; + ha.command[1] = code; + ha.command[2] = 2; + ha.command[3] = arg0; + ha.command[4] = arg1; +} + + +static void _set_command3(int code, uint8_t arg0, uint8_t arg1, uint8_t arg2) { + ha.command_length = 6; + ha.command[1] = code; + ha.command[2] = 3; + ha.command[3] = arg0; + ha.command[4] = arg1; + ha.command[5] = arg2; +} + + +static int _response_length(int code) { + switch (code) { + case HUANYANG_FUNC_READ: return 8; + case HUANYANG_FUNC_WRITE: return 8; + case HUANYANG_CTRL_WRITE: return 6; + case HUANYANG_CTRL_READ: return 8; + case HUANYANG_FREQ_WRITE: return 7; + default: return -1; + } +} + + +static bool _update(int index) { + // Read response + switch (index) { + case 1: ha.status = ha.response[5]; break; + case 2: ha.max_freq = FUNC_RESPONSE(ha.response) * 0.01; break; + case 3: ha.min_freq = FUNC_RESPONSE(ha.response) * 0.01; break; + case 4: ha.rated_rpm = FUNC_RESPONSE(ha.response); break; + default: break; + } + + // Setup next command + uint8_t var; + switch (index) { + case 0: { // Update mode + uint8_t state; + switch (ha.mode) { + case SPINDLE_CW: state = HUANYANG_FORWARD; break; + case SPINDLE_CCW: state = HUANYANG_REVERSE; break; + default: state = HUANYANG_STOP; break; + } + + _set_command1(HUANYANG_CTRL_WRITE, state); + + return true; + } + + case 1: var = HUANYANG_MAX_FREQ; break; + case 2: var = HUANYANG_MIN_FREQ; break; + case 3: var = HUANYANG_RATED_RPM; break; + + case 4: { // Update freqency + // Compute frequency in Hz + float freq = fabs(ha.speed * 50 / ha.rated_rpm); + + // Clamp frequency + if (ha.max_freq < freq) freq = ha.max_freq; + if (freq < ha.min_freq) freq = ha.min_freq; + + // Frequency write command + uint16_t f = freq * 100; + _set_command2(HUANYANG_FREQ_WRITE, f >> 8, f); + + return true; + } + + default: return false; + } + + _set_command3(HUANYANG_FUNC_READ, var, 0, 0); + + return true; +} + + +static bool _query_status(int index) { + // Read response + switch (index) { + case 1: ha.actual_freq = CTRL_STATUS_RESPONSE(ha.response) * 0.01; break; + case 2: ha.actual_current = CTRL_STATUS_RESPONSE(ha.response) * 0.1; break; + case 3: ha.actual_rpm = CTRL_STATUS_RESPONSE(ha.response); break; + case 4: ha.dc_voltage = CTRL_STATUS_RESPONSE(ha.response); break; + case 5: ha.ac_voltage = CTRL_STATUS_RESPONSE(ha.response); break; + case 6: ha.temperature = CTRL_STATUS_RESPONSE(ha.response); break; + default: break; + } + + // Setup next command + uint8_t var; + switch (index) { + case 0: var = HUANYANG_ACTUAL_FREQ; break; + case 1: var = HUANYANG_ACTUAL_CURRENT; break; + case 2: var = HUANYANG_ACTUAL_RPM; break; + case 3: var = HUANYANG_DC_VOLTAGE; break; + case 4: var = HUANYANG_AC_VOLTAGE; break; + case 5: var = HUANYANG_TEMPERATURE; break; + default: return false; + } + + _set_command1(HUANYANG_CTRL_READ, var); + + return true; +} + + +static void _next_command(); + + +static void _next_state() { + if (ha.changed) { + ha.next_command_cb = _update; + ha.changed = false; + + } else ha.next_command_cb = _query_status; + + _next_command(); +} + + +static bool _check_response() { + // Check CRC + uint16_t computed = _crc16(ha.response, ha.response_length - 2); + uint16_t expected = (ha.response[ha.response_length - 1] << 8) | + ha.response[ha.response_length - 2]; + + if (computed != expected) { + printf(PSTR("huanyang: invalid CRC, expected=0x%04u got=0x%04u\n"), + expected, computed); + return false; + } + + // Check return function code matches sent + if (ha.command[1] != ha.response[1]) { + printf_P(PSTR("huanyang: invalid function code, expected=%u got=%u\n"), + ha.command[2], ha.response[2]); + return false; + } + + return true; +} + + +static void _next_command() { + if (ha.next_command_cb && ha.next_command_cb(ha.command_index++)) { + ha.response_length = _response_length(ha.command[1]); + + ha.command[0] = ha.id; + + uint16_t crc = _crc16(ha.command, ha.command_length); + ha.command[ha.command_length++] = crc; + ha.command[ha.command_length++] = crc >> 8; + + _set_dre_interrupt(true); + + return; + } + + ha.command_index = 0; + _next_state(); +} + + +static void _retry_command() { + ha.last = 0; + ha.current_offset = 0; + ha.retry++; + + _set_write(true); // RS485 write mode + + _set_txc_interrupt(false); + _set_rxc_interrupt(false); + _set_dre_interrupt(true); + + if (ha.debug) printf_P(PSTR("huanyang: retry %d\n"), ha.retry); +} + + +// Data register empty interrupt +ISR(USARTD1_DRE_vect) { + USARTD1.DATA = ha.command[ha.current_offset++]; + + if (ha.current_offset == ha.command_length) { + _set_dre_interrupt(false); + _set_txc_interrupt(true); + ha.current_offset = 0; + } +} + + +/// Transmit complete interrupt +ISR(USARTD1_TXC_vect) { + _set_txc_interrupt(false); + _set_rxc_interrupt(true); + _set_write(false); // RS485 read mode + ha.last = rtc_get_time(); // Set timeout timer +} + + +// Data received interrupt +ISR(USARTD1_RXC_vect) { + ha.response[ha.current_offset++] = USARTD1.DATA; + + if (ha.current_offset == ha.response_length) { + _set_rxc_interrupt(false); + ha.current_offset = 0; + _set_write(true); // RS485 write mode + + if (_check_response()) { + ha.last = 0; // Clear timeout timer + ha.retry = 0; // Reset retry counter + ha.connected = true; + + _next_command(); + } + } +} + + +void huanyang_init() { + PR.PRPD &= ~PR_USART1_bm; // Disable power reduction + + // rs485_ro + PORTD.DIRCLR = 1 << 6; // Input + + // rs485_di + PORTD.OUTSET = 1 << 7; // High + PORTD.DIRSET = 1 << 7; // Output + + // rs485_re + PORTB.OUTSET = 1 << 4; // High + PORTB.DIRSET = 1 << 4; // Output + + // rs485_de + PORTB.OUTSET = 1 << 5; // High + PORTB.DIRSET = 1 << 5; // Output + + _set_baud(3325, 0b1101); // 9600 @ 32MHz with 2x USART + + // No parity, 8 data bits, 1 stop bit + USARTD1.CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | + USART_CHSIZE_8BIT_gc; + + // Configure receiver and transmitter + USARTD1.CTRLB = USART_RXEN_bm | USART_TXEN_bm | USART_CLK2X_bm; + + ha.id = HUANYANG_ID; + huanyang_reset(); +} + + +void huanyang_set(cmSpindleMode_t mode, float speed) { + if (ha.mode != mode || ha.speed != speed) { + if (ha.debug) + printf_P(PSTR("huanyang: mode=%d, speed=%0.2f\n"), mode, speed); + + ha.mode = mode; + ha.speed = speed; + ha.changed = true; + } +} + + +void huanyang_reset() { + _set_dre_interrupt(false); + _set_txc_interrupt(false); + _set_rxc_interrupt(false); + _set_write(true); // RS485 write mode + + // Flush USART + uint8_t x = USARTD1.DATA; + x = USARTD1.STATUS; + x = x; + + // Save settings + uint8_t id = ha.id; + cmSpindleMode_t mode = ha.mode; + float speed = ha.speed; + bool debug = ha.debug; + + // Clear state + memset(&ha, 0, sizeof(ha)); + + // Restore settings + ha.id = id; + ha.mode = mode; + ha.speed = speed; + ha.debug = debug; + ha.changed = true; + + _next_state(); +} + + +void huanyang_rtc_callback() { + if (ha.last && HUANYANG_TIMEOUT < rtc_get_time() - ha.last) { + if (ha.retry < HUANYANG_RETRIES) _retry_command(); + else { + if (ha.debug) printf_P(PSTR("huanyang: timedout\n")); + + if (ha.debug && ha.current_offset) { + printf_P(PSTR("huanyang: sent 0x")); + + for (uint8_t i = 0; i < ha.command_length; i++) + printf("%02x", ha.command[i]); + + printf_P(PSTR(" received 0x")); + for (uint8_t i = 0; i < ha.current_offset; i++) + printf("%02x", ha.response[i]); + + printf_P(PSTR(" expected %u bytes"), ha.response_length); + + putchar('\n'); + } + + huanyang_reset(); + } + } +} + + + +uint8_t get_huanyang_id(int index) { + return ha.id; +} + +void set_huanyang_id(int index, uint8_t value) { + ha.id = value; +} + + +bool get_huanyang_debug(int index) { + return ha.debug; +} + +void set_huanyang_debug(int index, uint8_t value) { + ha.debug = value; +} + + +bool get_huanyang_connected(int index) { + return ha.connected; +} + + +float get_huanyang_freq(int index) { + return ha.actual_freq; +} + + +float get_huanyang_current(int index) { + return ha.actual_current; +} + + +float get_huanyang_rpm(int index) { + return ha.actual_rpm; +} + + +float get_huanyang_dcv(int index) { + return ha.dc_voltage; +} + + +float get_huanyang_acv(int index) { + return ha.ac_voltage; +} + + +float get_huanyang_temp(int index) { + return ha.temperature; +} + + +float get_huanyang_max_freq(int index) { + return ha.max_freq; +} + + +float get_huanyang_min_freq(int index) { + return ha.min_freq; +} + + +uint16_t get_huanyang_rated_rpm(int index) { + return ha.rated_rpm; +} + + +float get_huanyang_status(int index) { + return ha.status; +} diff --git a/src/huanyang.h b/src/huanyang.h new file mode 100644 index 0000000..60d5bbf --- /dev/null +++ b/src/huanyang.h @@ -0,0 +1,36 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + 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 "canonical_machine.h" + + +void huanyang_init(); +void huanyang_set(cmSpindleMode_t mode, float speed); +void huanyang_reset(); +void huanyang_rtc_callback(); diff --git a/src/motor.c b/src/motor.c index 9ce699f..9a61855 100644 --- a/src/motor.c +++ b/src/motor.c @@ -71,6 +71,7 @@ typedef struct { uint32_t timeout; cmMotorFlags_t flags; int32_t encoder; + uint16_t steps; uint8_t last_clock; // Move prep @@ -114,7 +115,7 @@ static motor_t motors[MOTORS] = { .travel_rev = M3_TRAVEL_PER_REV, .microsteps = M3_MICROSTEPS, .polarity = M3_POLARITY, - .power_mode = M3_POWER_MODE, + .power_mode = M3_POWER_MODE, .port = &PORT_MOTOR_3, .timer = &M3_TIMER, .dma = &M3_DMA_CH, @@ -140,35 +141,10 @@ static uint8_t _dummy; /// Special interrupt for X-axis ISR(TCE1_CCA_vect) { PORT_MOTOR_1.OUTTGL = STEP_BIT_bm; + motors[0].steps++; } -#if 0 -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 -} -#endif - - void motor_init() { // Reset position mp_set_steps_to_runtime_position(); @@ -176,6 +152,7 @@ void motor_init() { // Enable DMA DMA.CTRL = DMA_RESET_bm; DMA.CTRL = DMA_ENABLE_bm; + DMA.INTFLAGS = 0xff; // clear all interrups for (int motor = 0; motor < MOTORS; motor++) { motor_t *m = &motors[motor]; @@ -183,22 +160,26 @@ void motor_init() { // Setup motor timer m->timer->CTRLB = TC_WGMODE_FRQ_gc | TC1_CCAEN_bm; + if (!motor) continue; // Don't configure DMA for motor 0 + // Setup DMA channel as timer event counter m->dma->ADDRCTRL = DMA_CH_SRCDIR_FIXED_gc | DMA_CH_DESTDIR_FIXED_gc; m->dma->TRIGSRC = m->dma_trigger; m->dma->REPCNT = 0; - m->dma->SRCADDR0 = (((uintptr_t)&_dummy) >> 0) & 0xff; - m->dma->SRCADDR1 = (((uintptr_t)&_dummy) >> 8) & 0xff; + // Note, the DMA transfer must read CCA to clear the trigger + m->dma->SRCADDR0 = (((uintptr_t)&m->timer->CCA) >> 0) & 0xff; + m->dma->SRCADDR1 = (((uintptr_t)&m->timer->CCA) >> 8) & 0xff; m->dma->SRCADDR2 = 0; m->dma->DESTADDR0 = (((uintptr_t)&_dummy) >> 0) & 0xff; m->dma->DESTADDR1 = (((uintptr_t)&_dummy) >> 8) & 0xff; m->dma->DESTADDR2 = 0; - m->dma->CTRLB = DMA_CH_TRNINTLVL_HI_gc; + m->dma->CTRLB = 0; m->dma->CTRLA = - DMA_CH_ENABLE_bm | DMA_CH_SINGLE_bm | DMA_CH_BURSTLEN_1BYTE_gc; + DMA_CH_REPEAT_bm | DMA_CH_SINGLE_bm | DMA_CH_BURSTLEN_1BYTE_gc; + m->dma->CTRLA |= DMA_CH_ENABLE_bm; } // Setup special interrupt for X-axis mapping @@ -240,7 +221,7 @@ bool motor_error(int motor) { } -bool motor_stall(int motor) { +bool motor_stalled(int motor) { return motors[motor].flags & MOTOR_FLAG_STALLED_bm; } @@ -396,6 +377,7 @@ void motor_load_move(int motor) { // Get actual step count from DMA channel uint16_t steps = 0xffff - m->dma->TRFCNT; m->dma->TRFCNT = 0xffff; + m->dma->CTRLB = DMA_CH_CHBUSY_bm | DMA_CH_CHPEND_bm; m->dma->CTRLA |= DMA_CH_ENABLE_bm; // Adjust clock count @@ -424,7 +406,11 @@ void motor_load_move(int motor) { else m->port->OUTSET = DIRECTION_BIT_bm; // Accumulate encoder - m->encoder += m->positive ? steps : -steps; + if (!motor) { + steps = m->steps; + m->steps = 0; + } + m->encoder += m->positive ? steps : -(int32_t)steps; } diff --git a/src/motor.h b/src/motor.h index 81cef29..348a871 100644 --- a/src/motor.h +++ b/src/motor.h @@ -70,7 +70,7 @@ float motor_get_steps_per_unit(int motor); int32_t motor_get_encoder(int motor); void motor_set_encoder(int motor, float encoder); bool motor_error(int motor); -bool motor_stall(int motor); +bool motor_stalled(int motor); void motor_reset(int motor); bool motor_energizing(); diff --git a/src/plan/calibrate.c b/src/plan/calibrate.c index 729b967..e128839 100644 --- a/src/plan/calibrate.c +++ b/src/plan/calibrate.c @@ -40,12 +40,13 @@ #include #include #include +#include -#define CAL_THRESHOLDS 32 #define CAL_VELOCITIES 256 -#define CAL_MIN_THRESH -10 -#define CAL_MAX_THRESH 63 +#define CAL_MIN_VELOCITY 1000 // mm/sec +#define CAL_TARGET_SG 100 +#define CAL_MAX_DELTA_SG 75 #define CAL_WAIT_TIME 3 // ms @@ -59,17 +60,17 @@ enum { typedef struct { bool busy; + bool stall_valid; + bool stalled; + bool reverse; uint32_t wait; int state; int motor; int axis; - int vstep; - float current_velocity; - - uint16_t stallguard[CAL_VELOCITIES]; - uint16_t velocities[CAL_VELOCITIES]; + float velocity; + uint16_t stallguard; } calibrate_t; static calibrate_t cal = {}; @@ -86,13 +87,10 @@ static stat_t _exec_calibrate(mpBuf_t *bf) { case CAL_START: { cal.axis = motor_get_axis(cal.motor); cal.state = CAL_ACCEL; - - cal.current_velocity = 0; - float max_velocity = cm.a[cal.axis].velocity_max / 2; - for (int i = 0; i < CAL_VELOCITIES; i++) - cal.velocities[i] = (1 + i) * max_velocity / CAL_VELOCITIES; - - memset(cal.stallguard, 0, sizeof(cal.stallguard)); + cal.velocity = 0; + cal.stall_valid = false; + cal.stalled = false; + cal.reverse = false; tmc2660_set_stallguard_threshold(cal.motor, 8); cal.wait = rtc_get_time() + CAL_WAIT_TIME; @@ -101,49 +99,39 @@ static stat_t _exec_calibrate(mpBuf_t *bf) { } case CAL_ACCEL: - if (cal.velocities[cal.vstep] == cal.current_velocity) - cal.state = CAL_MEASURE; - - else { - cal.current_velocity += maxDeltaV; - - if (cal.velocities[cal.vstep] <= cal.current_velocity) - cal.current_velocity = cal.velocities[cal.vstep]; - } - break; - - case CAL_MEASURE: - if (++cal.vstep == CAL_VELOCITIES) cal.state = CAL_DECEL; - else cal.state = CAL_ACCEL; - break; - - case CAL_DECEL: - cal.current_velocity -= maxDeltaV; - if (cal.current_velocity < 0) cal.current_velocity = 0; - - if (!cal.current_velocity) { - // Print results - putchar('\n'); - for (int i = 0; i < CAL_VELOCITIES; i++) - printf("%d,", cal.velocities[i]); - putchar('\n'); - for (int i = 0; i < CAL_VELOCITIES; i++) - printf("%d,", cal.stallguard[i]); - putchar('\n'); - - mp_free_run_buffer(); // Release buffer - cal.busy = false; - - return STAT_OK; + if (CAL_MIN_VELOCITY < cal.velocity) cal.stall_valid = true; + + if (cal.velocity < CAL_MIN_VELOCITY || CAL_TARGET_SG < cal.stallguard) + cal.velocity += maxDeltaV; + + if (cal.stalled) { + if (cal.reverse) { + int32_t steps = -motor_get_encoder(cal.motor); + float mm = (float)steps / motor_get_steps_per_unit(cal.motor); + printf("%"PRIi32" steps %0.2f mm\n", steps, mm); + + tmc2660_set_stallguard_threshold(cal.motor, 63); + mp_free_run_buffer(); // Release buffer + cal.busy = false; + return STAT_OK; + + } else { + motor_set_encoder(cal.motor, 0); + + cal.reverse = true; + cal.velocity = 0; + cal.stall_valid = false; + cal.stalled = false; + } } break; } - if (!cal.current_velocity) return STAT_OK; + if (!cal.velocity) return STAT_OK; // Compute travel float travel[AXES] = {}; // In mm - travel[cal.axis] = time * cal.current_velocity; + travel[cal.axis] = time * cal.velocity * (cal.reverse ? -1 : 1); // Convert to steps float steps[MOTORS] = {0}; @@ -161,8 +149,17 @@ bool calibrate_busy() {return cal.busy;} void calibrate_set_stallguard(int motor, uint16_t sg) { - if (cal.vstep < CAL_VELOCITIES && cal.motor == motor) - cal.stallguard[cal.vstep] = sg; + if (cal.motor != motor) return; + + if (cal.stall_valid) { + int16_t delta = sg - cal.stallguard; + if (!sg || CAL_MAX_DELTA_SG < abs(delta)) { + cal.stalled = true; + motor_end_move(cal.motor); + } + } + + cal.stallguard = sg; } @@ -178,6 +175,7 @@ uint8_t command_calibrate(int argc, char *argv[]) { // Start memset(&cal, 0, sizeof(cal)); cal.busy = true; + cal.motor = 1; bf->bf_func = _exec_calibrate; // register callback mp_commit_write_buffer(MOVE_TYPE_COMMAND); diff --git a/src/pwm_spindle.c b/src/pwm_spindle.c new file mode 100644 index 0000000..73284c0 --- /dev/null +++ b/src/pwm_spindle.c @@ -0,0 +1,181 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + 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 "pwm_spindle.h" + +#include "config.h" +#include "gpio.h" +#include "hardware.h" +#include "pwm.h" + + +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, +}; + + +/// return PWM phase (duty cycle) for dir and speed +static float _get_spindle_pwm(cmSpindleMode_t mode, float speed) { + float speed_lo; + float speed_hi; + float phase_lo; + float phase_hi; + + switch (mode) { + case SPINDLE_CW: + speed_lo = spindle.cw_speed_lo; + speed_hi = spindle.cw_speed_hi; + phase_lo = spindle.cw_phase_lo; + phase_hi = spindle.cw_phase_hi; + break; + + case SPINDLE_CCW: + speed_lo = spindle.ccw_speed_lo; + speed_hi = spindle.ccw_speed_hi; + phase_lo = spindle.ccw_phase_lo; + phase_hi = spindle.ccw_phase_hi; + break; + + default: return spindle.phase_off; + } + + // Clamp spindle speed to lo/hi range + if (speed < speed_lo) speed = speed_lo; + if (speed > speed_hi) speed = speed_hi; + + // Normalize speed to [0..1] + speed = (speed - speed_lo) / (speed_hi - speed_lo); + return speed * (phase_hi - phase_lo) + phase_lo; +} + + +void pwm_spindle_init() { + if (spindle.frequency < 0) spindle.frequency = 0; + + pwm_set_freq(PWM_1, spindle.frequency); + pwm_set_duty(PWM_1, spindle.phase_off); +} + + +void pwm_spindle_set(cmSpindleMode_t mode, float speed) { + switch (mode) { + case SPINDLE_CW: + gpio_set_bit_on(SPINDLE_BIT); + gpio_set_bit_off(SPINDLE_DIR); + break; + + case SPINDLE_CCW: + gpio_set_bit_on(SPINDLE_BIT); + gpio_set_bit_on(SPINDLE_DIR); + break; + + default: + gpio_set_bit_off(SPINDLE_BIT); // failsafe: any error causes stop + } + + // PWM spindle control + pwm_set_duty(PWM_1, _get_spindle_pwm(mode, speed)); +} + + +// TODO implement these +float get_max_spin(int index) { + return 0; +} + + +void set_max_spin(int axis, float value) { +} + + +float get_spin_min_pulse(int index) { + return 0; +} + + +void set_spin_min_pulse(int axis, float value) { +} + + +float get_spin_max_pulse(int index) { + return 0; +} + + +void set_spin_max_pulse(int axis, float value) { +} + + +uint8_t get_spin_polarity(int index) { + return 0; +} + + +void set_spin_polarity(int axis, uint8_t value) { +} + + +float get_spin_up(int index) { + return 0; +} + + +void set_spin_up(int axis, float value) { +} + + +float get_spin_down(int index) { + return 0; +} + + +void set_spin_down(int axis, float value) { +} diff --git a/src/pwm_spindle.h b/src/pwm_spindle.h new file mode 100644 index 0000000..48e7a6d --- /dev/null +++ b/src/pwm_spindle.h @@ -0,0 +1,34 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + 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 "canonical_machine.h" + + +void pwm_spindle_init(); +void pwm_spindle_set(cmSpindleMode_t mode, float speed); diff --git a/src/rtc.c b/src/rtc.c index 8fa9b9a..d856e52 100644 --- a/src/rtc.c +++ b/src/rtc.c @@ -29,6 +29,7 @@ #include "rtc.h" #include "switch.h" +#include "huanyang.h" #include #include @@ -42,6 +43,7 @@ static uint32_t ticks; ISR(RTC_OVF_vect) { ticks++; switch_rtc_callback(); // switch debouncing + huanyang_rtc_callback(); } diff --git a/src/spindle.c b/src/spindle.c index 7951bb2..7d1dcfd 100644 --- a/src/spindle.c +++ b/src/spindle.c @@ -29,112 +29,53 @@ #include "spindle.h" #include "config.h" -#include "gpio.h" -#include "hardware.h" -#include "pwm.h" +#include "pwm_spindle.h" +#include "huanyang.h" #include "plan/command.h" -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, -}; +typedef enum { + SPINDLE_TYPE_PWM, + SPINDLE_TYPE_HUANYANG, +} spindleType_t; +static spindleType_t spindle_type = SPINDLE_TYPE; -/// execute the spindle command (called from planner) -static void _exec_spindle_control(float *value, float *flag) { - uint8_t spindle_mode = value[0]; - - cm_set_spindle_mode(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); +static void _spindle_set(cmSpindleMode_t mode, float speed) { + switch (spindle_type) { + case SPINDLE_TYPE_PWM: pwm_spindle_set(mode, speed); break; + case SPINDLE_TYPE_HUANYANG: huanyang_set(mode, speed); break; + } +} - } 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)); +/// execute the spindle command (called from planner) +static void _exec_spindle_control(float *value, float *flag) { + cmSpindleMode_t mode = value[0]; + cm_set_spindle_mode(mode); + _spindle_set(mode, cm.gm.spindle_speed); } /// Spindle speed callback from planner queue static void _exec_spindle_speed(float *value, float *flag) { - cm_set_spindle_speed_parameter(value[0]); - // update spindle speed if we're running - pwm_set_duty(PWM_1, cm_get_spindle_pwm(cm.gm.spindle_mode)); + float speed = value[0]; + cm_set_spindle_speed_parameter(speed); + _spindle_set(cm.gm.spindle_mode, speed); } void cm_spindle_init() { - if( spindle.frequency < 0 ) - spindle.frequency = 0; - - 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(cmSpindleMode_t spindle_mode) { - float speed_lo = 0, speed_hi = 0, phase_lo = 0, phase_hi = 0; - - if (spindle_mode == SPINDLE_CW) { - 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 = 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) { - // clamp spindle speed to lo/hi range - if (cm.gm.spindle_speed < speed_lo) cm.gm.spindle_speed = speed_lo; - if (cm.gm.spindle_speed > speed_hi) cm.gm.spindle_speed = speed_hi; - - // normalize speed to [0..1] - float speed = (cm.gm.spindle_speed - speed_lo) / (speed_hi - speed_lo); - return speed * (phase_hi - phase_lo) + phase_lo; - - } else return spindle.phase_off; + pwm_spindle_init(); + huanyang_init(); } -/// queue the spindle command to the planner buffer -void cm_spindle_control(cmSpindleMode_t spindle_mode) { - float value[AXES] = {spindle_mode}; +/// Queue the spindle command to the planner buffer +void cm_spindle_control(cmSpindleMode_t mode) { + float value[AXES] = {mode}; mp_queue_command(_exec_spindle_control, value, value); } @@ -146,65 +87,15 @@ void cm_set_spindle_speed(float speed) { } -// TODO implement these -float get_max_spin(int index) { - return 0; -} - - -void set_max_spin(int axis, float value) { -} - - uint8_t get_spindle_type(int index) { - return 0; + return spindle_type; } -void set_spindle_type(int axis, uint8_t value) { -} - - -float get_spin_min_pulse(int index) { - return 0; -} - - -void set_spin_min_pulse(int axis, float value) { -} - - -float get_spin_max_pulse(int index) { - return 0; -} - - -void set_spin_max_pulse(int axis, float value) { -} - - -uint8_t get_spin_polarity(int index) { - return 0; -} - - -void set_spin_polarity(int axis, uint8_t value) { -} - - -float get_spin_up(int index) { - return 0; -} - - -void set_spin_up(int axis, float value) { -} - - -float get_spin_down(int index) { - return 0; -} - - -void set_spin_down(int axis, float value) { +void set_spindle_type(int index, uint8_t value) { + if (value != spindle_type) { + _spindle_set(SPINDLE_OFF, 0); + spindle_type = value; + _spindle_set(cm.gm.spindle_mode, cm.gm.spindle_speed); + } } diff --git a/src/spindle.h b/src/spindle.h index 07a7bc5..8bb6fc0 100644 --- a/src/spindle.h +++ b/src/spindle.h @@ -33,5 +33,4 @@ void cm_spindle_init(); void cm_set_spindle_speed(float speed); // S parameter -float cm_get_spindle_pwm(cmSpindleMode_t spindle_mode); void cm_spindle_control(cmSpindleMode_t spindle_mode); // M3, M4, M5 diff --git a/src/stepper.c b/src/stepper.c index b935bdb..44a8643 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -107,6 +107,7 @@ ISR(STEP_TIMER_ISR) { st.busy = false; TIMER_STEP.PER = STEP_TIMER_POLL; + DMA.INTFLAGS = 0xff; // clear all interrups for (int motor = 0; motor < MOTORS; motor++) motor_end_move(motor); diff --git a/src/switch.c b/src/switch.c index 72dad42..2ca5fcd 100644 --- a/src/switch.c +++ b/src/switch.c @@ -181,6 +181,8 @@ ISR(A_SWITCH_ISR_vect) {_switch_isr();} void switch_init() { + return; // TODO + for (int i = 0; i < SWITCHES; i++) { switch_t *s = &sw.switches[i]; PORT_t *port = s->port; diff --git a/src/tmc2660.c b/src/tmc2660.c index 729158c..97fc919 100644 --- a/src/tmc2660.c +++ b/src/tmc2660.c @@ -173,8 +173,8 @@ static bool _driver_read(int driver) { calibrate_set_stallguard(driver, drv->sguard); // Write driver 0 stallguard to DAC - if (driver == 0 && (DACB.STATUS & DAC_CH0DRE_bm)) - DACB.CH0DATA = drv->sguard << 2; + //if (driver == 0 && (DACB.STATUS & DAC_CH0DRE_bm)) + // DACB.CH0DATA = drv->sguard << 2; _report_error_flags(driver); } @@ -327,9 +327,9 @@ void tmc2660_init() { TMC2660_TIMER.CTRLA = TMC2660_TIMER_ENABLE; // Configure DAC channel 0 for output - DACB.CTRLA = DAC_CH0EN_bm | DAC_ENABLE_bm; - DACB.CTRLB = DAC_CHSEL_SINGLE_gc; - DACB.CTRLC = DAC_REFSEL_AVCC_gc; + //DACB.CTRLA = DAC_CH0EN_bm | DAC_ENABLE_bm; + //DACB.CTRLB = DAC_CHSEL_SINGLE_gc; + //DACB.CTRLC = DAC_REFSEL_AVCC_gc; } diff --git a/src/usart.c b/src/usart.c index 966acfc..e57f62e 100644 --- a/src/usart.c +++ b/src/usart.c @@ -124,7 +124,7 @@ void usart_init(void) { } -static void set_baud(uint8_t bsel, uint8_t bscale) { +static void set_baud(uint16_t bsel, uint8_t bscale) { USARTC0.BAUDCTRLB = (uint8_t)((bscale << 4) | (bsel >> 8)); USARTC0.BAUDCTRLA = bsel; } @@ -132,19 +132,20 @@ static void set_baud(uint8_t bsel, uint8_t bscale) { void usart_set_baud(int baud) { // The BSEL / BSCALE values provided below assume a 32 Mhz clock - // Assumes CTRLB CLK2X bit (0x04) is not enabled + // Assumes CTRLB CLK2X bit (0x04) is set + // See http://www.avrcalc.elektronik-projekt.de/xmega/baud_rate_calculator switch (baud) { - case USART_BAUD_9600: set_baud(207, 0); break; - case USART_BAUD_19200: set_baud(103, 0); break; - case USART_BAUD_38400: set_baud(51, 0); break; - case USART_BAUD_57600: set_baud(34, 0); break; - case USART_BAUD_115200: set_baud(33, -1 << 4); break; - case USART_BAUD_230400: set_baud(31, -2 << 4); break; - case USART_BAUD_460800: set_baud(27, -3 << 4); break; - case USART_BAUD_921600: set_baud(19, -4 << 4); break; - case USART_BAUD_500000: set_baud(1, 1 << 4); break; - case USART_BAUD_1000000: set_baud(1, 0); break; + case USART_BAUD_9600: set_baud(3325, 0b1101); break; + case USART_BAUD_19200: set_baud(3317, 0b1100); break; + case USART_BAUD_38400: set_baud(3301, 0b1011); break; + case USART_BAUD_57600: set_baud(1095, 0b1100); break; + case USART_BAUD_115200: set_baud(1079, 0b1011); break; + case USART_BAUD_230400: set_baud(1047, 0b1010); break; + case USART_BAUD_460800: set_baud(983, 0b1001); break; + case USART_BAUD_921600: set_baud(107, 0b1011); break; + case USART_BAUD_500000: set_baud(1, 0b0010); break; + case USART_BAUD_1000000: set_baud(1, 0b0001); break; } } diff --git a/src/vars.def b/src/vars.def index e43971a..61750aa 100644 --- a/src/vars.def +++ b/src/vars.def @@ -68,13 +68,28 @@ VAR(zero_backoff, "zb", float, AXES, 1, 0, "Homing zero backof") // Spindle VAR(max_spin, "ss", float, 0, 1, 0, "Maximum spindle speed") -VAR(spindle_type, "st", uint8_t, 0, 1, 0, "RS485 or PWM") +VAR(spindle_type, "st", uint8_t, 0, 1, 0, "PWM=0 or HUANYANG=1") VAR(spin_min_pulse, "np", float, 0, 1, 0, "Minimum pulse width") VAR(spin_max_pulse, "mp", float, 0, 1, 0, "Maximum pulse width") VAR(spin_polarity, "sp", uint8_t, 0, 1, 0, "Normal or reversed") VAR(spin_up, "su", float, 0, 1, 0, "Spin up velocity") VAR(spin_down, "sd", float, 0, 1, 0, "Spin down velocity") +// Huanyang spindle +VAR(huanyang_id, "hi", uint8_t, 0, 1, 0, "Huanyang ID") +VAR(huanyang_freq, "hz", float, 0, 0, 0, "Huanyang actual freq") +VAR(huanyang_current, "hc", float, 0, 0, 0, "Huanyang actual current") +VAR(huanyang_rpm, "hr", uint16_t, 0, 0, 0, "Huanyang actual RPM") +VAR(huanyang_dcv, "hd", uint16_t, 0, 0, 0, "Huanyang DC voltage") +VAR(huanyang_acv, "ha", uint16_t, 0, 0, 0, "Huanyang AC voltage") +VAR(huanyang_temp, "ht", uint16_t, 0, 0, 0, "Huanyang temperature") +VAR(huanyang_max_freq, "hx", float, 0, 0, 0, "Huanyang max freq") +VAR(huanyang_min_freq, "hm", float, 0, 0, 0, "Huanyang min freq") +VAR(huanyang_rated_rpm, "hq", uint16_t, 0, 0, 0, "Huanyang rated RPM") +VAR(huanyang_status, "hs", uint8_t, 0, 0, 0, "Huanyang status flags") +VAR(huanyang_debug, "hb", bool, 0, 1, 0, "Huanyang debugging") +VAR(huanyang_connected, "he", bool, 0, 0, 0, "Huanyang connected") + // Switches VAR(switch_type, "sw", uint8_t, SWITCHES, 1, 0, "Normally open or closed")