From c96ed5be92eedfc7da00c11fe88e74ea0e22b082 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sun, 27 Mar 2016 01:03:10 -0700 Subject: [PATCH] Removed controller and encoder, reorg of hardware, use DMA channels to count motor pulses --- Makefile | 20 ++-- src/canonical_machine.c | 5 +- src/clock.c | 112 ++------------------ src/command.c | 43 ++++++-- src/command.def | 14 ++- src/config.h | 28 ++--- src/controller.c | 155 --------------------------- src/controller.h | 50 --------- src/encoder.c | 63 ----------- src/encoder.h | 122 --------------------- src/gcode_parser.c | 1 - src/gpio.c | 68 ++---------- src/gpio.h | 18 +--- src/hardware.c | 153 ++++++++++++++------------- src/hardware.h | 25 +---- src/main.c | 108 ++++++++++++++++--- src/messages.def | 2 +- src/motor.c | 176 +++++++++++++++++++++---------- src/motor.h | 3 + src/plan/exec.c | 8 +- src/plan/feedhold.c | 4 +- src/plan/jog.c | 4 +- src/plan/line.c | 29 +++-- src/plan/planner.c | 4 +- src/status.h | 1 + src/stepper.c | 27 ++--- src/stepper.h | 4 +- src/switch.c | 124 +++++++++++++--------- src/switch.h | 10 +- src/tmc2660.c | 55 +++++----- src/usart.c | 13 ++- src/usart.h | 19 ++-- src/vars.c | 228 ++++++++++++++++++++++++++++++++++------ src/vars.def | 1 + src/vars.h | 9 ++ 35 files changed, 780 insertions(+), 926 deletions(-) delete mode 100644 src/controller.c delete mode 100644 src/controller.h delete mode 100644 src/encoder.c delete mode 100644 src/encoder.h diff --git a/Makefile b/Makefile index e396bf6..d9b0bd3 100644 --- a/Makefile +++ b/Makefile @@ -81,9 +81,17 @@ fuses: -U fuse2:w:$(FUSE2):m -U fuse4:w:$(FUSE4):m -U fuse5:w:$(FUSE5):m read_fuses: - avrdude $(AVRDUDE_OPTS) -U fuse0:r:fuse0.hex:h -U fuse1:r:fuse1.hex:h \ - -U fuse2:r:fuse2.hex:h -U fuse4:r:fuse4.hex:h -U fuse5:r:fuse5.hex:h - @ cat fuse?.hex + avrdude $(AVRDUDE_OPTS) -q -q -U fuse0:r:-:h -U fuse1:r:-:h -U fuse2:r:-:h \ + -U fuse4:r:-:h -U fuse5:r:-:h + +signature: + avrdude $(AVRDUDE_OPTS) -q -q -U signature:r:-:h + +prodsig: + avrdude $(AVRDUDE_OPTS) -q -q -U prodsig:r:-:h + +usersig: + avrdude $(AVRDUDE_OPTS) -q -q -U usersig:r:-:h # Clean tidy: @@ -91,10 +99,10 @@ tidy: clean: tidy rm -rf $(PROJECT).elf $(PROJECT).hex $(PROJECT).eep $(PROJECT).lss \ - $(PROJECT).map build fuse?.hex + $(PROJECT).map build -.PHONY: tidy clean size all reset erase program fuses read_fuses +.PHONY: tidy clean size all reset erase program fuses read_fuses prodsig +.PHONY: signature usersig # Dependencies -include $(shell mkdir -p build/dep) $(wildcard build/dep/*) - diff --git a/src/canonical_machine.c b/src/canonical_machine.c index 8664b5b..a113fe2 100644 --- a/src/canonical_machine.c +++ b/src/canonical_machine.c @@ -101,7 +101,6 @@ #include "config.h" #include "stepper.h" -#include "encoder.h" #include "spindle.h" #include "gpio.h" #include "switch.h" @@ -680,13 +679,15 @@ stat_t cm_clear() { /// Alarm state; send an exception report and shut down machine stat_t cm_hard_alarm(stat_t status) { // Hard stop the motors and the spindle - stepper_init(); + st_shutdown(); cm_spindle_control(SPINDLE_OFF); print_status_message("HARD ALARM", status); cm.machine_state = MACHINE_SHUTDOWN; + return status; } + // Representation (4.3.3) // // Affect the Gcode model only (asynchronous) diff --git a/src/clock.c b/src/clock.c index 006fde6..8c50d0b 100644 --- a/src/clock.c +++ b/src/clock.c @@ -33,8 +33,6 @@ #include #include -void CCPWrite(volatile uint8_t * address, uint8_t value); - /// This routine is lifted and modified from Boston Android and from /// http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&p=711659 @@ -44,10 +42,14 @@ void clock_init() { 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 - CCPWrite(&CLK.CTRL, CLK_SCLKSEL_PLL_gc); // switch to PLL clock + + 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 @@ -56,10 +58,14 @@ void clock_init() { 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 - CCPWrite(&CLK.CTRL, CLK_SCLKSEL_PLL_gc); // switch to PLL clock + + 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 @@ -68,104 +74,8 @@ void clock_init() { 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 } - -/****************************************************************************** - * The following code was excerpted from the Atmel AVR1003 clock driver example - * and carries its copyright: - * - * $Revision: 2771 $ - * $Date: 2009-09-11 11:54:26 +0200 (fr, 11 sep 2009) $ \n - * - * Copyright (c) 2008, Atmel Corporation All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. The name of ATMEL may not be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED - * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY AND - * SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT, - * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR FART - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF - * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SHOE DAMAGE. - *****************************************************************************/ - -/* Macros to protect the code from interrupts */ - -#define AVR_ENTER_CRITICAL_REGION() uint8_t volatile saved_sreg = SREG; cli(); -#define AVR_LEAVE_CRITICAL_REGION() SREG = saved_sreg; - - -/* CCP write helper function written in assembly. - * - * This function is written in assembly because of the time critial - * operation of writing to the registers. - * - * - address A pointer to the address to write to. - * - value The value to put in to the register. - */ -void CCPWrite( volatile uint8_t * address, uint8_t value ) { -#ifdef __ICCAVR__ - - // Store global interrupt setting in scratch register and disable interrupts. - asm("in R1, 0x3F \n" - "cli" - ); - - // Move destination address pointer to Z pointer registers. - asm("movw r30, r16"); -#ifdef RAMPZ - asm("ldi R16, 0 \n" - "out 0x3B, R16" - ); - -#endif - asm("ldi r16, 0xD8 \n" - "out 0x34, r16 \n" -#if (__MEMORY_MODEL__ == 1) - "st Z, r17 \n"); -#elif (__MEMORY_MODEL__ == 2) - "st Z, r18 \n"); -#else /* (__MEMORY_MODEL__ == 3) || (__MEMORY_MODEL__ == 5) */ -"st Z, r19 \n"); -#endif /* __MEMORY_MODEL__ */ - -// Restore global interrupt setting from scratch register. -asm("out 0x3F, R1"); - -#elif defined __GNUC__ -AVR_ENTER_CRITICAL_REGION(); -volatile uint8_t * tmpAddr = address; -#ifdef RAMPZ -RAMPZ = 0; -#endif -asm volatile( - "movw r30, %0" "\n\t" - "ldi r16, %2" "\n\t" - "out %3, r16" "\n\t" - "st Z, %1" "\n\t" - : - : "r" (tmpAddr), "r" (value), "M" (CCP_IOREG_gc), "i" (&CCP) - : "r16", "r30", "r31" - ); - -AVR_LEAVE_CRITICAL_REGION(); -#endif -} diff --git a/src/command.c b/src/command.c index 0b357df..33f10c7 100644 --- a/src/command.c +++ b/src/command.c @@ -46,23 +46,23 @@ // Command forward declarations // (Don't be afraid, X-Macros rock!) -#define CMD(name, func, minArgs, maxArgs, help) \ - uint8_t func(int, char *[]); +#define CMD(NAME, ...) \ + uint8_t command_##NAME(int, char *[]); #include "command.def" #undef CMD // Command names & help -#define CMD(name, func, minArgs, maxArgs, help) \ - static const char pstr_##name[] PROGMEM = #name; \ - static const char name##_help[] PROGMEM = help; +#define CMD(NAME, MINARGS, MAXARGS, HELP) \ + static const char pstr_##NAME[] PROGMEM = #NAME; \ + static const char NAME##_help[] PROGMEM = HELP; #include "command.def" #undef CMD // Command table -#define CMD(name, func, minArgs, maxArgs, help) \ - {pstr_##name, func, minArgs, maxArgs, name##_help}, +#define CMD(NAME, MINARGS, MAXARGS, HELP) \ + {pstr_##NAME, command_##NAME, MINARGS, MAXARGS, NAME##_help}, static const command_t commands[] PROGMEM = { #include "command.def" @@ -220,6 +220,35 @@ uint8_t command_reboot(int argc, char *argv[]) { } +uint8_t command_bootloader(int argc, char *argv[]) { + hw_request_bootloader(); + return 0; +} + + +uint8_t command_save(int argc, char *argv[]) { + vars_save(); + return 0; +} + + +uint8_t command_valid(int argc, char *argv[]) { + printf_P(vars_valid() ? PSTR("true\n") : PSTR("false\n")); + return 0; +} + + +uint8_t command_restore(int argc, char *argv[]) { + return vars_restore(); +} + + +uint8_t command_clear(int argc, char *argv[]) { + vars_clear(); + return 0; +} + + uint8_t command_jog(int argc, char *argv[]) { float velocity[AXES]; diff --git a/src/command.def b/src/command.def index d431f5f..534eed5 100644 --- a/src/command.def +++ b/src/command.def @@ -25,7 +25,13 @@ \******************************************************************************/ -CMD(help, command_help, 0, 1, "Print this help screen") -CMD(reboot, command_reboot, 0, 0, "Reboot the controller") -CMD(jog, command_jog, 1, 4, "Jog") -CMD(mreset, command_mreset, 0, 1, "Reset motor") +// Name Min, Max args, Help +CMD(help, 0, 1, "Print this help screen") +CMD(reboot, 0, 0, "Reboot the controller") +CMD(bootloader, 0, 0, "Load bootloader") +CMD(save, 0, 0, "Save settings") +CMD(valid, 0, 0, "Print 'true' if saved settings are valid") +CMD(restore, 0, 0, "Restore settings") +CMD(clear, 0, 0, "Clear saved settings") +CMD(jog, 1, 4, "Jog") +CMD(mreset, 0, 1, "Reset motor") diff --git a/src/config.h b/src/config.h index 5135383..ed9ac40 100644 --- a/src/config.h +++ b/src/config.h @@ -34,7 +34,7 @@ #define __STEP_CORRECTION //#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_EXTERNAL_16MHZ // uses PLL to provide 32 MHz system clock #define AXES 6 // number of axes @@ -268,15 +268,12 @@ typedef enum { #define SW_MIN_BIT_bm (1 << 6) // minimum switch inputs #define SW_MAX_BIT_bm (1 << 7) // maximum switch inputs -// Bit assignments for GPIO1_OUTs for spindle, PWM and coolant -#define SPINDLE_BIT 8 // spindle on/off -#define SPINDLE_DIR 4 // spindle direction, 1=CW, 0=CCW +// 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_COOLANT_BIT 1 // coolant on/off (same as flood) -#define FLOOD_COOLANT_BIT 1 // coolant on/off (same as mist) - -// Can use the spindle direction as an indicator LED. See gpio.h -#define INDICATOR_LED SPINDLE_DIR_LED +#define MIST_COOLANT_BIT 3 // coolant on/off (same as flood) +#define FLOOD_COOLANT_BIT 3 // coolant on/off (same as mist) /* Interrupt usage: * @@ -304,10 +301,15 @@ typedef enum { #define M3_TIMER TCE0 #define M4_TIMER TCD0 -#define M1_TIMER_CC CCA -#define M2_TIMER_CC CCA -#define M3_TIMER_CC CCA -#define M4_TIMER_CC CCA +#define M1_DMA_CH DMA.CH0 +#define M2_DMA_CH DMA.CH1 +#define M3_DMA_CH DMA.CH2 +#define M4_DMA_CH DMA.CH3 + +#define M1_DMA_TRIGGER DMA_CH_TRIGSRC_TCE1_CCA_gc +#define M2_DMA_TRIGGER DMA_CH_TRIGSRC_TCF0_CCA_gc +#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 diff --git a/src/controller.c b/src/controller.c deleted file mode 100644 index 205a4b6..0000000 --- a/src/controller.c +++ /dev/null @@ -1,155 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - 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 "controller.h" - -#include "canonical_machine.h" -#include "stepper.h" -#include "motor.h" -#include "tmc2660.h" -#include "gpio.h" -#include "switch.h" -#include "encoder.h" -#include "hardware.h" -#include "usart.h" -#include "util.h" -#include "rtc.h" -#include "command.h" -#include "report.h" - -#include "plan/buffer.h" -#include "plan/arc.h" -#include "plan/feedhold.h" - -#include -#include - - -controller_t cs; // controller state structure - - -/// Blink rapidly and prevent further activity from occurring Shutdown -/// idler flashes indicator LED rapidly to show everything is not OK. -/// Shutdown idler returns EAGAIN causing the control loop to never -/// advance beyond this point. It's important that the reset handler -/// is still called so a SW reset (ctrl-x) or bootloader request can -/// be processed. -static stat_t _shutdown_idler() { - if (cm_get_machine_state() != MACHINE_SHUTDOWN) return STAT_OK; - - if (rtc_get_time() > cs.led_timer) { - cs.led_timer = rtc_get_time() + LED_ALARM_TIMER; - indicator_led_toggle(); - } - - return STAT_EAGAIN; // EAGAIN prevents any lower-priority actions from running -} - - -/// Return eagain if TX queue is backed up -static stat_t _sync_to_tx_buffer() { - if (usart_tx_full()) return STAT_EAGAIN; - - return STAT_OK; -} - - -/// Return eagain if planner is not ready for a new command -static stat_t _sync_to_planner() { - // allow up to N planner buffers for this line - if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM) - return STAT_EAGAIN; - - return STAT_OK; -} - - -/// Shut down system if limit switch fired -static stat_t _limit_switch_handler() { - if (cm_get_machine_state() == MACHINE_ALARM) return STAT_NOOP; - if (!switch_get_limit_thrown()) return STAT_NOOP; - - return cm_hard_alarm(STAT_LIMIT_SWITCH_HIT); -} - - -void controller_init() { - // clear all values, job_id's, pointers and status - memset(&cs, 0, sizeof(controller_t)); -} - - -/* Main loop - top-level controller - * - * The order of the dispatched tasks is very important. - * Tasks are ordered by increasing dependency (blocking hierarchy). - * Tasks that are dependent on completion of lower-level tasks must be - * later in the list than the task(s) they are dependent upon. - * - * Tasks must be written as continuations as they will be called repeatedly, - * and are called even if they are not currently active. - * - * The DISPATCH macro calls the function and returns to the controller parent - * if not finished (STAT_EAGAIN), preventing later routines from running - * (they remain blocked). Any other condition - OK or ERR - drops through - * and runs the next routine in the list. - * - * A routine that had no action (i.e. is OFF or idle) should return STAT_NOOP - */ -void controller_run() { -#define DISPATCH(func) if (func == STAT_EAGAIN) return; - - // Interrupt Service Routines are the highest priority controller functions - // 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_hard_reset_handler()); // 1. handle hard reset requests - DISPATCH(hw_bootloader_handler()); // 2. handle bootloader requests - DISPATCH(_shutdown_idler()); // 3. idle in shutdown state - DISPATCH(_limit_switch_handler()); // 4. limit switch thrown - DISPATCH(cm_feedhold_sequencing_callback()); // 5a. feedhold state machine - DISPATCH(mp_plan_hold_callback()); // 5b. 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_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(command_dispatch()); // read and execute next command -} diff --git a/src/controller.h b/src/controller.h deleted file mode 100644 index 316f880..0000000 --- a/src/controller.h +++ /dev/null @@ -1,50 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - 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 "status.h" - - -#define LED_ALARM_TIMER 100 // blink rate for alarm state (in ms) - - -typedef struct controllerSingleton { // main TG controller struct - // system state variables - uint32_t led_timer; // used by idlers to flash indicator LED - uint8_t hard_reset_requested; // flag to perform a hard reset - uint8_t bootloader_requested; // flag to enter the bootloader -} controller_t; - -extern controller_t cs; // controller state structure - - -void controller_init(); -void controller_run(); diff --git a/src/encoder.c b/src/encoder.c deleted file mode 100644 index 8ba3fb3..0000000 --- a/src/encoder.c +++ /dev/null @@ -1,63 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2013 - 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 "encoder.h" - -#include -#include - - -enEncoder_t en[MOTORS]; - - -void encoder_init() { - memset(en, 0, sizeof(en)); // clear all values, pointers and status -} - - -/* Set encoder values to a current step count - * - * Sets encoder_steps. Takes floating point steps as input, - * writes integer steps. So it's not an exact representation of machine - * position except if the machine is at zero. - */ -void en_set_encoder_steps(uint8_t motor, float steps) { - en[motor].encoder_steps = (int32_t)round(steps); -} - - -/* The stepper ISR counts steps into steps_run. These values are - * accumulated to encoder_steps during LOAD (HI interrupt - * level). The encoder_steps is therefore always stable. But be - * advised: the position lags target and position values - * elsewhere in the system becuase the sample is taken when the - * steps for that segment are complete. - */ -float en_read_encoder(uint8_t motor) { - return en[motor].encoder_steps; -} diff --git a/src/encoder.h b/src/encoder.h deleted file mode 100644 index f226d12..0000000 --- a/src/encoder.h +++ /dev/null @@ -1,122 +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" - -\******************************************************************************/ - -/* Encoders - * - * Calling this file "encoders" is kind of a lie, at least for - * now. There are no encoders. Instead the steppers count steps to - * provide a "truth" reference for position. In the future when we - * have real encoders we'll stop counting steps and actually measure - * the position. Which should be a lot easier than how this module - * currently works. - * - * Measuring position - * - * The challenge is that you can't just measure the position at any - * arbitrary point because the system is so heavily queued (pipelined) - * by the planner queue and the stepper sequencing. - * - * You only know where the machine should be at known "targets", which - * are at the end of each move section (end of head, body, and - * tail). You need to take encoder readings at these points. This - * synchronization is taken care of by the Target, Position, - * Position_delayed sequence in exec. Referring to ASCII art in - * stepper.h and reproduced here: - * - * LOAD/STEP ~5000uS [L1][Seg1][L2][Seg2][L3][Seg3][L4][Seg4][Lb1][Segb1] - * PREP 100 uS [P1] [P2] [P3] [P4] [Pb1] [Pb2] - * EXEC 400 uS [EXEC1] [EXEC2] [EXEC3] [EXEC4] [EXECb1] [EXECb2] - * PLAN <4ms [PLANmoveA][PLANmoveB][PLANmoveC][PLANmoveD][PLANmoveE] etc. - * - * You can collect the target for moveA as early as the end of - * [PLANmoveA]. The system will not reach that target position until - * the end of [Seg4]. Data from Seg4 can only be processed - * during the EXECb2 or Pb2 interval as it's the first time that is - * not time-critical and you actually have enough cycles to calculate - * the position and error terms. We use Pb2. - * - * Additionally, by this time the target in Gcode model knows about - * has advanced quite a bit, so the moveA target needs to be saved - * somewhere. Targets are propagated downward to the planner runtime - * (the EXEC), but the exec will have moved on to moveB by the time we - * need it. So moveA's target needs to be saved somewhere. - * - * Error correction - * - * The purpose of this module is to calculate an error term between - * the programmed position (target) and the actual measured position - * (position). The error term is used during move execution (exec) to - * adjust the move to compensate for accumulated positional - * errors. It's also the basis of closed-loop (servoed) systems. - * - * Positional error occurs due to floating point numerical - * inaccuracies. This code uses 32 bit floating point (GCC 32 bit, - * which is NOT IEEE 32 bit). Errors creep in during planning, move - * execution, and stepper output phases. Care has been taken to - * minimize introducing errors throughout the process, but they still - * occur. In most cases errors are not noticeable as they fall below - * the step resolution for most jobs. For jobs that run > 1 hour the - * errors can accumulate and send results off by as much as a - * millimeter if not corrected. - * - * Note: Going to doubles (from floats) would reduce the errors but - * not eliminate them altogether. But this moot on AVRGCC which only - * does single precision floats. - * - * Applying the error term for error correction - * - * So if you want to use the error from moveA to correct moveB it has - * to be done in a region that is not already running (i.e. the head, - * body, or tail) as moveB is already 2 segments into run. Since most - * moves in very short line Gcode files are body only, for practical - * purposes the correction will be applied to moveC. (It's possible to - * recompute the body of moveB, but it may not be worth the trouble). - */ - -#pragma once - - -#include "config.h" - -#include - - -/// One real or virtual encoder per controlled motor -typedef struct enEncoder { - int8_t step_sign; // set to +1 or -1 - int16_t steps_run; // steps counted during stepper interrupt - int32_t encoder_steps; // counted encoder position in steps -} enEncoder_t; - - -extern enEncoder_t en[MOTORS]; // Used by stepper.c - - -void encoder_init(); -void en_set_encoder_steps(uint8_t motor, float steps); -float en_read_encoder(uint8_t motor); diff --git a/src/gcode_parser.c b/src/gcode_parser.c index 40c9eee..6afd7a4 100644 --- a/src/gcode_parser.c +++ b/src/gcode_parser.c @@ -28,7 +28,6 @@ #include "gcode_parser.h" -#include "controller.h" #include "canonical_machine.h" #include "spindle.h" #include "util.h" diff --git a/src/gpio.c b/src/gpio.c index 4fd46b1..bb7cb44 100644 --- a/src/gpio.c +++ b/src/gpio.c @@ -26,68 +26,18 @@ \******************************************************************************/ -/* This GPIO file is where all parallel port bits are managed that are - * not already taken up by steppers, serial ports, SPI or PDI programming - * - * There are 2 GPIO ports: - * - * gpio1 Located on 5x2 header next to the PDI programming plugs - * Four (4) output bits capable of driving 3.3v or 5v logic - * - * Note: On v6 and earlier boards there are also 4 inputs: - * Four (4) level converted input bits capable of being driven - * by 3.3v or 5v logic - connected to B0 - B3 (now used for SPI) - * - * gpio2 Located on 9x2 header on "bottom" edge of the board - * Eight (8) non-level converted input bits - * Eight (8) ground pins - one each "under" each input pin - * Two (2) 3.3v power pins (on left edge of connector) - * Inputs can be used as switch contact inputs or - * 3.3v input bits depending on port configuration - * **** These bits CANNOT be used as 5v inputs **** - */ - #include "gpio.h" -#include "hardware.h" - - -void indicator_led_set() {gpio_led_on(INDICATOR_LED);} -void indicator_led_clear() {gpio_led_off(INDICATOR_LED);} -void indicator_led_toggle() {gpio_led_toggle(INDICATOR_LED);} -void gpio_led_on(uint8_t led) {gpio_set_bit_on(8 >> led);} -void gpio_led_off(uint8_t led) {gpio_set_bit_off(8 >> led);} -void gpio_led_toggle(uint8_t led) {gpio_set_bit_toggle(8 >> led);} - - -uint8_t gpio_read_bit(uint8_t b) { - if (b & 0x08) return hw.out_port[0]->IN & GPIO1_OUT_BIT_bm; - if (b & 0x04) return hw.out_port[1]->IN & GPIO1_OUT_BIT_bm; - if (b & 0x02) return hw.out_port[2]->IN & GPIO1_OUT_BIT_bm; - if (b & 0x01) return hw.out_port[3]->IN & GPIO1_OUT_BIT_bm; - - return 0; -} - -void gpio_set_bit_on(uint8_t b) { - if (b & 0x08) hw.out_port[0]->OUTSET = GPIO1_OUT_BIT_bm; - if (b & 0x04) hw.out_port[1]->OUTSET = GPIO1_OUT_BIT_bm; - if (b & 0x02) hw.out_port[2]->OUTSET = GPIO1_OUT_BIT_bm; - if (b & 0x01) hw.out_port[3]->OUTSET = GPIO1_OUT_BIT_bm; -} +#include "config.h" +#include -void gpio_set_bit_off(uint8_t b) { - if (b & 0x08) hw.out_port[0]->OUTCLR = GPIO1_OUT_BIT_bm; - if (b & 0x04) hw.out_port[1]->OUTCLR = GPIO1_OUT_BIT_bm; - if (b & 0x02) hw.out_port[2]->OUTCLR = GPIO1_OUT_BIT_bm; - if (b & 0x01) hw.out_port[3]->OUTCLR = GPIO1_OUT_BIT_bm; -} +static PORT_t *_ports[MOTORS] = { + &PORT_OUT_X, &PORT_OUT_Y, &PORT_OUT_Z, &PORT_OUT_A +}; -void gpio_set_bit_toggle(uint8_t b) { - if (b & 0x08) hw.out_port[0]->OUTTGL = GPIO1_OUT_BIT_bm; - if (b & 0x04) hw.out_port[1]->OUTTGL = GPIO1_OUT_BIT_bm; - if (b & 0x02) hw.out_port[2]->OUTTGL = GPIO1_OUT_BIT_bm; - if (b & 0x01) hw.out_port[3]->OUTTGL = GPIO1_OUT_BIT_bm; -} +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 index 5f48d5b..daee7d1 100644 --- a/src/gpio.h +++ b/src/gpio.h @@ -31,17 +31,7 @@ #include -enum {SPINDLE_LED, SPINDLE_DIR_LED, SPINDLE_PWM_LED, COOLANT_LED}; - -void indicator_led_set(); -void indicator_led_clear(); -void indicator_led_toggle(); - -void gpio_led_on(uint8_t led); -void gpio_led_off(uint8_t led); -void gpio_led_toggle(uint8_t led); - -uint8_t gpio_read_bit(uint8_t b); -void gpio_set_bit_on(uint8_t b); -void gpio_set_bit_off(uint8_t b); -void gpio_set_bit_toggle(uint8_t b); +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/hardware.c b/src/hardware.c index 1a8f8d1..86223ae 100644 --- a/src/hardware.c +++ b/src/hardware.c @@ -27,120 +27,119 @@ \******************************************************************************/ #include "hardware.h" -#include "switch.h" -#include "controller.h" -#include "clock.h" #include "rtc.h" +#include "usart.h" +#include "clock.h" -#include // defines PROGMEM and PSTR -#include // used for software reset +#include +#include +#include #include +#include + + +typedef struct { + char id[26]; + bool hard_reset_requested; // flag to perform a hard reset + bool bootloader_requested; // flag to enter the bootloader +} hw_t; +static hw_t hw = {}; -hwSingleton_t hw; +#define PROD_SIGS (*(NVM_PROD_SIGNATURES_t *)0x0000) +#define HEXNIB(x) "0123456789abcdef"[(x) & 0xf] -/// Bind XMEGA ports to hardware -static void _port_bindings() { - hw.st_port[0] = &PORT_MOTOR_1; - hw.st_port[1] = &PORT_MOTOR_2; - hw.st_port[2] = &PORT_MOTOR_3; - hw.st_port[3] = &PORT_MOTOR_4; - hw.sw_port[0] = &PORT_SWITCH_X; - hw.sw_port[1] = &PORT_SWITCH_Y; - hw.sw_port[2] = &PORT_SWITCH_Z; - hw.sw_port[3] = &PORT_SWITCH_A; +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); + NVM.CMD = NVM_CMD_NO_OPERATION_gc; - hw.out_port[0] = &PORT_OUT_X; - hw.out_port[1] = &PORT_OUT_Y; - hw.out_port[2] = &PORT_OUT_Z; - hw.out_port[3] = &PORT_OUT_A; + hw.id[i] = HEXNIB(byte >> 4); + hw.id[i + 1] = HEXNIB(byte); +} + + +static void _read_hw_id() { + int i = 0; + _load_hw_id_byte(i, &PROD_SIGS.LOTNUM5); i += 2; + _load_hw_id_byte(i, &PROD_SIGS.LOTNUM4); i += 2; + _load_hw_id_byte(i, &PROD_SIGS.LOTNUM3); i += 2; + _load_hw_id_byte(i, &PROD_SIGS.LOTNUM2); i += 2; + _load_hw_id_byte(i, &PROD_SIGS.LOTNUM1); i += 2; + _load_hw_id_byte(i, &PROD_SIGS.LOTNUM0); i += 2; + hw.id[i++] = '-'; + _load_hw_id_byte(i, &PROD_SIGS.WAFNUM); i += 2; + hw.id[i++] = '-'; + _load_hw_id_byte(i, &PROD_SIGS.COORDX1); i += 2; + _load_hw_id_byte(i, &PROD_SIGS.COORDX0); i += 2; + hw.id[i++] = '-'; + _load_hw_id_byte(i, &PROD_SIGS.COORDY1); i += 2; + _load_hw_id_byte(i, &PROD_SIGS.COORDY0); i += 2; + hw.id[i] = 0; } /// Lowest level hardware init void hardware_init() { clock_init(); // set system clock - _port_bindings(); rtc_init(); // real time counter -} + _read_hw_id(); - -/* Get a human readable device signature - * - * Produce a unique deviceID based on the factory calibration data. - * - * Format is: 123456-ABC - * - * The number part is a direct readout of the 6 digit lot number - * The alpha is the low 5 bits of wafer number and XY coords in printable ASCII - * Refer to NVM_PROD_SIGNATURES_t in iox192a3.h for details. - */ -enum { - LOTNUM0 = 8, // Lot Number Byte 0, ASCII - LOTNUM1, // Lot Number Byte 1, ASCII - LOTNUM2, // Lot Number Byte 2, ASCII - LOTNUM3, // Lot Number Byte 3, ASCII - LOTNUM4, // Lot Number Byte 4, ASCII - LOTNUM5, // Lot Number Byte 5, ASCII - WAFNUM = 16, // Wafer Number - COORDX0 = 18, // Wafer Coordinate X Byte 0 - COORDX1, // Wafer Coordinate X Byte 1 - COORDY0, // Wafer Coordinate Y Byte 0 - COORDY1, // Wafer Coordinate Y Byte 1 -}; - - -void hw_get_id(char *id) { - char printable[33] = "ABCDEFGHJKLMNPQRSTUVWXYZ23456789"; - uint8_t i; - - // Load NVM Command register to read the calibration row - NVM_CMD = NVM_CMD_READ_CALIB_ROW_gc; - - for (i = 0; i < 6; i++) - id[i] = pgm_read_byte(LOTNUM0 + i); - - id[i++] = '-'; - id[i++] = printable[(pgm_read_byte(WAFNUM) & 0x1F)]; - id[i++] = printable[(pgm_read_byte(COORDX0) & 0x1F)]; - id[i++] = printable[(pgm_read_byte(COORDY0) & 0x1F)]; - id[i] = 0; - - NVM_CMD = NVM_CMD_NO_OPERATION_gc; // Clean up NVM Command register + // Round-robin, interrupts in application section, all interupts levels + CCP = CCP_IOREG_gc; + PMIC.CTRL = + PMIC_RREN_bm | PMIC_HILVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_LOLVLEN_bm; } -void hw_request_hard_reset() {cs.hard_reset_requested = true;} +void hw_request_hard_reset() {hw.hard_reset_requested = true;} /// Hard reset using watchdog timer /// software hard reset using the watchdog timer void hw_hard_reset() { - wdt_enable(WDTO_15MS); - while (true) continue; // loops for about 15ms then resets + usart_flush(); + cli(); + CCP = CCP_IOREG_gc; + RST.CTRL = RST_SWRST_bm; } /// Controller's rest handler -stat_t hw_hard_reset_handler() { - if (!cs.hard_reset_requested) return STAT_NOOP; - hw_hard_reset(); // identical to hitting RESET button - return STAT_EAGAIN; +stat_t hw_reset_handler() { + if (hw.hard_reset_requested) hw_hard_reset(); + + if (hw.bootloader_requested) { + // TODO enable bootloader interrupt vectors and jump to BOOT_SECTION_START + hw.bootloader_requested = false; + } + + return STAT_NOOP; } /// Executes a software reset using CCPWrite -void hw_request_bootloader() {cs.bootloader_requested = true;} +void hw_request_bootloader() {hw.bootloader_requested = true;} -stat_t hw_bootloader_handler() { - if (!cs.bootloader_requested) return STAT_NOOP; +uint8_t hw_disable_watchdog() { + uint8_t state = WDT.CTRL; + wdt_disable(); + return state; +} + +void hw_restore_watchdog(uint8_t state) { cli(); - CCPWrite(&RST.CTRL, RST_SWRST_bm); // fire a software reset + CCP = CCP_IOREG_gc; + WDT.CTRL = state | WDT_CEN_bm; + sei(); +} + - return STAT_EAGAIN; // never gets here but keeps the compiler happy +const char *get_hw_id() { + return hw.id; } diff --git a/src/hardware.h b/src/hardware.h index 4e40e4a..7e510d8 100644 --- a/src/hardware.h +++ b/src/hardware.h @@ -30,31 +30,14 @@ #pragma once #include "status.h" -#include "config.h" - -/* - Device singleton - global structure to allow iteration through similar devices - - Ports are shared between steppers and GPIO so we need a global struct. - Each xmega port has 3 bindings; motors, switches and the output bit - - Care needs to be taken in routines that use ports not to write to bits that - are not assigned to the designated function - ur unpredicatable results will - occur. -*/ -typedef struct { - PORT_t *st_port[MOTORS]; // bindings for stepper motor ports (stepper.c) - PORT_t *sw_port[MOTORS]; // bindings for switch ports (GPIO2) - PORT_t *out_port[MOTORS]; // bindings for output ports (GPIO1) -} hwSingleton_t; -extern hwSingleton_t hw; void hardware_init(); -void hw_get_id(char *id); void hw_request_hard_reset(); void hw_hard_reset(); -stat_t hw_hard_reset_handler(); +stat_t hw_reset_handler(); void hw_request_bootloader(); -stat_t hw_bootloader_handler(); + +uint8_t hw_disable_watchdog(); +void hw_restore_watchdog(uint8_t state); diff --git a/src/main.c b/src/main.c index 7d59634..58357bc 100644 --- a/src/main.c +++ b/src/main.c @@ -28,16 +28,22 @@ \******************************************************************************/ #include "hardware.h" -#include "controller.h" #include "canonical_machine.h" #include "stepper.h" #include "motor.h" -#include "encoder.h" #include "switch.h" #include "pwm.h" #include "usart.h" #include "tmc2660.h" +#include "vars.h" +#include "rtc.h" +#include "report.h" +#include "command.h" + #include "plan/planner.h" +#include "plan/buffer.h" +#include "plan/arc.h" +#include "plan/feedhold.h" #include #include @@ -46,7 +52,86 @@ #include -static void init() { +static stat_t _shutdown_idler() { + // TODO send some notification out the serial port + + // EAGAIN prevents any lower-priority actions from running + return cm_get_machine_state() == MACHINE_SHUTDOWN ? STAT_EAGAIN : STAT_OK; +} + + +/// Return eagain if TX queue is backed up +static stat_t _sync_to_tx_buffer() { + return usart_tx_full() ? STAT_EAGAIN : STAT_OK; +} + + +/// Return eagain if planner is not ready for a new command +static stat_t _sync_to_planner() { + return mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM ? + STAT_EAGAIN : STAT_OK; +} + + +/// Shut down system if limit switch fired +static stat_t _limit_switch_handler() { + if (cm_get_machine_state() == MACHINE_ALARM) return STAT_NOOP; + if (!switch_get_limit_thrown()) return STAT_NOOP; + + return cm_hard_alarm(STAT_LIMIT_SWITCH_HIT); +} + + +/* Main loop + * + * The order of the dispatched tasks is very important. + * Tasks are ordered by increasing dependency (blocking hierarchy). + * Tasks that are dependent on completion of lower-level tasks must be + * later in the list than the task(s) they are dependent upon. + * + * Tasks must be written as continuations as they will be called repeatedly, + * and are called even if they are not currently active. + * + * The DISPATCH macro calls the function and returns to the caller + * if not finished (STAT_EAGAIN), preventing later routines from running + * (they remain blocked). Any other condition - OK or ERR - drops through + * and runs the next routine in the list. + * + * A routine that had no action (i.e. is OFF or idle) should return STAT_NOOP + */ +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_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(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. @@ -60,36 +145,27 @@ static void init() { tmc2660_init(); // motor drivers stepper_init(); // stepper subsystem motor_init(); - encoder_init(); // virtual encoders switch_init(); // switches pwm_init(); // pulse width modulation drivers - controller_init(); // must be first app init planner_init(); // motion planning subsystem canonical_machine_init(); // must follow config_init() - - // set vector location to application, as opposed to boot ROM - uint8_t temp = PMIC.CTRL & ~PMIC_IVSEL_bm; - CCP = CCP_IOREG_gc; - PMIC.CTRL = temp; - - // all levels are used, so don't bother to abstract them - PMIC.CTRL |= PMIC_HILVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_LOLVLEN_bm; + vars_init(); sei(); // enable global interrupts - fprintf_P(stderr, PSTR("Buildbotics firmware\n")); + fprintf_P(stderr, PSTR("\nBuildbotics firmware\n")); } int main() { wdt_enable(WDTO_250MS); - init(); + _init(); // main loop while (1) { - controller_run(); // single pass through the controller + _run(); // single pass wdt_reset(); } diff --git a/src/messages.def b/src/messages.def index fcb5585..355e7f6 100644 --- a/src/messages.def +++ b/src/messages.def @@ -46,7 +46,7 @@ MSG(14, "Buffer full - fatal") MSG(15, "Initializing") MSG(16, "Entering boot loader") MSG(17, "Function is stubbed") -MSG(18, "18") +MSG(18, "EEPROM data invalid") MSG(19, "19") MSG(20, "Internal error") diff --git a/src/motor.c b/src/motor.c index fa4ad17..c56c454 100644 --- a/src/motor.c +++ b/src/motor.c @@ -32,7 +32,6 @@ #include "rtc.h" #include "report.h" #include "stepper.h" -#include "encoder.h" #include "tmc2660.h" #include "plan/planner.h" @@ -46,9 +45,6 @@ #include -#define TIMER_CC_BM(x) CAT3(TC1_, x, EN_bm) - - typedef enum { MOTOR_IDLE, // motor stopped and may be partially energized MOTOR_ENERGIZING, @@ -64,12 +60,16 @@ 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; TC0_t *timer; + DMA_CH_t *dma; + uint8_t dma_trigger; // Runtime state motorPowerState_t power_state; // state machine for managing motor power uint32_t timeout; cmMotorFlags_t flags; + int32_t encoder; // Move prep uint8_t timer_clock; // clock divisor setting or zero for off @@ -85,62 +85,124 @@ typedef struct { static motor_t motors[MOTORS] = { { - .motor_map = M1_MOTOR_MAP, - .step_angle = M1_STEP_ANGLE, - .travel_rev = M1_TRAVEL_PER_REV, - .microsteps = M1_MICROSTEPS, - .polarity = M1_POLARITY, - .power_mode = M1_POWER_MODE, - .timer = (TC0_t *)&M1_TIMER, + .motor_map = M1_MOTOR_MAP, + .step_angle = M1_STEP_ANGLE, + .travel_rev = M1_TRAVEL_PER_REV, + .microsteps = M1_MICROSTEPS, + .polarity = M1_POLARITY, + .power_mode = M1_POWER_MODE, + .port = &PORT_MOTOR_1, + .timer = (TC0_t *)&M1_TIMER, + .dma = &M1_DMA_CH, + .dma_trigger = M1_DMA_TRIGGER, }, { - .motor_map = M2_MOTOR_MAP, - .step_angle = M2_STEP_ANGLE, - .travel_rev = M2_TRAVEL_PER_REV, - .microsteps = M2_MICROSTEPS, - .polarity = M2_POLARITY, - .power_mode = M2_POWER_MODE, - .timer = &M2_TIMER, + .motor_map = M2_MOTOR_MAP, + .step_angle = M2_STEP_ANGLE, + .travel_rev = M2_TRAVEL_PER_REV, + .microsteps = M2_MICROSTEPS, + .polarity = M2_POLARITY, + .power_mode = M2_POWER_MODE, + .port = &PORT_MOTOR_2, + .timer = &M2_TIMER, + .dma = &M2_DMA_CH, + .dma_trigger = M2_DMA_TRIGGER, }, { - .motor_map = M3_MOTOR_MAP, - .step_angle = M3_STEP_ANGLE, - .travel_rev = M3_TRAVEL_PER_REV, - .microsteps = M3_MICROSTEPS, - .polarity = M3_POLARITY, - .power_mode = M3_POWER_MODE, - .timer = &M3_TIMER, + .motor_map = M3_MOTOR_MAP, + .step_angle = M3_STEP_ANGLE, + .travel_rev = M3_TRAVEL_PER_REV, + .microsteps = M3_MICROSTEPS, + .polarity = M3_POLARITY, + .power_mode = M3_POWER_MODE, + .port = &PORT_MOTOR_3, + .timer = &M3_TIMER, + .dma = &M3_DMA_CH, + .dma_trigger = M3_DMA_TRIGGER, }, { - .motor_map = M4_MOTOR_MAP, - .step_angle = M4_STEP_ANGLE, - .travel_rev = M4_TRAVEL_PER_REV, - .microsteps = M4_MICROSTEPS, - .polarity = M4_POLARITY, - .power_mode = M4_POWER_MODE, - .timer = &M4_TIMER, + .motor_map = M4_MOTOR_MAP, + .step_angle = M4_STEP_ANGLE, + .travel_rev = M4_TRAVEL_PER_REV, + .microsteps = M4_MICROSTEPS, + .polarity = M4_POLARITY, + .power_mode = M4_POWER_MODE, + .port = &PORT_MOTOR_4, + .timer = &M4_TIMER, + .dma = &M4_DMA_CH, + .dma_trigger = M4_DMA_TRIGGER, } }; +static uint8_t _dummy; + + /// Special interrupt for X-axis ISR(TCE1_CCA_vect) { PORT_MOTOR_1.OUTTGL = STEP_BIT_bm; } +ISR(DMA_CH0_vect) { + M1_TIMER.CTRLA = 0; // Top motor clock +} + + +ISR(DMA_CH1_vect) { + M2_TIMER.CTRLA = 0; // Top motor clock +} + + +ISR(DMA_CH2_vect) { + M3_TIMER.CTRLA = 0; // Top motor clock +} + + +ISR(DMA_CH3_vect) { + M4_TIMER.CTRLA = 0; // Top motor clock +} + + void motor_init() { // Reset position mp_set_steps_to_runtime_position(); - // Setup motor timers - M1_TIMER.CTRLB = TC_WGMODE_FRQ_gc | TIMER_CC_BM(M1_TIMER_CC); - M2_TIMER.CTRLB = TC_WGMODE_FRQ_gc | TIMER_CC_BM(M2_TIMER_CC); - M3_TIMER.CTRLB = TC_WGMODE_FRQ_gc | TIMER_CC_BM(M3_TIMER_CC); - M4_TIMER.CTRLB = TC_WGMODE_FRQ_gc | TIMER_CC_BM(M4_TIMER_CC); + // Enable DMA + DMA.CTRL = DMA_RESET_bm; + DMA.CTRL = DMA_ENABLE_bm; + + for (int motor = 0; motor < MOTORS; motor++) { + motor_t *m = &motors[motor]; + + // Setup motor timer + m->timer->CTRLB = TC_WGMODE_FRQ_gc | TC1_CCAEN_bm; + + // 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; + 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->CTRLA = + DMA_CH_ENABLE_bm | DMA_CH_SINGLE_bm | DMA_CH_BURSTLEN_1BYTE_gc; + } // Setup special interrupt for X-axis mapping M1_TIMER.INTCTRLB = TC_CCAINTLVL_HI_gc; } +void motor_shutdown(int motor) { + motors[motor].port->OUTSET = MOTOR_ENABLE_BIT_bm; // Disable +} + + int motor_get_axis(int motor) { return motors[motor].motor_map; } @@ -152,6 +214,16 @@ int motor_get_steps_per_unit(int motor) { } +int32_t motor_get_encoder(int motor) { + return motors[motor].encoder; +} + + +void motor_set_encoder(int motor, float encoder) { + motors[motor].encoder = round(encoder); +} + + /// returns true if motor is in an error state static bool _error(int motor) { return motors[motor].flags & MOTOR_FLAG_ERROR_bm; @@ -263,7 +335,7 @@ void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps, // negative drift. uint16_t steps = round(fabs(travel_steps)); // TODO why do we need to multiply by 2 here? - uint32_t ticks_per_step = seg_clocks / (steps + 0.5) * 2; + uint32_t ticks_per_step = seg_clocks / steps * 2; // Find the clock rate that will fit the required number of steps if (ticks_per_step & 0xffff0000UL) { @@ -297,14 +369,11 @@ void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps, void motor_begin_move(int motor) { - motor_t *m = &motors[motor]; - - // Energize motor - switch (m->power_mode) { + switch (motors[motor].power_mode) { case MOTOR_DISABLED: return; case MOTOR_POWERED_ONLY_WHEN_MOVING: - if (!m->timer_clock) return; // Not moving + if (!motors[motor].timer_clock) return; // Not moving // Fall through case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE: @@ -319,27 +388,28 @@ void motor_begin_move(int motor) { void motor_load_move(int motor) { motor_t *m = &motors[motor]; + // Setup DMA count + m->dma->TRFCNT = m->steps < 0 ? -m->steps : m->steps; + // Set or zero runtime clock and period - m->timer->CTRLFCLR = TC0_DIR_bm; // Count up - m->timer->CNT = 0; // Start at zero - m->timer->CCA = m->timer_period; // Set frequency - m->timer->CTRLA = m->timer_clock; // Start or stop - m->timer_clock = 0; // Clear clock + m->timer->CNT = 0; // Start at zero + m->timer->CCA = m->timer_period; // Set frequency + m->timer->CTRLA = m->timer_clock; // Start or stop + m->timer_clock = 0; // Clear clock // Set direction - if (m->direction == DIRECTION_CW) - hw.st_port[motor]->OUTCLR = DIRECTION_BIT_bm; - else hw.st_port[motor]->OUTSET = DIRECTION_BIT_bm; + if (m->direction == DIRECTION_CW) m->port->OUTCLR = DIRECTION_BIT_bm; + else m->port->OUTSET = DIRECTION_BIT_bm; // Accumulate encoder - en[motor].encoder_steps += m->steps; + m->encoder += m->steps; m->steps = 0; } void motor_end_move(int motor) { // Disable motor clock - motors[motor].timer->CTRLA = 0; + //motors[motor].timer->CTRLA = 0; } diff --git a/src/motor.h b/src/motor.h index 0db5282..5482be2 100644 --- a/src/motor.h +++ b/src/motor.h @@ -63,9 +63,12 @@ typedef enum { void motor_init(); +void motor_shutdown(int motor); int motor_get_axis(int motor); int motor_get_steps_per_unit(int motor); +int32_t motor_get_encoder(int motor); +void motor_set_encoder(int motor, float encoder); bool motor_energizing(); diff --git a/src/plan/exec.c b/src/plan/exec.c index 1bb6106..a35494c 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -31,7 +31,7 @@ #include "buffer.h" #include "kinematics.h" #include "stepper.h" -#include "encoder.h" +#include "motor.h" #include "util.h" #include "report.h" @@ -49,9 +49,9 @@ static stat_t _exec_aline_segment(); /// Dequeues buffer and executes move continuations. Manages run buffers and /// other details. stat_t mp_exec_move() { - mpBuf_t *bf; + mpBuf_t *bf = mp_get_run_buffer(); - if (!(bf = mp_get_run_buffer())) { // null if nothing's running + if (!bf) { // null if nothing's running st_prep_null(); return STAT_NOOP; } @@ -777,7 +777,7 @@ static stat_t _exec_aline_segment() { // previous segment's target becomes position mr.position_steps[i] = mr.target_steps[i]; // current encoder position (aligns to commanded_steps) - mr.encoder_steps[i] = en_read_encoder(i); + mr.encoder_steps[i] = motor_get_encoder(i); mr.following_error[i] = mr.encoder_steps[i] - mr.commanded_steps[i]; } diff --git a/src/plan/feedhold.c b/src/plan/feedhold.c index 266d053..0b6458b 100644 --- a/src/plan/feedhold.c +++ b/src/plan/feedhold.c @@ -124,8 +124,8 @@ stat_t mp_plan_hold_callback() { if (cm.hold_state != FEEDHOLD_PLAN) return STAT_NOOP; // not planning a feedhold - mpBuf_t *bp; // working buffer pointer - if (!(bp = mp_get_run_buffer())) return STAT_NOOP; // Oops! nothing's running + mpBuf_t *bp = mp_get_run_buffer(); // working buffer pointer + if (!bp) return STAT_NOOP; // Oops! nothing's running uint8_t mr_flag = true; // used to tell replan to account for mr buffer Vx float mr_available_length; // length left in mr buffer for deceleration diff --git a/src/plan/jog.c b/src/plan/jog.c index 3bc3fa3..206f7c8 100644 --- a/src/plan/jog.c +++ b/src/plan/jog.c @@ -33,7 +33,7 @@ #include "motor.h" #include "canonical_machine.h" #include "kinematics.h" -#include "encoder.h" +#include "motor.h" #include #include @@ -88,7 +88,7 @@ static stat_t _exec_jog(mpBuf_t *bf) { // Update machine position for (int motor = 0; motor < MOTORS; motor++) { int axis = motor_get_axis(motor); - float steps = en_read_encoder(axis); + float steps = motor_get_encoder(axis); cm_set_position(axis, steps / motor_get_steps_per_unit(motor)); } diff --git a/src/plan/line.c b/src/plan/line.c index bf2658a..5007e7f 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -33,7 +33,6 @@ #include "exec.h" #include "buffer.h" #include "zoid.h" -#include "controller.h" #include "canonical_machine.h" #include "stepper.h" #include "util.h" @@ -299,7 +298,6 @@ static void _calc_move_times(GCodeState_t *gms, const float axis_length[], * the minimums. */ stat_t mp_aline(GCodeState_t *gm_in) { - mpBuf_t *bf; // current move pointer float exact_stop = 0; // preset this value OFF float junction_velocity; uint8_t mr_flag = false; @@ -343,7 +341,8 @@ stat_t mp_aline(GCodeState_t *gm_in) { float delta_velocity = pow(length, 0.66666666) * mm.cbrt_jerk; float entry_velocity = 0; // pre-set as if no previous block - if ((bf = mp_get_run_buffer())) { + mpBuf_t *bf = mp_get_run_buffer(); + if (bf) { if (bf->replannable) // not optimally planned entry_velocity = bf->entry_velocity + bf->delta_vmax; else entry_velocity = bf->exit_velocity; // optimally planned @@ -355,8 +354,8 @@ stat_t mp_aline(GCodeState_t *gm_in) { } // Get a cleared buffer and setup move variables - if (!(bf = mp_get_write_buffer())) - return cm_hard_alarm(STAT_BUFFER_FULL_FATAL); // never supposed to fail + mpBuf_t *bf = mp_get_write_buffer(); // current move pointer + if (!bf) return cm_hard_alarm(STAT_BUFFER_FULL_FATAL); // never fails // Register callback to exec function bf->bf_func = mp_exec_aline; @@ -374,17 +373,17 @@ stat_t mp_aline(GCodeState_t *gm_in) { // // The math for time-to-decelerate T from speed S to speed E with constant // jerk J is: - // T = 2*sqrt((S-E)/J[n]) + // T = 2 * sqrt((S - E) / J[n]) // // Since E is always zero in this calculation, we can simplify: - // T = 2*sqrt(S/J[n]) + // T = 2 * sqrt(S / J[n]) // // The math for distance-to-decelerate l from speed S to speed E with // constant jerk J is: - // l = (S+E)*sqrt((S-E)/J) + // l = (S + E) * sqrt((S - E) / J) // // Since E is always zero in this calculation, we can simplify: - // l = S*sqrt(S/J) + // l = S * sqrt(S / J) // // The final value we want is to know which one is *longest*, // compared to the others, so we don't need the actual value. This @@ -405,22 +404,22 @@ stat_t mp_aline(GCodeState_t *gm_in) { // L = total length of the move in all axes // C[n] = "axis contribution" of axis n // - // For each axis n: C[n] = sqrt(1/J[n]) * (D[n]/L) + // For each axis n: C[n] = sqrt(1 / J[n]) * (D[n] / L) // // Keeping in mind that we only need a rank compared to the other // axes, we can further optimize the calculations:: // // Square the expression to remove the square root: - // C[n]^2 = (1/J[n]) * (D[n]/L)^2 (We don't care the C is squared, - // we'll use it that way.) + // C[n]^2 = (1 / J[n]) * (D[n] / L)^2 (We don't care the C is squared, + // we'll use it that way.) // // Re-arranged to optimize divides for pre-calculated values, // we create a value M that we compute once: - // M = (1/(L^2)) + // M = 1 / L^2 // Then we use it in the calculations for every axis: - // C[n] = (1/J[n]) * D[n]^2 * M + // C[n] = 1 / J[n] * D[n]^2 * M // - // Also note that we already have (1/J[n]) calculated for each axis, + // Also note that we already have 1 / J[n] calculated for each axis, // which simplifies it further. // // Finally, the selected jerk term needs to be scaled by the diff --git a/src/plan/planner.c b/src/plan/planner.c index dbbb4d1..296bf6b 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -64,7 +64,7 @@ #include "canonical_machine.h" #include "kinematics.h" #include "stepper.h" -#include "encoder.h" +#include "motor.h" #include #include @@ -135,7 +135,7 @@ void mp_set_steps_to_runtime_position() { mr.commanded_steps[motor] = step_position[motor]; // write steps to encoder register - en_set_encoder_steps(motor, step_position[motor]); + motor_set_encoder(motor, step_position[motor]); // must be zero mr.following_error[motor] = 0; diff --git a/src/status.h b/src/status.h index 9203c8a..2b2993e 100644 --- a/src/status.h +++ b/src/status.h @@ -85,6 +85,7 @@ void print_status_message(const char *msg, stat_t status); #define STAT_INITIALIZING 15 // initializing - not ready for use #define STAT_ENTERING_BOOT_LOADER 16 // emitted from boot loader #define STAT_FUNCTION_IS_STUBBED 17 +#define STAT_EEPROM_DATA_INVALID 18 // Internal errors and startup messages #define STAT_INTERNAL_ERROR 20 // unrecoverable internal error diff --git a/src/stepper.c b/src/stepper.c index 07a2176..c3321a5 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -67,6 +67,12 @@ void stepper_init() { } +void st_shutdown() { + for (int motor = 0; motor < MOTORS; motor++) + motor_shutdown(motor); +} + + /// Return true if motors or dwell are running uint8_t st_runtime_isbusy() {return st.busy;} @@ -92,14 +98,9 @@ ISR(STEP_TIMER_ISR) { if (st.dwell && --st.dwell) return; // End last move - if (st.busy) { - for (int motor = 0; motor < MOTORS; motor++) - motor_end_move(motor); - - st.busy = false; - } + st.busy = false; - // If there are no more moves + // If there are no more moves try to load one if (!st.move_ready) { TIMER_STEP.PER = STEP_TIMER_POLL; mp_exec_move(); @@ -116,9 +117,6 @@ ISR(STEP_TIMER_ISR) { return; } - // Start dwell - st.dwell = st.prep_dwell; - // Start move if (st.seg_period) { for (int motor = 0; motor < MOTORS; motor++) @@ -128,6 +126,9 @@ ISR(STEP_TIMER_ISR) { st.busy = true; } + // Start dwell + st.dwell = st.prep_dwell; + // Execute command if (st.move_type == MOVE_TYPE_COMMAND) mp_runtime_command(st.bf); @@ -173,7 +174,7 @@ stat_t st_prep_line(float travel_steps[], float error[], float seg_time) { uint32_t seg_clocks = (uint32_t)st.seg_period * STEP_TIMER_DIV; // Prepare motor moves - for (uint8_t motor = 0; motor < MOTORS; motor++) + for (int motor = 0; motor < MOTORS; motor++) motor_prep_move(motor, seg_clocks, travel_steps[motor], error[motor]); st.move_ready = true; // signal prep buffer ready(do this last) @@ -191,10 +192,10 @@ void st_prep_null() { /// Stage command to execution -void st_prep_command(void *bf) { +void st_prep_command(mpBuf_t *bf) { if (st.move_ready) cm_hard_alarm(STAT_INTERNAL_ERROR); st.move_type = MOVE_TYPE_COMMAND; - st.bf = (mpBuf_t *)bf; + st.bf = bf; st.move_ready = true; // signal prep buffer ready } diff --git a/src/stepper.h b/src/stepper.h index eafe195..bcd318b 100644 --- a/src/stepper.h +++ b/src/stepper.h @@ -30,14 +30,16 @@ #pragma once #include "status.h" +#include "plan/buffer.h" #include void stepper_init(); +void st_shutdown(); uint8_t st_runtime_isbusy(); stat_t st_prep_line(float travel_steps[], float following_error[], float segment_time); void st_prep_null(); -void st_prep_command(void *bf); // void * since mpBuf_t is not visible here +void st_prep_command(mpBuf_t *bf); void st_prep_dwell(float seconds); diff --git a/src/switch.c b/src/switch.c index e165baa..9bf5dd0 100644 --- a/src/switch.c +++ b/src/switch.c @@ -77,6 +77,8 @@ typedef struct { swMode_t mode; swDebounce_t debounce; // debounce state int8_t count; // deglitching and lockout counter + PORT_t *port; + bool min; } switch_t; /* Switch control structures @@ -91,30 +93,62 @@ typedef struct { swSingleton_t sw = { .switches = { - {.type = SWITCH_TYPE, .mode = X_SWITCH_MODE_MIN, .debounce = SW_IDLE}, - {.type = SWITCH_TYPE, .mode = X_SWITCH_MODE_MAX, .debounce = SW_IDLE}, - {.type = SWITCH_TYPE, .mode = Y_SWITCH_MODE_MIN, .debounce = SW_IDLE}, - {.type = SWITCH_TYPE, .mode = Y_SWITCH_MODE_MAX, .debounce = SW_IDLE}, - {.type = SWITCH_TYPE, .mode = Z_SWITCH_MODE_MIN, .debounce = SW_IDLE}, - {.type = SWITCH_TYPE, .mode = Z_SWITCH_MODE_MAX, .debounce = SW_IDLE}, - {.type = SWITCH_TYPE, .mode = A_SWITCH_MODE_MIN, .debounce = SW_IDLE}, - {.type = SWITCH_TYPE, .mode = A_SWITCH_MODE_MAX, .debounce = SW_IDLE}, + { + .type = SWITCH_TYPE, + .mode = X_SWITCH_MODE_MIN, + .debounce = SW_IDLE, + .port = &PORT_SWITCH_X, + .min = true, + }, { + .type = SWITCH_TYPE, + .mode = X_SWITCH_MODE_MAX, + .debounce = SW_IDLE, + .port = &PORT_SWITCH_X, + .min = false, + }, { + .type = SWITCH_TYPE, + .mode = Y_SWITCH_MODE_MIN, + .debounce = SW_IDLE, + .port = &PORT_SWITCH_Y, + .min = true, + }, { + .type = SWITCH_TYPE, + .mode = Y_SWITCH_MODE_MAX, + .debounce = SW_IDLE, + .port = &PORT_SWITCH_Y, + .min = false, + }, { + .type = SWITCH_TYPE, + .mode = Z_SWITCH_MODE_MIN, + .debounce = SW_IDLE, + .port = &PORT_SWITCH_Z, + .min = true, + }, { + .type = SWITCH_TYPE, + .mode = Z_SWITCH_MODE_MAX, + .debounce = SW_IDLE, + .port = &PORT_SWITCH_Z, + .min = false, + }, { + .type = SWITCH_TYPE, + .mode = A_SWITCH_MODE_MIN, + .debounce = SW_IDLE, + .port = &PORT_SWITCH_A, + .min = true, + }, { + .type = SWITCH_TYPE, + .mode = A_SWITCH_MODE_MAX, + .debounce = SW_IDLE, + .port = &PORT_SWITCH_A, + .min = false, + }, } }; -static bool _read_switch(uint8_t sw_num) { - switch (sw_num) { - case SW_MIN_X: return hw.sw_port[AXIS_X]->IN & SW_MIN_BIT_bm; - case SW_MAX_X: return hw.sw_port[AXIS_X]->IN & SW_MAX_BIT_bm; - case SW_MIN_Y: return hw.sw_port[AXIS_Y]->IN & SW_MIN_BIT_bm; - case SW_MAX_Y: return hw.sw_port[AXIS_Y]->IN & SW_MAX_BIT_bm; - case SW_MIN_Z: return hw.sw_port[AXIS_Z]->IN & SW_MIN_BIT_bm; - case SW_MAX_Z: return hw.sw_port[AXIS_Z]->IN & SW_MAX_BIT_bm; - case SW_MIN_A: return hw.sw_port[AXIS_A]->IN & SW_MIN_BIT_bm; - case SW_MAX_A: return hw.sw_port[AXIS_A]->IN & SW_MAX_BIT_bm; - default: return false; - } +static bool _read_switch(uint8_t i) { + return sw.switches[i].port->IN & + (sw.switches[i].min ? SW_MIN_BIT_bm : SW_MAX_BIT_bm); } @@ -147,27 +181,21 @@ ISR(A_SWITCH_ISR_vect) {_switch_isr();} void switch_init() { - for (int i = 0; i < SWITCHES / 2; i++) { - // setup input bits and interrupts (previously set to inputs by st_init()) - if (sw.switches[MIN_SWITCH(i)].mode != SW_MODE_DISABLED) { - hw.sw_port[i]->DIRCLR = SW_MIN_BIT_bm; // set min input - see 13.14.14 - hw.sw_port[i]->PIN6CTRL = PORT_OPC_PULLUP_gc | PORT_ISC_BOTHEDGES_gc; - hw.sw_port[i]->INT0MASK |= SW_MIN_BIT_bm; // min on INT0 - } + for (int i = 0; i < SWITCHES; i++) { + switch_t *s = &sw.switches[i]; + PORT_t *port = s->port; + uint8_t bm = s->min ? SW_MIN_BIT_bm : SW_MAX_BIT_bm; - if (sw.switches[MAX_SWITCH(i)].mode != SW_MODE_DISABLED) { - hw.sw_port[i]->DIRCLR = SW_MAX_BIT_bm; // set max input - see 13.14.14 - hw.sw_port[i]->PIN7CTRL = PORT_OPC_PULLUP_gc | PORT_ISC_BOTHEDGES_gc; - hw.sw_port[i]->INT0MASK |= SW_MAX_BIT_bm; // max on INT0 - } + if (s->mode == SW_MODE_DISABLED) continue; - // set interrupt levels. Interrupts must be enabled in main() - hw.sw_port[i]->INTCTRL |= SWITCH_INTLVL; - } + port->DIRCLR = bm; // See 13.14.14 + port->INT0MASK |= bm; // Enable INT0 + port->INTCTRL |= SWITCH_INTLVL; // Set interrupt level - // Initialize state - for (int i = 0; i < SWITCHES; i++) { - switch_t *s = &sw.switches[i]; + if (s->min) port->PIN6CTRL = PORT_OPC_PULLUP_gc | PORT_ISC_BOTHEDGES_gc; + else port->PIN7CTRL = PORT_OPC_PULLUP_gc | PORT_ISC_BOTHEDGES_gc; + + // Initialize state s->state = (s->type == SW_TYPE_NORMALLY_OPEN) ^ _read_switch(i); } } @@ -210,28 +238,28 @@ void switch_rtc_callback() { } -bool switch_get_closed(uint8_t n) { - return sw.switches[n].state; +bool switch_get_closed(int index) { + return sw.switches[index].state; } -swType_t switch_get_type(uint8_t n) { - return sw.switches[n].type; +swType_t switch_get_type(int index) { + return sw.switches[index].type; } -void switch_set_type(uint8_t n, swType_t type) { - sw.switches[n].type = type; +void switch_set_type(int index, swType_t type) { + sw.switches[index].type = type; } -swMode_t switch_get_mode(uint8_t n) { - return sw.switches[n].mode; +swMode_t switch_get_mode(int index) { + return sw.switches[index].mode; } -void switch_set_mode(uint8_t n, swMode_t mode) { - sw.switches[n].mode = mode; +void switch_set_mode(int index, swMode_t mode) { + sw.switches[index].mode = mode; } diff --git a/src/switch.h b/src/switch.h index e5bedbb..42ffd3f 100644 --- a/src/switch.h +++ b/src/switch.h @@ -65,9 +65,9 @@ typedef enum { void switch_init(); void switch_rtc_callback(); -bool switch_get_closed(uint8_t n); -swType_t switch_get_type(uint8_t n); -void switch_set_type(uint8_t n, swType_t type); -swMode_t switch_get_mode(uint8_t n); -void switch_set_mode(uint8_t n, swMode_t mode); +bool switch_get_closed(int index); +swType_t switch_get_type(int index); +void switch_set_type(int index, swType_t type); +swMode_t switch_get_mode(int index); +void switch_set_mode(int index, swMode_t mode); bool switch_get_limit_thrown(); diff --git a/src/tmc2660.c b/src/tmc2660.c index 90d9baf..0623587 100644 --- a/src/tmc2660.c +++ b/src/tmc2660.c @@ -28,7 +28,6 @@ #include "tmc2660.h" #include "status.h" #include "motor.h" -#include "hardware.h" #include "rtc.h" #include "cpp_magic.h" @@ -67,6 +66,8 @@ typedef struct { uint16_t sguard; uint8_t flags; uint32_t regs[5]; + + PORT_t *port; } tmc2660_driver_t; @@ -79,7 +80,13 @@ static const uint32_t reg_addrs[] = { }; -static tmc2660_driver_t drivers[MOTORS]; +static tmc2660_driver_t drivers[MOTORS] = { + {.port = &PORT_MOTOR_1}, + {.port = &PORT_MOTOR_2}, + {.port = &PORT_MOTOR_3}, + {.port = &PORT_MOTOR_4}, +}; + typedef struct { volatile uint8_t driver; @@ -89,13 +96,15 @@ typedef struct { volatile uint32_t in; } spi_t; -static spi_t spi; +static spi_t spi = {}; static void _report_error_flags(int driver) { - if (drivers[driver].stabilizing < rtc_get_time()) return; + tmc2660_driver_t *drv = &drivers[driver]; - uint8_t dflags = drivers[driver].flags; + if (drv->stabilizing < rtc_get_time()) return; + + uint8_t dflags = drv->flags; uint8_t mflags = 0; if ((TMC2660_DRVSTATUS_SHORT_TO_GND_A | TMC2660_DRVSTATUS_SHORT_TO_GND_B) & @@ -104,19 +113,17 @@ static void _report_error_flags(int driver) { if (TMC2660_DRVSTATUS_OVERTEMP_WARN & dflags) mflags |= MOTOR_FLAG_OVERTEMP_WARN_bm; - if (TMC2660_DRVSTATUS_OVERTEMP & dflags) - mflags |= MOTOR_FLAG_OVERTEMP_bm; + if (TMC2660_DRVSTATUS_OVERTEMP & dflags) mflags |= MOTOR_FLAG_OVERTEMP_bm; - if (hw.st_port[driver]->IN & FAULT_BIT_bm) - mflags |= MOTOR_FLAG_STALLED_bm; + if (drv->port->IN & FAULT_BIT_bm) mflags |= MOTOR_FLAG_STALLED_bm; if (mflags) motor_error_callback(driver, mflags); } -static void spi_cs(int motor, int enable) { - if (enable) hw.st_port[motor]->OUTCLR = CHIP_SELECT_BIT_bm; - else hw.st_port[motor]->OUTSET = CHIP_SELECT_BIT_bm; +static void spi_cs(int driver, int enable) { + if (enable) drivers[driver].port->OUTCLR = CHIP_SELECT_BIT_bm; + else drivers[driver].port->OUTSET = CHIP_SELECT_BIT_bm; } @@ -184,7 +191,7 @@ void spi_next() { drv->reg = 0; drv->stabilizing = rtc_get_time() + TMC2660_STABILIZE_TIME * 1000; drv->callback = true; - hw.st_port[spi.driver]->OUTCLR = MOTOR_ENABLE_BIT_bm; // Enable + drv->port->OUTCLR = MOTOR_ENABLE_BIT_bm; // Enable } break; @@ -198,7 +205,7 @@ void spi_next() { DACB.CH0DATA = drv->sguard << 2; } - if (drv->stabilizing < rtc_get_time() && drv->callback) { + if (drv->callback && drv->stabilizing < rtc_get_time()) { motor_driver_callback(spi.driver); drv->callback = false; } @@ -254,10 +261,6 @@ ISR(PORT_4_FAULT_ISR_vect) {_fault_isr(3);} void tmc2660_init() { - // Reset state - memset(&spi, 0, sizeof(spi)); - memset(drivers, 0, sizeof(drivers)); - // Configure motors for (int i = 0; i < MOTORS; i++) { drivers[i].state = TMC2660_STATE_CONFIG; @@ -308,14 +311,16 @@ void tmc2660_init() { TMC2660_SPI_PORT.OUTSET = 1 << TMC2660_SPI_MOSI_PIN; // High TMC2660_SPI_PORT.DIRSET = 1 << TMC2660_SPI_MOSI_PIN; // Output - for (int motor = 0; motor < MOTORS; motor++) { - hw.st_port[motor]->OUTSET = CHIP_SELECT_BIT_bm; // High - hw.st_port[motor]->OUTSET = MOTOR_ENABLE_BIT_bm; // High (disabled) - hw.st_port[motor]->DIR = MOTOR_PORT_DIR_gm; // Pin directions + for (int driver = 0; driver < MOTORS; driver++) { + PORT_t *port = drivers[driver].port; + + port->OUTSET = CHIP_SELECT_BIT_bm; // High + port->OUTSET = MOTOR_ENABLE_BIT_bm; // High (disabled) + port->DIR = MOTOR_PORT_DIR_gm; // Pin directions - hw.st_port[motor]->PIN4CTRL = PORT_ISC_RISING_gc; - hw.st_port[motor]->INT1MASK = FAULT_BIT_bm; // INT1 - hw.st_port[motor]->INTCTRL |= PORT_INT1LVL_HI_gc; + port->PIN4CTRL = PORT_ISC_RISING_gc; + port->INT1MASK = FAULT_BIT_bm; // INT1 + port->INTCTRL |= PORT_INT1LVL_HI_gc; } // Configure SPI diff --git a/src/usart.c b/src/usart.c index 5e44046..1d42620 100644 --- a/src/usart.c +++ b/src/usart.c @@ -149,7 +149,7 @@ void usart_set_baud(int baud) { } -void usart_ctrl(int flag, int enable) { +void usart_ctrl(int flag, bool enable) { if (enable) usart_flags |= flag; else usart_flags &= ~flag; } @@ -164,7 +164,7 @@ static void usart_sleep() { void usart_putc(char c) { - while (tx_buf_full()) usart_sleep(); + while (tx_buf_full() | (usart_flags & USART_FLUSH)) usart_sleep(); tx_buf_push(c); @@ -230,6 +230,15 @@ int16_t usart_peek() { } +void usart_flush() { + usart_ctrl(USART_FLUSH, true); + + while (!tx_buf_empty() || !(USARTC0.STATUS & USART_DREIF_bm) || + !(USARTC0.STATUS & USART_TXCIF_bm)) + usart_sleep(); +} + + void usart_rx_flush() { rx_buf_init(); } diff --git a/src/usart.h b/src/usart.h index acd6842..cd5640b 100644 --- a/src/usart.h +++ b/src/usart.h @@ -29,6 +29,7 @@ #include +#include #define USART_TX_RING_BUF_SIZE 256 #define USART_RX_RING_BUF_SIZE 256 @@ -47,27 +48,29 @@ enum { }; enum { - USART_CRLF = (1 << 0), - USART_ECHO = (1 << 1), - USART_XOFF = (1 << 2), + USART_CRLF = (1 << 0), + USART_ECHO = (1 << 1), + USART_XOFF = (1 << 2), + USART_FLUSH = (1 << 3), }; void usart_init(); void usart_set_baud(int baud); -void usart_ctrl(int flag, int enable); +void usart_ctrl(int flag, bool enable); void usart_putc(char c); void usart_puts(const char *s); int8_t usart_getc(); char *usart_readline(); int16_t usart_peek(); +void usart_flush(); void usart_rx_flush(); int16_t usart_rx_fill(); int16_t usart_rx_space(); -inline int usart_rx_empty() {return !usart_rx_fill();} -inline int usart_rx_full() {return !usart_rx_space();} +inline bool usart_rx_empty() {return !usart_rx_fill();} +inline bool usart_rx_full() {return !usart_rx_space();} int16_t usart_tx_fill(); int16_t usart_tx_space(); -inline int usart_tx_empty() {return !usart_tx_fill();} -inline int usart_tx_full() {return !usart_tx_space();} +inline bool usart_tx_empty() {return !usart_tx_fill();} +inline bool usart_tx_full() {return !usart_tx_space();} diff --git a/src/vars.c b/src/vars.c index 18c9a25..c29a33b 100644 --- a/src/vars.c +++ b/src/vars.c @@ -29,6 +29,7 @@ #include "cpp_magic.h" #include "status.h" +#include "hardware.h" #include "config.h" #include @@ -40,16 +41,29 @@ #include #include -#include +#include typedef uint8_t flags_t; +typedef const char *string; // Type names static const char bool_name [] PROGMEM = ""; #define TYPE_NAME(TYPE) static const char TYPE##_name [] PROGMEM = "<" #TYPE ">" -MAP(TYPE_NAME, SEMI, flags_t, float, int8_t, uint8_t, uint16_t); +MAP(TYPE_NAME, SEMI, flags_t, string, float, int8_t, uint8_t, uint16_t); + + +static void var_print_string(const char *s) { + printf_P(PSTR("\"%s\""), s); +} + + +#if 0 +static void var_print_bool(bool x) { + printf_P(x ? PSTR("true") : PSTR("false")); +} +#endif extern void print_status_flags(uint8_t x); @@ -99,6 +113,13 @@ static float var_parse_float(const char *value) { } +#if 0 +static bool var_parse_bool(const char *value) { + return !strcasecmp(value, "true") || var_parse_float(value); +} +#endif + + static int8_t var_parse_int8_t(const char *value) { return strtol(value, 0, 0); } @@ -113,6 +134,36 @@ static uint16_t var_parse_uint16_t(const char *value) { } +static int8_t eeprom_read_int8_t(int8_t *addr) { + return eeprom_read_byte((uint8_t *)addr); +} + + +static uint8_t eeprom_read_uint8_t(uint8_t *addr) { + return eeprom_read_byte(addr); +} + + +static uint16_t eeprom_read_uint16_t(uint16_t *addr) { + return eeprom_read_word(addr); +} + + +static void eeprom_update_int8_t(int8_t *addr, int8_t value) { + eeprom_update_byte((uint8_t *)addr, value); +} + + +static void eeprom_update_uint8_t(uint8_t *addr, uint8_t value) { + eeprom_update_byte(addr, value); +} + + +static void eeprom_update_uint16_t(uint16_t *addr, uint16_t value) { + eeprom_update_word(addr, value); +} + + // Var forward declarations #define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \ TYPE get_##NAME(IF(INDEX)(int index)); \ @@ -125,27 +176,37 @@ static uint16_t var_parse_uint16_t(const char *value) { // Var names, count & help #define VAR(NAME, CODE, TYPE, INDEX, SET, DEFAULT, HELP) \ static const char NAME##_name[] PROGMEM = #NAME; \ - static const int NAME##_count = INDEX ? INDEX : 1; \ static const char NAME##_help[] PROGMEM = HELP; #include "vars.def" #undef VAR +// EEPROM storage +#define VAR(NAME, CODE, TYPE, INDEX, SET, DEFAULT, HELP) \ + IF(SET) \ + (static TYPE NAME##_eeprom IF(INDEX)([INDEX]) EEMEM;) + +#include "vars.def" +#undef VAR + +static uint16_t eeprom_crc EEMEM; + // Last value #define VAR(NAME, CODE, TYPE, INDEX, ...) \ - static TYPE NAME##_last IF(INDEX)([INDEX]); + static TYPE NAME##_state IF(INDEX)([INDEX]); #include "vars.def" #undef VAR + void vars_init() { -#define VAR(NAME, CODE, TYPE, INDEX, SET, DEFAULT, ...) \ - IF(SET) \ - (IF(INDEX)(for (int i = 0; i < INDEX; i++)) { \ - set_##NAME(IF(INDEX)(i,) DEFAULT); \ - (NAME##_last)IF(INDEX)([i]) = DEFAULT; \ - }) + vars_restore(); + + // Initialize var state +#define VAR(NAME, CODE, TYPE, INDEX, SET, DEFAULT, ...) \ + IF(INDEX)(for (int i = 0; i < INDEX; i++)) \ + (NAME##_state)IF(INDEX)([i]) = get_##NAME(IF(INDEX)(i)); #include "vars.def" #undef VAR @@ -153,37 +214,42 @@ void vars_init() { void vars_report(bool full) { + // Save and disable watchdog + uint8_t wd_state = hw_disable_watchdog(); + bool reported = false; static const char value_fmt[] PROGMEM = "\"%s\":"; static const char index_value_fmt[] PROGMEM = "\"%c%s\":"; -#define VAR(NAME, CODE, TYPE, INDEX, ...) \ - IF(INDEX)(for (int i = 0; i < NAME##_count; i++)) { \ - TYPE value = get_##NAME(IF(INDEX)(i)); \ - TYPE last = (NAME##_last)IF(INDEX)([i]); \ - \ - if (value != last || full) { \ - (NAME##_last)IF(INDEX)([i]) = value; \ - \ - if (!reported) { \ - reported = true; \ - putchar('{'); \ - } else putchar(','); \ - \ - printf_P \ - (IF_ELSE(INDEX)(index_value_fmt, value_fmt), \ - IF(INDEX)(INDEX##_LABEL[i],) CODE); \ - \ - var_print_##TYPE(value); \ - } \ - wdt_reset(); \ +#define VAR(NAME, CODE, TYPE, INDEX, ...) \ + IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \ + TYPE value = get_##NAME(IF(INDEX)(i)); \ + TYPE last = (NAME##_state)IF(INDEX)([i]); \ + \ + if (value != last || full) { \ + (NAME##_state)IF(INDEX)([i]) = value; \ + \ + if (!reported) { \ + reported = true; \ + putchar('{'); \ + } else putchar(','); \ + \ + printf_P \ + (IF_ELSE(INDEX)(index_value_fmt, value_fmt), \ + IF(INDEX)(INDEX##_LABEL[i],) CODE); \ + \ + var_print_##TYPE(value); \ + } \ } #include "vars.def" #undef VAR if (reported) printf("}\n"); + + // Restore watchdog + hw_restore_watchdog(wd_state); } @@ -247,7 +313,9 @@ bool vars_set(const char *name, const char *value) { (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \ if (INDEX <= i) return false); \ \ - set_##NAME(IF(INDEX)(i ,) var_parse_##TYPE(value)); \ + TYPE x = var_parse_##TYPE(value); \ + set_##NAME(IF(INDEX)(i,) x); \ + NAME##_state IF(INDEX)([i]) = x; \ \ return true; \ }) \ @@ -296,9 +364,101 @@ int vars_parser(char *vars) { void vars_print_help() { static const char fmt[] PROGMEM = " $%-5s %-14S %-12S %S\n"; -#define VAR(NAME, CODE, TYPE, ...) \ - printf_P(fmt, CODE, NAME##_name, TYPE##_name, NAME##_help); \ - wdt_reset(); + // Save and disable watchdog + uint8_t wd_state = hw_disable_watchdog(); + +#define VAR(NAME, CODE, TYPE, ...) \ + printf_P(fmt, CODE, NAME##_name, TYPE##_name, NAME##_help); #include "vars.def" #undef VAR + + // Restore watchdog + hw_restore_watchdog(wd_state); +} + + +uint16_t vars_crc() { + // Save and disable watchdog + uint8_t wd_state = hw_disable_watchdog(); + + CRC.CTRL = CRC_RESET_RESET0_gc; + CRC.CTRL = CRC_SOURCE_IO_gc; // Must be after reset + +#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \ + IF(SET) \ + ({ \ + for (int j = 0; ; j++) { \ + char c = pgm_read_byte(&NAME##_name[j]); \ + if (!c) break; \ + CRC.DATAIN = c; \ + } \ + \ + CRC.DATAIN = INDEX; \ + }) + +#include "vars.def" +#undef VAR + + // Restore watchdog + hw_restore_watchdog(wd_state); + + CRC.STATUS = CRC_BUSY_bm; // Done + return CRC.CHECKSUM1 << 8 | CRC.CHECKSUM0; +} + + +void vars_save() { + // Save and disable watchdog + uint8_t wd_state = hw_disable_watchdog(); + +#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \ + IF(SET) \ + (IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \ + TYPE value = get_##NAME(IF(INDEX)(i)); \ + eeprom_update_##TYPE(&NAME##_eeprom IF(INDEX)([i]), value); \ + }) \ + +#include "vars.def" +#undef VAR + + // Restore watchdog + hw_restore_watchdog(wd_state); + + // Save CRC + eeprom_update_word(&eeprom_crc, vars_crc()); +} + + + +bool vars_valid() { + return eeprom_read_word(&eeprom_crc) == vars_crc(); +} + + +stat_t vars_restore() { + if (!vars_valid()) return STAT_EEPROM_DATA_INVALID; + + // Save and disable watchdog + uint8_t wd_state = hw_disable_watchdog(); + +#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \ + IF(SET) \ + (IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \ + TYPE value = eeprom_read_##TYPE(&NAME##_eeprom IF(INDEX)([i])); \ + set_##NAME(IF(INDEX)(i,) value); \ + NAME##_state IF(INDEX)([i]) = value; \ + }) \ + +#include "vars.def" +#undef VAR + + // Restore watchdog + hw_restore_watchdog(wd_state); + + return STAT_OK; +} + + +void vars_clear() { + eeprom_update_word(&eeprom_crc, -1); } diff --git a/src/vars.def b/src/vars.def index 2403167..a8e7295 100644 --- a/src/vars.def +++ b/src/vars.def @@ -78,3 +78,4 @@ VAR(switch_type, "sw", uint8_t, SWITCHES, 1, 0, "Normally open or closed") // System VAR(velocity, "v", float, 0, 0, 0, "Current velocity") +VAR(hw_id, "id", string, 0, 0, 0, "Hardware ID") diff --git a/src/vars.h b/src/vars.h index 4e02216..e918e91 100644 --- a/src/vars.h +++ b/src/vars.h @@ -27,12 +27,21 @@ #pragma once +#include "status.h" + #include + void vars_init(); + void vars_report(bool full); int vars_find(const char *name); bool vars_print(const char *name); bool vars_set(const char *name, const char *value); int vars_parser(char *vars); void vars_print_help(); + +void vars_save(); +bool vars_valid(); +stat_t vars_restore(); +void vars_clear(); -- 2.27.0