From: Joseph Coffland Date: Tue, 28 Jun 2016 10:12:54 +0000 (-0700) Subject: Implemented better pin mapping X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=aafa07927c8e4425cc631fa7e83f517ce8987f72;p=bbctrl-firmware Implemented better pin mapping --- diff --git a/src/canonical_machine.c b/src/canonical_machine.c index b88db71..22bc0de 100644 --- a/src/canonical_machine.c +++ b/src/canonical_machine.c @@ -97,23 +97,12 @@ cmSingleton_t cm = { .offset = { {}, // ABSOLUTE_COORDS - {G54_X_OFFSET, G54_Y_OFFSET, G54_Z_OFFSET, - G54_A_OFFSET, G54_B_OFFSET, G54_C_OFFSET}, - - {G55_X_OFFSET, G55_Y_OFFSET, G55_Z_OFFSET, - G55_A_OFFSET, G55_B_OFFSET, G55_C_OFFSET}, - - {G56_X_OFFSET, G56_Y_OFFSET, G56_Z_OFFSET, - G56_A_OFFSET, G56_B_OFFSET, G56_C_OFFSET}, - - {G57_X_OFFSET, G57_Y_OFFSET, G57_Z_OFFSET, - G57_A_OFFSET, G57_B_OFFSET, G57_C_OFFSET}, - - {G58_X_OFFSET, G58_Y_OFFSET, G58_Z_OFFSET, - G58_A_OFFSET, G58_B_OFFSET, G58_C_OFFSET}, - - {G59_X_OFFSET, G59_Y_OFFSET, G59_Z_OFFSET, - G59_A_OFFSET, G59_B_OFFSET, G59_C_OFFSET}, + {0, 0, 0, 0, 0, 0}, // G54 + {X_TRAVEL_MAX / 2, Y_TRAVEL_MAX / 2, 0, 0, 0, 0}, // G55 + {0, 0, 0, 0, 0, 0}, // G56 + {0, 0, 0, 0, 0, 0}, // G57 + {0, 0, 0, 0, 0, 0}, // G58 + {0, 0, 0, 0, 0, 0}, // G59 }, // Axes diff --git a/src/config.h b/src/config.h index 53a24be..ca84819 100644 --- a/src/config.h +++ b/src/config.h @@ -27,10 +27,72 @@ #pragma once +#include "pins.h" + #include + #define VERSION "0.3.0" + +// Pins +enum { + STEP_X_PIN = PORT_A << 3, + DIR_X_PIN, + ENABLE_X_PIN, + SPI_CS_X_PIN, + FAULT_X_PIN, + FAULT_PIN, + MIN_X_PIN, + MAX_X_PIN, + + SPIN_PWM_PIN = PORT_B << 3, + SPIN_DIR_PIN, + MIN_Y_PIN, + MAX_Y_PIN, + RS485_RE_PIN, + RS485_DE_PIN, + SPIN_ENABLE_PIN, + BOOT_PIN, + + READY_PIN = PORT_C << 3, + PROBE_PIN, + SERIAL_RX_PIN, + SERIAL_TX_PIN, + SERIAL_CTS_PIN, + SPI_CLK_PIN, + SPI_MOSI_PIN, + SPI_MISO_PIN, + + STEP_A_PIN = PORT_D << 3, + DIR_A_PIN, + ENABLE_A_PIN, + SPI_CS_A_PIN, + FAULT_A_PIN, + ESTOP_PIN, + RS485_RO_PIN, + RS485_DI_PIN, + + STEP_Z_PIN = PORT_E << 3, + DIR_Z_PIN, + ENABLE_Z_PIN, + SPI_CS_Z_PIN, + FAULT_Z_PIN, + SWITCH_1_PIN, + MIN_Z_PIN, + MAX_Z_PIN, + + STEP_Y_PIN = PORT_F << 3, + DIR_Y_PIN, + ENABLE_Y_PIN, + SPI_CS_Y_PIN, + FAULT_Y_PIN, + SWITCH_2_PIN, + MIN_A_PIN, + MAX_A_PIN, +}; + + // Compile-time settings //#define __STEP_CORRECTION //#define __JERK_EXEC // Use computed jerk (vs. forward difference) @@ -200,13 +262,6 @@ typedef enum { #define SPINDLE_MAX_DUTY 0.99 #define SPINDLE_POLARITY 0 // 0 = normal, 1 = reverse -#define SPINDLE_PWM_PORT PORTD -#define SPINDLE_PWM_PIN_bm (1 << 5) -#define SPINDLE_DIR_PORT PORTB -#define SPINDLE_DIR_PIN_bm (1 << 1) -#define SPINDLE_ENABLE_PORT PORTB -#define SPINDLE_ENABLE_PIN_bm (1 << 6) - // Gcode defaults #define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES @@ -216,49 +271,12 @@ typedef enum { #define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE -// Motors mapped to ports -#define PORT_MOTOR_1 PORTA -#define PORT_MOTOR_2 PORTF -#define PORT_MOTOR_3 PORTE -#define PORT_MOTOR_4 PORTD - // Motor fault ISRs #define PORT_1_FAULT_ISR_vect PORTA_INT1_vect #define PORT_2_FAULT_ISR_vect PORTD_INT1_vect #define PORT_3_FAULT_ISR_vect PORTE_INT1_vect #define PORT_4_FAULT_ISR_vect PORTF_INT1_vect -// Switch axes mapped to ports -#define PORT_SWITCH_X PORT_MOTOR_1 -#define PORT_SWITCH_Y PORT_MOTOR_2 -#define PORT_SWITCH_Z PORT_MOTOR_3 -#define PORT_SWITCH_A PORT_MOTOR_4 - -// Axes mapped to output ports -#define PORT_OUT_X PORT_MOTOR_1 -#define PORT_OUT_Y PORT_MOTOR_2 -#define PORT_OUT_Z PORT_MOTOR_3 -#define PORT_OUT_A PORT_MOTOR_4 - -// Motor control port -#define STEP_BIT_bm (1 << 0) -#define DIRECTION_BIT_bm (1 << 1) -#define MOTOR_ENABLE_BIT_bm (1 << 2) -#define CHIP_SELECT_BIT_bm (1 << 3) -#define FAULT_BIT_bm (1 << 4) -#define GPIO1_OUT_BIT_bm (1 << 5) // spindle and coolant -#define SW_MIN_BIT_bm (1 << 6) // minimum switch inputs -#define SW_MAX_BIT_bm (1 << 7) // maximum switch inputs - -// Assignments for GPIO1_OUTs for spindle, PWM and coolant -#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_PORT PORTE -#define MIST_PIN_bm (1 << 5) -#define FLOOD_PORT PORTF -#define FLOOD_PIN_bm (1 << 5) /* Interrupt usage: * @@ -294,6 +312,7 @@ typedef enum { #define M3_DMA_TRIGGER DMA_CH_TRIGSRC_TCE0_CCA_gc #define M4_DMA_TRIGGER DMA_CH_TRIGSRC_TCD0_CCA_gc + // Timer setup for stepper and dwells #define STEP_TIMER_DISABLE 0 #define STEP_TIMER_ENABLE TC_CLKSEL_DIV4_gc @@ -304,6 +323,7 @@ typedef enum { #define STEP_TIMER_ISR TCC0_OVF_vect #define STEP_TIMER_INTLVL TC_OVFINTLVL_HI_gc + /* Step correction settings * * Step correction settings determine how the encoder error is fed @@ -324,35 +344,37 @@ typedef enum { #define STEP_CORRECTION_MAX 0.60 /// minimum wait between error correction #define STEP_CORRECTION_HOLDOFF 5 -#define STEP_INITIAL_DIRECTION DIRECTION_CW // TMC2660 driver settings -#define TMC2660_SPI_PORT PORTC -#define TMC2660_SPI_SS_PIN 4 -#define TMC2660_SPI_SCK_PIN 5 -#define TMC2660_SPI_MISO_PIN 6 -#define TMC2660_SPI_MOSI_PIN 7 -#define TMC2660_TIMER TCC1 +#define TMC2660_OVF_vect TCC1_OVF_vect +#define TMC2660_SPI_SS_PIN SERIAL_CTS_PIN +#define TMC2660_SPI_SCK_PIN SPI_CLK_PIN +#define TMC2660_SPI_MISO_PIN SPI_MOSI_PIN +#define TMC2660_SPI_MOSI_PIN SPI_MISO_PIN +#define TMC2660_TIMER TIMER_TMC2660 #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.01 // 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_CTRLB 3 // single slope PWM no output -#define PWM2_ISR_vect TCE1_CCB_vect - - // Huanyang settings +#define HUANYANG_PORT USARTD1 +#define HUANYANG_DRE_vect USARTD1_DRE_vect +#define HUANYANG_TXC_vect USARTD1_TXC_vect +#define HUANYANG_RXC_vect USARTD1_RXC_vect #define HUANYANG_TIMEOUT 50 // ms. response timeout #define HUANYANG_RETRIES 4 // Number of retries before failure #define HUANYANG_ID 1 // Default ID + +// Serial settings +#define SERIAL_BAUD USART_BAUD_115200 +#define SERIAL_PORT USARTC0 +#define SERIAL_DRE_vect USARTC0_DRE_vect +#define SERIAL_RXC_vect USARTC0_RXC_vect + + // Input #define INPUT_BUFFER_LEN 255 // text buffer size (255 max) @@ -361,46 +383,3 @@ typedef enum { #define ARC_RADIUS_ERROR_MAX 1.0 // max mm diff between start and end radius #define ARC_RADIUS_ERROR_MIN 0.005 // min mm where 1% rule applies #define ARC_RADIUS_TOLERANCE 0.001 // 0.1% radius variance test - -// Coordinate offsets -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros -#define G54_Y_OFFSET 0 -#define G54_Z_OFFSET 0 -#define G54_A_OFFSET 0 -#define G54_B_OFFSET 0 -#define G54_C_OFFSET 0 - -#define G55_X_OFFSET (X_TRAVEL_MAX / 2) // set to middle of table -#define G55_Y_OFFSET (Y_TRAVEL_MAX / 2) -#define G55_Z_OFFSET 0 -#define G55_A_OFFSET 0 -#define G55_B_OFFSET 0 -#define G55_C_OFFSET 0 - -#define G56_X_OFFSET 0 -#define G56_Y_OFFSET 0 -#define G56_Z_OFFSET 0 -#define G56_A_OFFSET 0 -#define G56_B_OFFSET 0 -#define G56_C_OFFSET 0 - -#define G57_X_OFFSET 0 -#define G57_Y_OFFSET 0 -#define G57_Z_OFFSET 0 -#define G57_A_OFFSET 0 -#define G57_B_OFFSET 0 -#define G57_C_OFFSET 0 - -#define G58_X_OFFSET 0 -#define G58_Y_OFFSET 0 -#define G58_Z_OFFSET 0 -#define G58_A_OFFSET 0 -#define G58_B_OFFSET 0 -#define G58_C_OFFSET 0 - -#define G59_X_OFFSET 0 -#define G59_Y_OFFSET 0 -#define G59_Z_OFFSET 0 -#define G59_A_OFFSET 0 -#define G59_B_OFFSET 0 -#define G59_C_OFFSET 0 diff --git a/src/coolant.c b/src/coolant.c index a15d385..b023c1c 100644 --- a/src/coolant.c +++ b/src/coolant.c @@ -32,20 +32,20 @@ void coolant_init() { - MIST_PORT.OUTSET = MIST_PIN_bm; // High - MIST_PORT.DIRSET = MIST_PIN_bm; // Output - FLOOD_PORT.OUTSET = FLOOD_PIN_bm; // High - FLOOD_PORT.DIRSET = FLOOD_PIN_bm; // Output + OUTSET_PIN(SWITCH_1_PIN); // High + DIRSET_PIN(SWITCH_1_PIN); // Output + OUTSET_PIN(SWITCH_2_PIN); // High + DIRSET_PIN(SWITCH_2_PIN); // Output } void coolant_set_mist(bool x) { - if (x) MIST_PORT.OUTCLR = MIST_PIN_bm; - else MIST_PORT.OUTSET = MIST_PIN_bm; + if (x) OUTCLR_PIN(SWITCH_1_PIN); + else OUTSET_PIN(SWITCH_1_PIN); } void coolant_set_flood(bool x) { - if (x) FLOOD_PORT.OUTCLR = FLOOD_PIN_bm; - else FLOOD_PORT.OUTSET = FLOOD_PIN_bm; + if (x) OUTCLR_PIN(SWITCH_2_PIN); + else OUTSET_PIN(SWITCH_2_PIN); } diff --git a/src/gpio.c b/src/gpio.c deleted file mode 100644 index bb7cb44..0000000 --- a/src/gpio.c +++ /dev/null @@ -1,43 +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 "gpio.h" - -#include "config.h" - -#include - - -static PORT_t *_ports[MOTORS] = { - &PORT_OUT_X, &PORT_OUT_Y, &PORT_OUT_Z, &PORT_OUT_A -}; - -uint8_t gpio_read_bit(int port) {return _ports[port]->IN & GPIO1_OUT_BIT_bm;} -void gpio_set_bit_on(int port) {_ports[port]->OUTSET = GPIO1_OUT_BIT_bm;} -void gpio_set_bit_off(int port) {_ports[port]->OUTCLR = GPIO1_OUT_BIT_bm;} -void gpio_set_bit_toggle(int port) {_ports[port]->OUTTGL = GPIO1_OUT_BIT_bm;} diff --git a/src/gpio.h b/src/gpio.h deleted file mode 100644 index daee7d1..0000000 --- a/src/gpio.h +++ /dev/null @@ -1,37 +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 - - -uint8_t gpio_read_bit(int b); -void gpio_set_bit_on(int b); -void gpio_set_bit_off(int b); -void gpio_set_bit_toggle(int b); diff --git a/src/huanyang.c b/src/huanyang.c index 8ee5d4e..932c6b4 100644 --- a/src/huanyang.c +++ b/src/huanyang.c @@ -149,38 +149,38 @@ static uint16_t _crc16(const uint8_t *buffer, unsigned length) { static void _set_baud(uint16_t bsel, uint8_t bscale) { - USARTD1.BAUDCTRLB = (uint8_t)((bscale << 4) | (bsel >> 8)); - USARTD1.BAUDCTRLA = bsel; + HUANYANG_PORT.BAUDCTRLB = (uint8_t)((bscale << 4) | (bsel >> 8)); + HUANYANG_PORT.BAUDCTRLA = bsel; } static void _set_write(bool x) { if (x) { - PORTB.OUTSET = 1 << 4; // High - PORTB.OUTSET = 1 << 5; // High + OUTSET_PIN(RS485_RE_PIN); // High + OUTSET_PIN(RS485_DE_PIN); // High } else { - PORTB.OUTCLR = 1 << 4; // Low - PORTB.OUTCLR = 1 << 5; // Low + OUTCLR_PIN(RS485_RE_PIN); // Low + OUTCLR_PIN(RS485_DE_PIN); // Low } } static void _set_dre_interrupt(bool enable) { - if (enable) USARTD1.CTRLA |= USART_DREINTLVL_MED_gc; - else USARTD1.CTRLA &= ~USART_DREINTLVL_MED_gc; + if (enable) HUANYANG_PORT.CTRLA |= USART_DREINTLVL_MED_gc; + else HUANYANG_PORT.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; + if (enable) HUANYANG_PORT.CTRLA |= USART_TXCINTLVL_MED_gc; + else HUANYANG_PORT.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; + if (enable) HUANYANG_PORT.CTRLA |= USART_RXCINTLVL_MED_gc; + else HUANYANG_PORT.CTRLA &= ~USART_RXCINTLVL_MED_gc; } @@ -384,8 +384,8 @@ static void _retry_command() { // Data register empty interrupt -ISR(USARTD1_DRE_vect) { - USARTD1.DATA = ha.command[ha.current_offset++]; +ISR(HUANYANG_DRE_vect) { + HUANYANG_PORT.DATA = ha.command[ha.current_offset++]; if (ha.current_offset == ha.command_length) { _set_dre_interrupt(false); @@ -396,7 +396,7 @@ ISR(USARTD1_DRE_vect) { /// Transmit complete interrupt -ISR(USARTD1_TXC_vect) { +ISR(HUANYANG_TXC_vect) { _set_txc_interrupt(false); _set_rxc_interrupt(true); _set_write(false); // RS485 read mode @@ -405,7 +405,7 @@ ISR(USARTD1_TXC_vect) { // Data received interrupt -ISR(USARTD1_RXC_vect) { +ISR(HUANYANG_RXC_vect) { ha.response[ha.current_offset++] = USARTD1.DATA; if (ha.current_offset == ha.response_length) { @@ -427,20 +427,13 @@ ISR(USARTD1_RXC_vect) { 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 + DIRCLR_PIN(RS485_RO_PIN); // Input + OUTSET_PIN(RS485_DI_PIN); // High + DIRSET_PIN(RS485_DI_PIN); // Output + OUTSET_PIN(RS485_RE_PIN); // High + DIRSET_PIN(RS485_RE_PIN); // Output + OUTSET_PIN(RS485_DE_PIN); // High + DIRSET_PIN(RS485_DE_PIN); // Output _set_baud(3325, 0b1101); // 9600 @ 32MHz with 2x USART diff --git a/src/motor.c b/src/motor.c index 26a87ad..6348232 100644 --- a/src/motor.c +++ b/src/motor.c @@ -62,7 +62,9 @@ typedef struct { cmMotorPowerMode_t power_mode; float step_angle; // degrees per whole step float travel_rev; // mm or deg of travel per motor revolution - PORT_t *port; + uint8_t step_pin; + uint8_t dir_pin; + uint8_t enable_pin; TC0_t *timer; DMA_CH_t *dma; uint8_t dma_trigger; @@ -95,7 +97,9 @@ static motor_t motors[MOTORS] = { .microsteps = M1_MICROSTEPS, .polarity = M1_POLARITY, .power_mode = M1_POWER_MODE, - .port = &PORT_MOTOR_1, + .step_pin = STEP_X_PIN, + .dir_pin = DIR_X_PIN, + .enable_pin = ENABLE_X_PIN, .timer = (TC0_t *)&M1_TIMER, .dma = &M1_DMA_CH, .dma_trigger = M1_DMA_TRIGGER, @@ -106,7 +110,9 @@ static motor_t motors[MOTORS] = { .microsteps = M2_MICROSTEPS, .polarity = M2_POLARITY, .power_mode = M2_POWER_MODE, - .port = &PORT_MOTOR_2, + .step_pin = STEP_Y_PIN, + .dir_pin = DIR_Y_PIN, + .enable_pin = ENABLE_Y_PIN, .timer = &M2_TIMER, .dma = &M2_DMA_CH, .dma_trigger = M2_DMA_TRIGGER, @@ -117,7 +123,9 @@ static motor_t motors[MOTORS] = { .microsteps = M3_MICROSTEPS, .polarity = M3_POLARITY, .power_mode = M3_POWER_MODE, - .port = &PORT_MOTOR_3, + .step_pin = STEP_Z_PIN, + .dir_pin = DIR_Z_PIN, + .enable_pin = ENABLE_Z_PIN, .timer = &M3_TIMER, .dma = &M3_DMA_CH, .dma_trigger = M3_DMA_TRIGGER, @@ -128,7 +136,9 @@ static motor_t motors[MOTORS] = { .microsteps = M4_MICROSTEPS, .polarity = M4_POLARITY, .power_mode = M4_POWER_MODE, - .port = &PORT_MOTOR_4, + .step_pin = STEP_A_PIN, + .dir_pin = DIR_A_PIN, + .enable_pin = ENABLE_A_PIN, .timer = &M4_TIMER, .dma = &M4_DMA_CH, .dma_trigger = M4_DMA_TRIGGER, @@ -141,7 +151,7 @@ static uint8_t _dummy; /// Special interrupt for X-axis ISR(TCE1_CCA_vect) { - PORT_MOTOR_1.OUTTGL = STEP_BIT_bm; + OUTTGL_PIN(STEP_X_PIN); motors[0].steps++; } @@ -159,10 +169,10 @@ void motor_init() { motor_t *m = &motors[motor]; // IO pins - m->port->DIRSET = STEP_BIT_bm; // Output - m->port->DIRSET = DIRECTION_BIT_bm; // Output - m->port->OUTSET = MOTOR_ENABLE_BIT_bm; // High (disabled) - m->port->DIRSET = MOTOR_ENABLE_BIT_bm; // Output + DIRSET_PIN(m->step_pin); // Output + DIRSET_PIN(m->dir_pin); // Output + OUTSET_PIN(m->enable_pin); // High (disabled) + DIRSET_PIN(m->enable_pin); // Output // Setup motor timer m->timer->CTRLB = TC_WGMODE_FRQ_gc | TC1_CCAEN_bm; @@ -195,9 +205,9 @@ void motor_init() { void motor_enable(int motor, bool enable) { - if (enable) motors[motor].port->OUTCLR = MOTOR_ENABLE_BIT_bm; // Active low + if (enable) OUTCLR_PIN(motors[motor].enable_pin); // Active low else { - motors[motor].port->OUTSET = MOTOR_ENABLE_BIT_bm; + OUTSET_PIN(motors[motor].enable_pin); motors[motor].power_state = MOTOR_IDLE; } } @@ -346,8 +356,8 @@ void motor_load_move(int motor) { m->timer_clock = 0; // Clear clock // Set direction - if (m->direction == DIRECTION_CW) m->port->OUTCLR = DIRECTION_BIT_bm; - else m->port->OUTSET = DIRECTION_BIT_bm; + if (m->direction == DIRECTION_CW) OUTCLR_PIN(m->dir_pin); + else OUTSET_PIN(m->dir_pin); // Accumulate encoder // TODO we currently accumulate the x-axis here diff --git a/src/pwm_spindle.c b/src/pwm_spindle.c index a06ba44..196319d 100644 --- a/src/pwm_spindle.c +++ b/src/pwm_spindle.c @@ -96,24 +96,24 @@ static void _spindle_set_pwm(cmSpindleMode_t mode, float speed) { static void _spindle_set_enable(bool enable) { if (enable ^ spindle.enable_invert) - SPINDLE_ENABLE_PORT.OUTSET = SPINDLE_ENABLE_PIN_bm; - else SPINDLE_ENABLE_PORT.OUTCLR = SPINDLE_ENABLE_PIN_bm; + PORT(SPIN_ENABLE_PIN)->OUTSET = BM(SPIN_ENABLE_PIN); + else PORT(SPIN_ENABLE_PIN)->OUTCLR = BM(SPIN_ENABLE_PIN); } static void _spindle_set_dir(bool forward) { - if (forward ^ spindle.reverse) SPINDLE_DIR_PORT.OUTCLR = SPINDLE_DIR_PIN_bm; - else SPINDLE_DIR_PORT.OUTSET = SPINDLE_DIR_PIN_bm; + if (forward ^ spindle.reverse) PORT(SPIN_DIR_PIN)->OUTCLR = BM(SPIN_DIR_PIN); + else PORT(SPIN_DIR_PIN)->OUTSET = BM(SPIN_DIR_PIN); } void pwm_spindle_init() { // Configure IO - SPINDLE_PWM_PORT.DIRSET = SPINDLE_PWM_PIN_bm; // PWM Output + PORT(SPIN_PWM_PIN)->DIRSET = BM(SPIN_PWM_PIN); // PWM Output _spindle_set_dir(true); - SPINDLE_DIR_PORT.DIRSET = SPINDLE_DIR_PIN_bm; // Dir Output + PORT(SPIN_DIR_PIN)->DIRSET = BM(SPIN_DIR_PIN); // Dir Output _spindle_set_enable(false); - SPINDLE_ENABLE_PORT.DIRSET = SPINDLE_ENABLE_PIN_bm; // Enable output + PORT(SPIN_ENABLE_PIN)->DIRSET = BM(SPIN_ENABLE_PIN); // Enable output // Configure clock TIMER_PWM.CTRLB = TC1_CCBEN_bm | TC_WGMODE_SINGLESLOPE_gc; diff --git a/src/switch.c b/src/switch.c index 28cd0e8..ae60854 100644 --- a/src/switch.c +++ b/src/switch.c @@ -74,7 +74,6 @@ typedef enum { typedef struct { swType_t type; swMode_t mode; - PORT_t *port; uint8_t pin; bool min; @@ -95,61 +94,51 @@ swSingleton_t sw = { { // X min .type = SW_TYPE_NORMALLY_OPEN, .mode = SW_MODE_HOMING, - .port = &PORTA, - .pin = 6, + .pin = MIN_X_PIN, .min = true, }, { // X max .type = SW_TYPE_NORMALLY_OPEN, .mode = SW_MODE_DISABLED, - .port = &PORTA, - .pin = 7, + .pin = MAX_X_PIN, .min = false, }, { // Y min .type = SW_TYPE_NORMALLY_OPEN, .mode = SW_MODE_HOMING, - .port = &PORTD, - .pin = 6, + .pin = MIN_Y_PIN, .min = true, }, { // Y max .type = SW_TYPE_NORMALLY_OPEN, .mode = SW_MODE_DISABLED, - .port = &PORTD, - .pin = 7, + .pin = MAX_X_PIN, .min = false, }, { // Z min .type = SW_TYPE_NORMALLY_OPEN, .mode = SW_MODE_DISABLED, - .port = &PORTE, - .pin = 6, + .pin = MIN_Z_PIN, .min = true, }, { // Z max .type = SW_TYPE_NORMALLY_OPEN, .mode = SW_MODE_HOMING, - .port = &PORTE, - .pin = 7, + .pin = MAX_Z_PIN, .min = false, }, { // A min .type = SW_TYPE_NORMALLY_OPEN, .mode = SW_MODE_HOMING, - .port = &PORTF, - .pin = 6, + .pin = MIN_A_PIN, .min = true, }, { // A max .type = SW_TYPE_NORMALLY_OPEN, .mode = SW_MODE_DISABLED, - .port = &PORTF, - .pin = 7, + .pin = MAX_A_PIN, .min = false, }, { // EStop .type = SW_TYPE_NORMALLY_CLOSED, .mode = SW_ESTOP_BIT, - .port = &PORTD, - .pin = 5, + .pin = ESTOP_PIN, }, { // Probe .type = SW_TYPE_NORMALLY_OPEN, .mode = SW_PROBE_BIT, - .port = &PORTF, - .pin = 1, + .pin = PROBE_PIN, }, } }; @@ -157,7 +146,7 @@ swSingleton_t sw = { static bool _read_state(const switch_t *s) { // A normally open switch drives the pin low when thrown - return (s->type == SW_TYPE_NORMALLY_OPEN) ^ (s->port->IN & (1 << s->pin)); + return (s->type == SW_TYPE_NORMALLY_OPEN) ^ IN_PIN(s->pin); } @@ -187,17 +176,17 @@ ISR(PORTF_INT0_vect) {_switch_isr();} void _switch_enable(switch_t *s, bool enable) { if (enable) { - s->port->INT0MASK |= 1 << s->pin; // Enable INT0 + PORT(s->pin)->INT0MASK |= BM(s->pin); // Enable INT0 // Pull up and trigger on both edges - (&s->port->PIN0CTRL)[s->pin] = PORT_OPC_PULLUP_gc | PORT_ISC_BOTHEDGES_gc; + PINCTRL_PIN(s->pin) = PORT_OPC_PULLUP_gc | PORT_ISC_BOTHEDGES_gc; // Initialize state s->state = _read_state(s); } else { - s->port->INT0MASK &= ~(1 << s->pin); // Disable INT0 - (&s->port->PIN0CTRL)[s->pin] = 0; + PORT(s->pin)->INT0MASK &= ~BM(s->pin); // Disable INT0 + PINCTRL_PIN(s->pin) = 0; } } @@ -208,8 +197,8 @@ void switch_init() { for (int i = 0; i < SWITCHES; i++) { switch_t *s = &sw.switches[i]; - s->port->DIRCLR = 1 << s->pin; // Input - s->port->INTCTRL |= SWITCH_INTLVL; // Set interrupt level + DIRCLR_PIN(s->pin); // Input + PORT(s->pin)->INTCTRL |= SWITCH_INTLVL; // Set interrupt level _switch_enable(s, s->mode != SW_MODE_DISABLED); } diff --git a/src/tmc2660.c b/src/tmc2660.c index ba761b2..57eb0e5 100644 --- a/src/tmc2660.c +++ b/src/tmc2660.c @@ -59,7 +59,8 @@ typedef struct { float idle_current; float drive_current; - PORT_t *port; + uint8_t cs_pin; + uint8_t fault_pin; } tmc2660_driver_t; @@ -73,10 +74,10 @@ static const uint32_t reg_addrs[] = { static tmc2660_driver_t drivers[MOTORS] = { - {.port = &PORT_MOTOR_1}, - {.port = &PORT_MOTOR_2}, - {.port = &PORT_MOTOR_3}, - {.port = &PORT_MOTOR_4}, + {.cs_pin = SPI_CS_X_PIN, .fault_pin = FAULT_X_PIN}, + {.cs_pin = SPI_CS_Y_PIN, .fault_pin = FAULT_Y_PIN}, + {.cs_pin = SPI_CS_Z_PIN, .fault_pin = FAULT_Z_PIN}, + {.cs_pin = SPI_CS_A_PIN, .fault_pin = FAULT_A_PIN}, }; @@ -116,15 +117,15 @@ static void _report_error_flags(int driver) { if ((TMC2660_DRVSTATUS_OPEN_LOAD_A | TMC2660_DRVSTATUS_OPEN_LOAD_A) & dflags) mflags |= MOTOR_FLAG_OPEN_LOAD_bm; - if (drv->port->IN & FAULT_BIT_bm) mflags |= MOTOR_FLAG_STALLED_bm; + if (IN_PIN(drv->fault_pin)) mflags |= MOTOR_FLAG_STALLED_bm; if (mflags) motor_error_callback(driver, mflags); } 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; + if (enable) OUTCLR_PIN(drivers[driver].cs_pin); + else OUTSET_PIN(drivers[driver].cs_pin); } @@ -233,7 +234,7 @@ ISR(SPIC_INT_vect) { } -ISR(TCC1_OVF_vect) { +ISR(TMC2660_OVF_vect) { TMC2660_TIMER.CTRLA = 0; // Disable clock _spi_next(); } @@ -296,32 +297,31 @@ void tmc2660_init() { // Setup pins // Must set the SS pin either in/high or any/output for master mode to work // Note, this pin is also used by the USART as the CTS line - TMC2660_SPI_PORT.DIRSET = 1 << TMC2660_SPI_SS_PIN; // Output - - TMC2660_SPI_PORT.OUTSET = 1 << TMC2660_SPI_SCK_PIN; // High - TMC2660_SPI_PORT.DIRSET = 1 << TMC2660_SPI_SCK_PIN; // Output - - TMC2660_SPI_PORT.DIRCLR = 1 << TMC2660_SPI_MISO_PIN; // Input - TMC2660_SPI_PORT.OUTSET = 1 << TMC2660_SPI_MOSI_PIN; // High - TMC2660_SPI_PORT.DIRSET = 1 << TMC2660_SPI_MOSI_PIN; // Output + DIRSET_PIN(TMC2660_SPI_SS_PIN); // Output + OUTSET_PIN(TMC2660_SPI_SCK_PIN); // High + DIRSET_PIN(TMC2660_SPI_SCK_PIN); // Output + DIRCLR_PIN(TMC2660_SPI_MISO_PIN); // Intput + OUTSET_PIN(TMC2660_SPI_MOSI_PIN); // High + DIRSET_PIN(TMC2660_SPI_MOSI_PIN); // Output for (int driver = 0; driver < MOTORS; driver++) { - PORT_t *port = drivers[driver].port; + uint8_t cs_pin = drivers[driver].cs_pin; + uint8_t fault_pin = drivers[driver].fault_pin; - port->OUTSET = CHIP_SELECT_BIT_bm; // High - port->DIRSET = CHIP_SELECT_BIT_bm; // Output - port->DIRCLR = FAULT_BIT_bm; // Input + OUTSET_PIN(cs_pin); // High + DIRSET_PIN(cs_pin); // Output + OUTCLR_PIN(fault_pin); // Input - port->PIN4CTRL = PORT_ISC_RISING_gc; - port->INT1MASK = FAULT_BIT_bm; // INT1 - port->INTCTRL |= PORT_INT1LVL_HI_gc; + PINCTRL_PIN(fault_pin) = PORT_ISC_RISING_gc; + PORT(fault_pin)->INT1MASK = BM(fault_pin); // INT1 + PORT(fault_pin)->INTCTRL |= PORT_INT1LVL_HI_gc; } // Configure SPI PR.PRPC &= ~PR_SPI_bm; // Disable power reduction 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 + PORT(TMC2660_SPI_SCK_PIN)->REMAP = PORT_SPI_bm; // Swap SCK and MOSI SPIC.INTCTRL = SPI_INTLVL_LO_gc; // interupt level // Configure timer diff --git a/src/usart.c b/src/usart.c index a99528c..3846178 100644 --- a/src/usart.c +++ b/src/usart.c @@ -26,6 +26,7 @@ \******************************************************************************/ #include "usart.h" +#include "cpp_magic.h" #include "config.h" #include @@ -47,39 +48,39 @@ static int usart_flags = USART_CRLF | USART_ECHO; static void _set_dre_interrupt(bool enable) { - if (enable) USARTC0.CTRLA |= USART_DREINTLVL_HI_gc; - else USARTC0.CTRLA &= ~USART_DREINTLVL_HI_gc; + if (enable) SERIAL_PORT.CTRLA |= USART_DREINTLVL_HI_gc; + else SERIAL_PORT.CTRLA &= ~USART_DREINTLVL_HI_gc; } static void _set_rxc_interrupt(bool enable) { if (enable) { - USARTC0.CTRLA |= USART_RXCINTLVL_HI_gc; - if (4 <= rx_buf_space()) PORTC.OUTCLR = 1 << 4; // CTS Lo (enable) + SERIAL_PORT.CTRLA |= USART_RXCINTLVL_HI_gc; + if (4 <= rx_buf_space()) OUTCLR_PIN(SERIAL_CTS_PIN); // CTS Lo (enable) - } else USARTC0.CTRLA &= ~USART_RXCINTLVL_HI_gc; + } else SERIAL_PORT.CTRLA &= ~USART_RXCINTLVL_HI_gc; } // Data register empty interrupt vector -ISR(USARTC0_DRE_vect) { +ISR(SERIAL_DRE_vect) { if (tx_buf_empty()) _set_dre_interrupt(false); // Disable interrupt else { - USARTC0.DATA = tx_buf_peek(); + SERIAL_PORT.DATA = tx_buf_peek(); tx_buf_pop(); } } // Data received interrupt vector -ISR(USARTC0_RXC_vect) { +ISR(SERIAL_RXC_vect) { if (rx_buf_full()) _set_rxc_interrupt(false); // Disable interrupt else { - uint8_t data = USARTC0.DATA; + uint8_t data = SERIAL_PORT.DATA; rx_buf_push(data); - if (rx_buf_space() < 4) PORTC.OUTSET = 1 << 4; // CTS Hi (disable) + if (rx_buf_space() < 4) OUTSET_PIN(SERIAL_CTS_PIN); // CTS Hi (disable) } } @@ -100,21 +101,21 @@ void usart_init(void) { PR.PRPC &= ~PR_USART0_bm; // Disable power reduction // Setup pins - PORTC.OUTSET = 1 << 4; // CTS Hi (disable) - PORTC.DIRSET = 1 << 4; // CTS Output - PORTC.OUTSET = 1 << 3; // Tx High - PORTC.DIRSET = 1 << 3; // Tx Output - PORTC.DIRCLR = 1 << 2; // Rx Input + OUTSET_PIN(SERIAL_CTS_PIN); // CTS Hi (disable) + DIRSET_PIN(SERIAL_CTS_PIN); // CTS Output + OUTSET_PIN(SERIAL_TX_PIN); // Tx High + DIRSET_PIN(SERIAL_TX_PIN); // Tx Output + DIRCLR_PIN(SERIAL_RX_PIN); // Rx Input // Set baud rate - usart_set_baud(USART_BAUD_115200); + usart_set_baud(SERIAL_BAUD); // No parity, 8 data bits, 1 stop bit - USARTC0.CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | + SERIAL_PORT.CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | USART_CHSIZE_8BIT_gc; // Configure receiver and transmitter - USARTC0.CTRLB = USART_RXEN_bm | USART_TXEN_bm | USART_CLK2X_bm; + SERIAL_PORT.CTRLB = USART_RXEN_bm | USART_TXEN_bm | USART_CLK2X_bm; PMIC.CTRL |= PMIC_HILVLEN_bm; // Interrupt level on @@ -128,8 +129,8 @@ void usart_init(void) { static void _set_baud(uint16_t bsel, uint8_t bscale) { - USARTC0.BAUDCTRLB = (uint8_t)((bscale << 4) | (bsel >> 8)); - USARTC0.BAUDCTRLA = bsel; + SERIAL_PORT.BAUDCTRLB = (uint8_t)((bscale << 4) | (bsel >> 8)); + SERIAL_PORT.BAUDCTRLA = bsel; } @@ -251,8 +252,8 @@ int16_t usart_peek() { void usart_flush() { usart_set(USART_FLUSH, true); - while (!tx_buf_empty() || !(USARTC0.STATUS & USART_DREIF_bm) || - !(USARTC0.STATUS & USART_TXCIF_bm)) + while (!tx_buf_empty() || !(SERIAL_PORT.STATUS & USART_DREIF_bm) || + !(SERIAL_PORT.STATUS & USART_TXCIF_bm)) continue; }