Verison 1.0.3 Release

Based on Buildbotics 0.4.14

src/avr/.gitignore

@@ -0,0 +1,14 @@
+# Backup files
+*~
+\#*
+
+build
+.dep
+
+/*.eep
+/*.hex
+/*.elf
+/*.lss
+/*.map
+
+*.o

src/avr/BezierMath.md

@@ -0,0 +1,56 @@
+# Cubic Bezier
+
+    f(x) = A(1 - x)^3 + 3B(1 - x)^2 x + 3C(1 - x) x^2 + Dx^3
+
+     -Ax^3 + 3Ax^2 - 3Ax + A
+     3Bx^3 - 6Bx^2 + 3Bx
+    -3Cx^3 + 3Cx^2
+      Dx^3
+
+
+    f(x) = (-A + 3B -3C + D)x^3 + (3A - 6B + 3C)x^2 + (-3A + 3B)x + A
+
+      a =  -A + 3B - 3C + D
+      b =  3A - 6B + 3C
+      c = -3A + 3B
+      d =   A
+
+    f(x) = ax^3 + bx^2 + cx + d
+
+    integral f(x) dx = a/4 x^4 + b/3 x^3 + c/2 x^2 + dx + E
+
+    = (-A + 3B - 3C + D)/4 x^4 + (A - 2B + B) x^3 + 3/2 (B - A) x^2 + Ax + E
+
+# Quintic Bezier
+
+    A(1 - x)^5 + 5A(1 - x)^4 x + 10A(1 - x)^3 x^2 + 10B(1 - x)^2 x^3 +
+    5B(1 - x)x^4 + Bx^5
+
+    (-6A + 6B)x^5 + (15A - 15B)x^4 + (-10A + 10B)x^3 + A
+
+    6(B - A)x^5 + 15(A - B)x^4 + 10(B - A)x^3 + A
+
+    x^3 (6(B - A)x^2 + 15(A - B)x + 10(B - A)) + A
+
+    a =   6(B - A)
+    b = -15(B - A)
+    c =  10(B - A)
+    d = A
+
+    f(x) = ax^5 + bx^4 + cx^3 + d
+
+    f(x) = (ax^2 + bx + c)x^3 + d
+
+
+    integral f(x) = a/6 x^6 + b/5 x^5 + c/4 x^4 + dx + e
+
+    = (B - A)x^6 - 3(B - A)x^5 + 5/2(B - A)x^4 + Ax + e
+
+    = (B - A)x^4 (x^2 - 3x + 5/2) + Ax + e
+
+    A = 0
+    B = 1
+    e = 0
+
+    f(x) = 6x^5 -15x^4 + 10x^3
+    int f(x) dx = x^6 - 3x^5 + 5/2x^4 + C

src/avr/Makefile

@@ -0,0 +1,40 @@
+# Makefile for the project Bulidbotics firmware
+PROJECT  = bbctrl-avr-firmware
+MCU      = atxmega192a3u
+CLOCK    = 32000000
+
+# SRC
+SRC = $(wildcard src/*.c) $(wildcard src/*.cpp) $(wildcard src/vfd/*.c)
+OBJ := $(patsubst src/%.c,build/%.o,$(SRC))
+OBJ := $(patsubst src/%.cpp,build/%.o,$(OBJ))
+OBJ := $(patsubst src/vfd/%.c,build/vfd/%.o,$(OBJ))
+JSON = vars command messages
+JSON := $(patsubst %,build/%.json,$(JSON))
+
+all: $(PROJECT).hex $(JSON) size
+
+include Makefile.common
+
+CFLAGS += -Isrc
+
+# Build
+$(PROJECT).elf: $(OBJ)
+	$(CC) $(LDFLAGS) $(OBJ) $(LIBS) -o $@
+
+
+# JSON
+build/%.json: src/%.json.in src/%.def
+	cpp -Isrc $< | sed "/^#.*$$/d;s/'\(.\)'/\"\1\"/g" > $@
+
+# Program
+init:
+	$(MAKE) erase
+	-$(MAKE) fuses
+	$(MAKE) fuses
+	$(MAKE) program-boot
+	$(MAKE) program
+
+program-boot:
+	$(MAKE) -C ../boot program
+
+.PHONY: all init program-boot

src/avr/Makefile.common

@@ -0,0 +1,115 @@
+# Compile flags
+CC = avr-g++
+
+COMMON = -mmcu=$(MCU) -flto -fwhole-program
+
+CFLAGS += $(COMMON)
+CFLAGS += -Wall -Werror
+CFLAGS += -Wno-error=strict-aliasing # for _invsqrt
+CFLAGS += -std=gnu++98 -DF_CPU=$(CLOCK)UL -O3
+CFLAGS += -funsigned-bitfields -fpack-struct -fshort-enums -funsigned-char
+CFLAGS += -MD -MP -MT $@ -MF build/dep/$(@F).d
+CFLAGS += -D__STDC_LIMIT_MACROS
+
+# Linker flags
+LDFLAGS += $(COMMON) -Wl,-u,vfprintf -lprintf_flt -lm
+LIBS += -lm
+
+# EEPROM flags
+EEFLAGS += -j .eeprom
+EEFLAGS += --set-section-flags=.eeprom="alloc,load"
+EEFLAGS += --change-section-lma .eeprom=0 --no-change-warnings
+
+# Programming flags
+ifndef (PROGRAMMER)
+PROGRAMMER = avrispmkII
+#PROGRAMMER = jtag3pdi
+endif
+PDEV = usb
+AVRDUDE_OPTS = -c $(PROGRAMMER) -p $(MCU) -P $(PDEV)
+
+FUSE0=0xff
+FUSE1=0x00
+FUSE2=0xbe
+FUSE4=0xff
+FUSE5=0xeb
+
+# Compile
+build/%.o: src/%.c
+	@mkdir -p $(shell dirname $@)
+	$(CC) $(CFLAGS) -c -o $@ $<
+
+build/%.o: src/%.cpp
+	@mkdir -p $(shell dirname $@)
+	$(CC) $(CFLAGS) -c -o $@ $<
+
+build/%.o: src/%.S
+	@mkdir -p $(shell dirname $@)
+	$(CC) $(CFLAGS) -c -o $@ $<
+
+# Link
+%.hex: %.elf
+	avr-objcopy -O ihex -R .eeprom -R .fuse -R .lock -R .signature $< $@
+
+%.eep: %.elf
+	avr-objcopy $(EEFLAGS) -O ihex $< $@
+
+%.lss: %.elf
+	avr-objdump -h -S $< > $@
+
+size: $(PROJECT).elf
+	@for X in A B C; do\
+	  echo '****************************************************************' ;\
+	  avr-size -$$X --mcu=$(MCU) $(PROJECT).elf ;\
+	done
+
+data-usage: $(PROJECT).elf
+	avr-nm -CS --size-sort -t decimal $(PROJECT).elf | grep ' [BbDd] '
+
+
+prog-usage: $(PROJECT).elf
+	avr-nm -CS --size-sort -t decimal $(PROJECT).elf | grep -v ' [BbDd] '
+
+# Program
+reset:
+	avrdude $(AVRDUDE_OPTS)
+
+erase:
+	avrdude $(AVRDUDE_OPTS) -e
+
+program: $(PROJECT).hex
+	avrdude $(AVRDUDE_OPTS) -U flash:w:$(PROJECT).hex:i
+
+verify: $(PROJECT).hex
+	avrdude $(AVRDUDE_OPTS) -U flash:v:$(PROJECT).hex:i
+
+fuses:
+	avrdude $(AVRDUDE_OPTS) -U fuse0:w:$(FUSE0):m -U fuse1:w:$(FUSE1):m \
+	  -U fuse2:w:$(FUSE2):m -U fuse4:w:$(FUSE4):m -U fuse5:w:$(FUSE5):m
+
+read_fuses:
+	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:
+	rm -f $(shell find -name \*~ -o -name \#\*)
+
+clean: tidy
+	rm -rf $(PROJECT).elf $(PROJECT).hex $(PROJECT).eep $(PROJECT).lss \
+	  $(PROJECT).map build
+
+.PHONY: tidy clean size reset erase program fuses read_fuses prodsig
+.PHONY: signature usersig data-usage prog-usage
+
+# Dependencies
+-include $(shell mkdir -p build/dep) $(wildcard build/dep/*)

src/avr/README.md

@@ -0,0 +1,18 @@
+The Buildbotics firmware is a 4 axis motion control system designed for
+high-performance on small to mid-sized machines.  It operates in conjunction
+with the Buildbotics RaspberryPi firmware.
+
+# Build Instructions
+To build in Linux run:
+
+    make
+
+Other make commands are:
+
+ * **size** - Display program and data sizes
+ * **program** - program using AVR dude and an avrispmkII
+ * **erase** - Erase chip
+ * **fuses** - Write AVR fuses bytes
+ * **read_fuses** - Read and print AVR fuse bytes
+ * **clean** - Remove build files
+ * **tidy** - Remove backup files

src/avr/data_usage.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+
+################################################################################
+#                                                                              #
+#                This file is part of the Buildbotics firmware.                #
+#                                                                              #
+#                  Copyright (c) 2015 - 2018, Buildbotics LLC                  #
+#                             All rights reserved.                             #
+#                                                                              #
+#     This file ("the software") is free software: you can redistribute it     #
+#     and/or modify it under the terms of the GNU General Public License,      #
+#      version 2 as published by the Free Software Foundation. You should      #
+#      have received a copy of the GNU General Public License, version 2       #
+#     along with the software. If not, see <http://www.gnu.org/licenses/>.     #
+#                                                                              #
+#     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                 #
+#                       <http://www.gnu.org/licenses/>.                        #
+#                                                                              #
+#                For information regarding this software email:                #
+#                  "Joseph Coffland" <joseph@buildbotics.com>                  #
+#                                                                              #
+################################################################################
+
+import os
+import re
+import shlex
+import subprocess
+
+
+lineRE = r'%(addr)s '
+command = 'avr-objdump -j .bss -t buildbotics.elf'
+
+proc = subprocess.Popen(shlex.split(command), stdout = subprocess.PIPE)
+
+out, err = proc.communicate()
+
+if proc.returncode:
+    print(out)
+    raise Exception('command failed')
+
+def get_sizes(data):
+    for line in data.decode().split('\n'):
+        if not re.match(r'^[0-9a-f]{8} .*', line): continue
+
+        size, name  = int(line[22:30], 16), line[31:]
+        if not size: continue
+
+        yield (size, name)
+
+sizes = sorted(get_sizes(out))
+total = sum(x[0] for x in sizes)
+
+for size, name in sizes:
+    print('% 6d %5.2f%% %s' % (size, size / total * 100, name))
+
+print('-' * 40)
+print('% 6d Total' % total)

src/avr/emu/.gitignore

@@ -0,0 +1 @@
+bbemu

src/avr/emu/Makefile

@@ -0,0 +1,38 @@
+TARGET = bbemu
+
+SRC:=$(wildcard ../src/*.c) $(wildcard ../src/*.cpp)
+OBJ:=$(patsubst %.cpp,%.o,$(patsubst %.c,%.o,$(SRC)))
+OBJ:=$(patsubst ../src/%,build/%,$(OBJ))
+SRC+=src/emu.c
+OBJ+=build/emu.o
+
+CFLAGS = -I../src -Isrc -Wall -Werror -DDEBUG -g -std=gnu++98
+CFLAGS += -MD -MP -MT $@ -MF build/$(@F).d
+CFLAGS += -DF_CPU=32000000 -Wno-class-memaccess -pthread
+LDFLAGS = -lm -pthread
+
+all: $(TARGET)
+
+$(TARGET): $(OBJ)
+	g++ -o $@ $(OBJ) $(LDFLAGS)
+
+build/%.o: ../src/%.c
+	g++ -c -o $@ $(CFLAGS) $<
+
+build/%.o: src/%.c
+	g++ -c -o $@ $(CFLAGS) $<
+
+build/%.o: ../src/%.cpp
+	g++ -c -o $@ $(CFLAGS) $<
+
+# Clean
+tidy:
+	rm -f $(shell find -name \*~ -o -name \#\*)
+
+clean: tidy
+	rm -rf $(TARGET) build
+
+.PHONY: tidy clean all
+
+# Dependencies
+-include $(shell mkdir -p build) $(wildcard build/*.d)

src/avr/emu/src/avr/eeprom.h

@@ -0,0 +1,34 @@
+/******************************************************************************\
+
+                  This file is part of the Buildbotics firmware.
+
+                    Copyright (c) 2015 - 2018, Buildbotics LLC
+                               All rights reserved.
+
+       This file ("the software") is free software: you can redistribute it
+       and/or modify it under the terms of the GNU General Public License,
+        version 2 as published by the Free Software Foundation. You should
+        have received a copy of the GNU General Public License, version 2
+       along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+       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
+                         <http://www.gnu.org/licenses/>.
+
+                  For information regarding this software email:
+                    "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#define EEMEM
+
+#define eeprom_update_word(PTR, VAL) *(PTR) = (VAL)
+#define eeprom_read_word(PTR) *(PTR)
+#define eeprom_is_ready() true

src/avr/emu/src/avr/interrupt.h

@@ -0,0 +1,35 @@
+/******************************************************************************\
+
+                  This file is part of the Buildbotics firmware.
+
+                    Copyright (c) 2015 - 2018, Buildbotics LLC
+                               All rights reserved.
+
+       This file ("the software") is free software: you can redistribute it
+       and/or modify it under the terms of the GNU General Public License,
+        version 2 as published by the Free Software Foundation. You should
+        have received a copy of the GNU General Public License, version 2
+       along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+       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
+                         <http://www.gnu.org/licenses/>.
+
+                  For information regarding this software email:
+                    "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "io.h"
+
+void cli();
+void sei();
+
+#define ISR(X) void __##X()

src/avr/emu/src/avr/pgmspace.h

@@ -0,0 +1,41 @@
+/******************************************************************************\
+
+                  This file is part of the Buildbotics firmware.
+
+                    Copyright (c) 2015 - 2018, Buildbotics LLC
+                               All rights reserved.
+
+       This file ("the software") is free software: you can redistribute it
+       and/or modify it under the terms of the GNU General Public License,
+        version 2 as published by the Free Software Foundation. You should
+        have received a copy of the GNU General Public License, version 2
+       along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+       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
+                         <http://www.gnu.org/licenses/>.
+
+                  For information regarding this software email:
+                    "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#define PRPSTR "s"
+#define PROGMEM
+#define PGM_P const char *
+#define PSTR(X) X
+#define vfprintf_P vfprintf
+#define printf_P printf
+#define puts_P puts
+#define sprintf_P sprintf
+#define strcmp_P strcmp
+#define pgm_read_ptr(x) *(x)
+#define pgm_read_word(x) *(x)
+#define pgm_read_byte(x) *(x)

src/avr/emu/src/avr/wdt.h

@@ -0,0 +1,33 @@
+/******************************************************************************\
+
+                  This file is part of the Buildbotics firmware.
+
+                    Copyright (c) 2015 - 2018, Buildbotics LLC
+                               All rights reserved.
+
+       This file ("the software") is free software: you can redistribute it
+       and/or modify it under the terms of the GNU General Public License,
+        version 2 as published by the Free Software Foundation. You should
+        have received a copy of the GNU General Public License, version 2
+       along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+       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
+                         <http://www.gnu.org/licenses/>.
+
+                  For information regarding this software email:
+                    "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#define WDTO_250MS 0
+#define wdt_enable(...)
+#define wdt_disable()
+#define wdt_reset()

src/avr/emu/src/emu.c

@@ -0,0 +1,163 @@
+/******************************************************************************\
+
+                  This file is part of the Buildbotics firmware.
+
+                    Copyright (c) 2015 - 2018, Buildbotics LLC
+                               All rights reserved.
+
+       This file ("the software") is free software: you can redistribute it
+       and/or modify it under the terms of the GNU General Public License,
+        version 2 as published by the Free Software Foundation. You should
+        have received a copy of the GNU General Public License, version 2
+       along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+       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
+                         <http://www.gnu.org/licenses/>.
+
+                  For information regarding this software email:
+                    "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include <config.h>
+
+#include <avr/io.h>
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/select.h>
+
+
+void __SPIC_INT_vect();      // DRV8711 SPI
+void __I2C_ISR();            // I2C from RPi
+void __ADCA_CH0_vect();      // Analog input
+void __ADCA_CH1_vect();      // Analog input
+void __RS485_DRE_vect();     // RS848
+void __RS485_TXC_vect();     // RS848
+void __RS485_RXC_vect();     // RS848
+void __SERIAL_DRE_vect();    // Serial to RPi
+void __SERIAL_RXC_vect();    // Serial from RPi
+void __STEP_LOW_LEVEL_ISR(); // Stepper lo interrupt
+void __STEP_TIMER_ISR();     // Stepper hi interrupt
+void __RTC_OVF_vect();       // RTC tick
+
+void motor_emulate_steps(int motor);
+
+extern int __argc;
+extern char **__argv;
+
+
+volatile uint8_t io_mem[4096] = {0};
+
+
+bool fast = false;
+int serialByte = -1;
+uint8_t i2cData[I2C_MAX_DATA];
+int i2cIndex = 0;
+bool haveI2C = false;
+fd_set readFDs;
+
+
+void cli() {}
+void sei() {}
+
+
+void emu_init() {
+  // Parse command line args
+  for (int i = 0; i < __argc; i++)
+    if (strcmp(__argv[i], "--fast") == 0) fast = true;
+
+  // Mark clocks ready
+  OSC.STATUS = OSC_XOSCRDY_bm | OSC_PLLRDY_bm | OSC_RC32KRDY_bm;
+
+  // So usart_flush() returns
+  SERIAL_PORT.STATUS = USART_DREIF_bm | USART_TXCIF_bm;
+
+  // Clear motor fault
+  PIN_PORT(MOTOR_FAULT_PIN)->IN |= PIN_BM(MOTOR_FAULT_PIN);
+
+  FD_ZERO(&readFDs);
+}
+
+
+void emu_callback() {
+  fflush(stdout);
+
+  if (RST.CTRL == RST_SWRST_bm) exit(0);
+
+  struct timeval t = {0, fast ? 0 : 1000};
+  bool readData = true;
+  while (readData) {
+    readData = false;
+
+    // Try to read
+    FD_SET(0, &readFDs);
+    if (fcntl(3, F_GETFL) != -1) FD_SET(3, &readFDs);
+
+    if (0 < select(4, &readFDs, 0, 0, &t)) {
+      uint8_t data;
+
+      if (serialByte == -1 && FD_ISSET(0, &readFDs) && read(0, &data, 1) == 1)
+        serialByte = data;
+
+      if (!haveI2C && FD_ISSET(3, &readFDs) && read(3, &data, 1) == 1) {
+        if (data == '\n') haveI2C = true;
+        else if (i2cIndex < I2C_MAX_DATA) i2cData[i2cIndex++] = data;
+      }
+    }
+
+    // Send message to i2c port
+    if (haveI2C && (I2C_DEV.SLAVE.CTRLA & TWI_SLAVE_INTLVL_LO_gc)) {
+      // START
+      I2C_DEV.SLAVE.STATUS = TWI_SLAVE_APIF_bm | TWI_SLAVE_AP_bm;
+      __I2C_ISR();
+
+      // DATA
+      for (int i = 0; i < i2cIndex; i++) {
+        I2C_DEV.SLAVE.STATUS = TWI_SLAVE_DIF_bm;
+        I2C_DEV.SLAVE.DATA = i2cData[i];
+        __I2C_ISR();
+      }
+
+      // STOP
+      I2C_DEV.SLAVE.STATUS = TWI_SLAVE_APIF_bm;
+      __I2C_ISR();
+
+      i2cIndex = 0;
+      haveI2C = false;
+      readData = true;
+    }
+
+    // Send byte to serial port
+    if (serialByte != -1 && SERIAL_PORT.CTRLA & USART_RXCINTLVL_MED_gc) {
+      SERIAL_PORT.DATA = (uint8_t)serialByte;
+      __SERIAL_RXC_vect();
+
+      if (SERIAL_PORT.CTRLA & USART_RXCINTLVL_MED_gc) {
+        serialByte = -1;
+        readData = true;
+      }
+    }
+  }
+
+  // Call stepper ISRs
+  if (ADCB_CH0_INTCTRL == ADC_CH_INTLVL_LO_gc) __STEP_LOW_LEVEL_ISR();
+  for (int motor = 0; motor < 4; motor++) motor_emulate_steps(motor);
+  __STEP_TIMER_ISR();
+
+  // Call RTC
+  __RTC_OVF_vect();
+
+  // Throttle with remaining time
+  if (t.tv_usec) usleep(t.tv_usec);
+}

src/avr/emu/src/util/atomic.h

@@ -0,0 +1,31 @@
+/******************************************************************************\
+
+                  This file is part of the Buildbotics firmware.
+
+                    Copyright (c) 2015 - 2018, Buildbotics LLC
+                               All rights reserved.
+
+       This file ("the software") is free software: you can redistribute it
+       and/or modify it under the terms of the GNU General Public License,
+        version 2 as published by the Free Software Foundation. You should
+        have received a copy of the GNU General Public License, version 2
+       along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+       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
+                         <http://www.gnu.org/licenses/>.
+
+                  For information regarding this software email:
+                    "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#define ATOMIC_BLOCK(x)
+#define ATOMIC_RESTORESTATE

src/avr/emu/src/util/crc16.h

@@ -0,0 +1,30 @@
+/******************************************************************************\
+
+                  This file is part of the Buildbotics firmware.
+
+                    Copyright (c) 2015 - 2018, Buildbotics LLC
+                               All rights reserved.
+
+       This file ("the software") is free software: you can redistribute it
+       and/or modify it under the terms of the GNU General Public License,
+        version 2 as published by the Free Software Foundation. You should
+        have received a copy of the GNU General Public License, version 2
+       along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+       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
+                         <http://www.gnu.org/licenses/>.
+
+                  For information regarding this software email:
+                    "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#define _crc16_update(...) 0

src/avr/emu/src/util/delay.h

@@ -0,0 +1,33 @@
+/******************************************************************************\
+
+                  This file is part of the Buildbotics firmware.
+
+                    Copyright (c) 2015 - 2018, Buildbotics LLC
+                               All rights reserved.
+
+       This file ("the software") is free software: you can redistribute it
+       and/or modify it under the terms of the GNU General Public License,
+        version 2 as published by the Free Software Foundation. You should
+        have received a copy of the GNU General Public License, version 2
+       along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+       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
+                         <http://www.gnu.org/licenses/>.
+
+                  For information regarding this software email:
+                    "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include <unistd.h>
+
+#define _delay_ms(x) usleep((x) * 1000)
+#define _delay_us(x) usleep(x)

src/avr/src/SCurve.cpp

@@ -0,0 +1,175 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+
+#include "SCurve.h"
+
+#include <math.h>
+
+
+SCurve::SCurve(float maxV, float maxA, float maxJ) :
+  maxV(maxV), maxA(maxA), maxJ(maxJ), v(0), a(0), j(0) {}
+
+
+unsigned SCurve::getPhase() const {
+  if (!v) return 0;
+
+  // Handle negative velocity
+  float v = this->v;
+  float a = this->a;
+  if (v < 0) {
+    v = -v;
+    a = -a;
+  }
+
+  if (0 < a) {
+    if (0 < j) return 1;
+    if (!j) return 2;
+    return 3;
+  }
+
+  if (!a) return 4;
+  if (j < 0) return 5;
+  if (!j) return 6;
+
+  return 7;
+}
+
+
+float SCurve::getStoppingDist() const {return stoppingDist(v, a, maxA, maxJ);}
+
+
+float SCurve::next(float t, float targetV) {
+  // Compute next acceleration
+  float nextA = nextAccel(t, targetV, v, a, maxA, maxJ);
+
+  // Compute next velocity
+  float deltaV = nextA * t;
+  if ((deltaV < 0 && targetV < v && v + deltaV < targetV) ||
+      (0 < deltaV && v < targetV && targetV < v + deltaV)) {
+    nextA = (targetV - v) / t;
+    v = targetV;
+
+  } else v += deltaV;
+
+  // Compute jerk = delta accel / time
+  j = (nextA - a) / t;
+  a = nextA;
+
+  return v;
+}
+
+
+float SCurve::stoppingDist(float v, float a, float maxA, float maxJ) {
+  // Already stopped
+  if (!v) return 0;
+
+  // Handle negative velocity
+  if (v < 0) {
+    v = -v;
+    a = -a;
+  }
+
+  float d = 0;
+
+  // Compute distance and velocity change to accel = 0
+  if (0 < a) {
+    // Compute distance to decrease accel to zero
+    float t = a / maxJ;
+    d += distance(t, v, a, -maxJ);
+    v += velocity(t, a, -maxJ);
+    a = 0;
+  }
+
+  // Compute max deccel
+  float maxDeccel = -sqrt(v * maxJ + 0.5 * a * a);
+  if (maxDeccel < -maxA) maxDeccel = -maxA;
+
+  // Compute distance and velocity change to max deccel
+  if (maxDeccel < a) {
+    float t = (a - maxDeccel) / maxJ;
+    d += distance(t, v, a, -maxJ);
+    v += velocity(t, a, -maxJ);
+    a = maxDeccel;
+  }
+
+  // Compute velocity change over remaining accel
+  float deltaV = 0.5 * a * a / maxJ;
+
+  // Compute constant deccel period
+  if (deltaV < v) {
+    float t = (v - deltaV) / -a;
+    d += distance(t, v, a, 0);
+    v += velocity(t, a, 0);
+  }
+
+  // Compute distance to zero vel
+  d += distance(-a / maxJ, v, a, maxJ);
+
+  return d;
+}
+
+
+float SCurve::nextAccel(float t, float targetV, float v, float a, float maxA,
+                        float maxJ) {
+  bool increasing = v < targetV;
+  float deltaA = acceleration(t, maxJ);
+
+  if (increasing && a < -deltaA)
+    return a + deltaA; // negative accel, increasing speed
+
+  if (!increasing && deltaA < a)
+    return a - deltaA; // positive accel, decreasing speed
+
+  float deltaV = fabs(targetV - v);
+  float targetA = sqrt(2 * deltaV * maxJ);
+  if (maxA < targetA) targetA = maxA;
+
+  if (increasing) {
+    if (targetA < a + deltaA) return targetA;
+    return a + deltaA;
+
+  } else {
+    if (a - deltaA < -targetA) return -targetA;
+    return a - deltaA;
+  }
+}
+
+
+float SCurve::distance(float t, float v, float a, float j) {
+  // v * t + 1/2 * a * t^2 + 1/6 * j * t^3
+  return t * (v + t * (0.5 * a + 1.0 / 6.0 * j * t));
+}
+
+
+float SCurve::velocity(float t, float a, float j) {
+  // a * t + 1/2 * j * t^2
+  return t * (a + 0.5 * j * t);
+}
+
+
+float SCurve::acceleration(float t, float j) {return j * t;}

src/avr/src/SCurve.h

@@ -0,0 +1,66 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include <math.h>
+
+
+class SCurve {
+  float maxV;
+  float maxA;
+  float maxJ;
+
+  float v;
+  float a;
+  float j;
+
+public:
+  SCurve(float maxV = 0, float maxA = 0, float maxJ = 0);
+
+  float getMaxVelocity() const {return maxV;}
+  void setMaxVelocity(float v) {maxV = v;}
+  float getMaxAcceleration() const {return maxA;}
+  void setMaxAcceleration(float a) {maxA = a;}
+  float getMaxJerk() const {return maxJ;}
+  void setMaxJerk(float j) {maxJ = j;}
+
+  float getVelocity() const {return v;}
+  float getAcceleration() const {return a;}
+  float getJerk() const {return j;}
+
+  unsigned getPhase() const;
+  float getStoppingDist() const;
+  float next(float t, float targetV);
+
+  static float stoppingDist(float v, float a, float maxA, float maxJ);
+  static float nextAccel(float t, float targetV, float v, float a, float maxA,
+                         float maxJ);
+  static float distance(float t, float v, float a, float j);
+  static float velocity(float t, float a, float j);
+  static float acceleration(float t, float j);
+};

src/avr/src/analog.c

@@ -0,0 +1,90 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "analog.h"
+
+#include "config.h"
+
+#include <avr/interrupt.h>
+
+#include <stdint.h>
+
+
+typedef struct {
+  uint8_t pin;
+  uint16_t value;
+} analog_port_t;
+
+
+analog_port_t ports[] = {
+  {.pin = ANALOG_1_PIN},
+  {.pin = ANALOG_2_PIN},
+};
+
+
+ISR(ADCA_CH0_vect) {ports[0].value = ADCA.CH0.RES;}
+ISR(ADCA_CH1_vect) {ports[1].value = ADCA.CH1.RES;}
+
+
+void analog_init() {
+  // Channel 0
+  ADCA.CH0.CTRL = ADC_CH_GAIN_1X_gc | ADC_CH_INPUTMODE_SINGLEENDED_gc;
+  ADCA.CH0.MUXCTRL = ADC_CH_MUXPOS_PIN6_gc;
+  ADCA.CH0.INTCTRL = ADC_CH_INTLVL_LO_gc;
+
+  // Channel 1
+  ADCA.CH1.CTRL = ADC_CH_GAIN_1X_gc | ADC_CH_INPUTMODE_SINGLEENDED_gc;
+  ADCA.CH1.MUXCTRL = ADC_CH_MUXPOS_PIN7_gc;
+  ADCA.CH1.INTCTRL = ADC_CH_INTLVL_LO_gc;
+
+  // ADC
+  ADCA.REFCTRL = ADC_REFSEL_INTVCC_gc; // 3.3V / 1.6 = 2.06V
+  ADCA.PRESCALER = ADC_PRESCALER_DIV512_gc;
+  ADCA.EVCTRL = ADC_SWEEP_01_gc;
+  ADCA.CTRLA = ADC_FLUSH_bm | ADC_ENABLE_bm;
+}
+
+
+float analog_get(unsigned port) {
+  if (1 < port) return 0;
+  return ports[port].value * (1.0 / 0x1000);
+}
+
+
+void analog_rtc_callback() {
+  static uint8_t count = 0;
+
+  // Every 1/4 sec
+  if (++count == 250) {
+    count = 0;
+    ADCA.CTRLA |= ADC_CH0START_bm | ADC_CH1START_bm;
+  }
+}
+
+
+// Var callbacks
+float get_analog_input(int port) {return analog_get(port);}

src/avr/src/analog.h

@@ -0,0 +1,33 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+void analog_init();
+float analog_get(unsigned port);
+void analog_rtc_callback();

src/avr/src/axis.c

@@ -0,0 +1,124 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "axis.h"
+#include "motor.h"
+#include "switch.h"
+#include "util.h"
+
+#include <math.h>
+#include <string.h>
+#include <ctype.h>
+
+
+int motor_map[AXES] = {-1, -1, -1, -1, -1, -1};
+
+
+typedef struct {
+  float velocity_max;    // max velocity in mm/min or deg/min
+  float accel_max;       // max acceleration in mm/min^2
+  float jerk_max;        // max jerk (Jm) in km/min^3
+} axis_t;
+
+
+axis_t axes[MOTORS] = {};
+
+
+bool axis_is_enabled(int axis) {
+  int motor = axis_get_motor(axis);
+  return motor != -1 && motor_is_enabled(motor) &&
+    !fp_ZERO(axis_get_velocity_max(axis));
+}
+
+
+int axis_get_id(char axis) {
+  const char *axes = "XYZABCUVW";
+  const char *ptr = strchr(axes, toupper(axis));
+  return ptr == 0 ? -1 : (ptr - axes);
+}
+
+
+int axis_get_motor(int axis) {return motor_map[axis];}
+
+
+bool axis_get_homed(int axis) {
+  return axis_is_enabled(axis) ? motor_get_homed(axis_get_motor(axis)) : false;
+}
+
+
+float axis_get_soft_limit(int axis, bool min) {
+  if (!axis_is_enabled(axis)) return min ? -INFINITY : INFINITY;
+  return motor_get_soft_limit(axis_get_motor(axis), min);
+}
+
+
+// Map axes to first matching motor
+void axis_map_motors() {
+  for (int axis = 0; axis < AXES; axis++)
+    for (int motor = 0; motor < MOTORS; motor++)
+      if (motor_get_axis(motor) == axis) {
+        motor_map[axis] = motor;
+        break;
+      }
+}
+
+
+#define AXIS_VAR_GET(NAME, TYPE)                        \
+  TYPE get_##NAME(int axis) {return axes[axis].NAME;}
+
+
+#define AXIS_VAR_SET(NAME, TYPE)                        \
+  void set_##NAME(int axis, TYPE value) {axes[axis].NAME = value;}
+
+
+/// Velocity is scaled by 1,000
+float axis_get_velocity_max(int axis) {
+  int motor = axis_get_motor(axis);
+  return motor == -1 ? 0 : axes[motor].velocity_max * VELOCITY_MULTIPLIER;
+}
+AXIS_VAR_GET(velocity_max, float)
+
+
+/// Acceleration is scaled by 1,000
+float axis_get_accel_max(int axis) {
+  int motor = axis_get_motor(axis);
+  return motor == -1 ? 0 : axes[motor].accel_max * ACCEL_MULTIPLIER;
+}
+AXIS_VAR_GET(accel_max, float)
+
+
+/// Jerk is scaled by 1,000,000
+float axis_get_jerk_max(int axis) {
+  int motor = axis_get_motor(axis);
+  return motor == -1 ? 0 : axes[motor].jerk_max * JERK_MULTIPLIER;
+}
+AXIS_VAR_GET(jerk_max, float)
+
+
+AXIS_VAR_SET(velocity_max, float)
+AXIS_VAR_SET(accel_max, float)
+AXIS_VAR_SET(jerk_max, float)

src/avr/src/axis.h

@@ -0,0 +1,50 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "config.h"
+
+#include <stdbool.h>
+
+
+enum {
+  AXIS_X, AXIS_Y, AXIS_Z,
+  AXIS_A, AXIS_B, AXIS_C,
+};
+
+
+bool axis_is_enabled(int axis);
+int axis_get_id(char axis);
+int axis_get_motor(int axis);
+bool axis_get_homed(int axis);
+float axis_get_soft_limit(int axis, bool min);
+void axis_map_motors();
+
+float axis_get_velocity_max(int axis);
+float axis_get_accel_max(int axis);
+float axis_get_jerk_max(int axis);

src/avr/src/base64.c

@@ -0,0 +1,151 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "base64.h"
+
+#include "util.h"
+
+#include <ctype.h>
+
+
+static const char *_b64_encode =
+  "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
+
+
+static const int8_t _b64_decode[] = { // 43-122
+                                              62, -1, -1, -1, 63,
+  52, 53, 54, 55, 56, 57, 58, 59, 60, 61, -1, -1, -1, -2, -1, -1,
+  -1,  0,  1,  2,  3,  4,  5,  6,  7,  8,  9, 10, 11, 12, 13, 14,
+  15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, -1, -1, -1, -1, -1,
+  -1, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40,
+  41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51
+};
+
+
+static int8_t _decode(char c) {
+  return (c < 43 || 122 < c) ? -1 : _b64_decode[c - 43];
+}
+
+
+static char _encode(uint8_t b) {return _b64_encode[b & 63];}
+
+
+static void _skip_space(const char **s, const char *end) {
+  while (*s < end && isspace(**s)) (*s)++;
+}
+
+
+static char _next(const char **s, const char *end) {
+  char c = *(*s)++;
+  _skip_space(s, end);
+  return c;
+}
+
+
+unsigned b64_encoded_length(unsigned len, bool pad) {
+  unsigned elen = len / 3 * 4;
+
+  switch (len % 3) {
+  case 1: elen += pad ? 4 : 2; break;
+  case 2: elen += pad ? 4 : 3; break;
+  }
+
+  return elen;
+}
+
+
+void b64_encode(const uint8_t *in, unsigned len, char *out, bool pad) {
+  const uint8_t *end = in + len;
+  int padding = 0;
+  uint8_t a, b, c;
+
+  while (in < end) {
+    a = *in++;
+
+    if (in < end) {
+      b = *in++;
+
+      if (in < end) c = *in++;
+      else {c = 0; padding = 1;}
+
+    } else {c = b = 0; padding = 2;}
+
+    *out++ = _encode(63 & (a >> 2));
+    *out++ = _encode(63 & (a << 4 | b >> 4));
+
+    if (pad && padding == 2) *out++ = '=';
+    else *out++ = _encode(63 & (b << 2 | c >> 6));
+
+    if (pad && padding) *out++ = '=';
+    else *out++ = _encode(63 & c);
+  }
+}
+
+
+bool b64_decode(const char *in, unsigned len, uint8_t *out) {
+  const char *end = in + len;
+
+  _skip_space(&in, end);
+
+  while (in < end) {
+    int8_t w = _decode(_next(&in, end));
+    int8_t x = in < end ? _decode(_next(&in, end)) : -2;
+    int8_t y = in < end ? _decode(_next(&in, end)) : -2;
+    int8_t z = in < end ? _decode(_next(&in, end)) : -2;
+
+    if (w == -2 || x == -2 || w == -1 || x == -1 || y == -1 || z == -1)
+      return false;
+
+    *out++ = (uint8_t)(w << 2 | x >> 4);
+    if (y != -2) {
+      *out++ = (uint8_t)(x << 4 | y >> 2);
+      if (z != -2) *out++ = (uint8_t)(y << 6 | z);
+    }
+  }
+
+  return true;
+}
+
+
+bool b64_decode_float(const char *s, float *f) {
+  union {
+    float f;
+    uint8_t b[4];
+  } u;
+
+  if (!b64_decode(s, 6, u.b)) return false;
+
+#if __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+#define XORSWAP(a, b) a ^= (b ^ (b ^= a)
+  XORSWAP(u.b[0], u.b[3]);
+  XORSWAP(u.b[1], u.b[2]);
+#endif
+
+  *f = u.f;
+
+  return true;
+}

src/avr/src/base64.h

@@ -0,0 +1,37 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
+
+unsigned b64_encoded_length(unsigned len, bool pad);
+void b64_encode(const uint8_t *in, unsigned len, char *out, bool pad);
+bool b64_decode(const char *in, unsigned len, uint8_t *out);
+bool b64_decode_float(const char *s, float *f);

src/avr/src/command.c

@@ -0,0 +1,278 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "command.h"
+
+#include "usart.h"
+#include "hardware.h"
+#include "vars.h"
+#include "estop.h"
+#include "i2c.h"
+#include "config.h"
+#include "pgmspace.h"
+#include "state.h"
+#include "exec.h"
+#include "base64.h"
+#include "rtc.h"
+#include "stepper.h"
+#include "cpp_magic.h"
+
+#include <util/atomic.h>
+
+#include <stdio.h>
+#include <string.h>
+#include <ctype.h>
+#include <stdlib.h>
+
+
+#define RING_BUF_NAME sync_q
+#define RING_BUF_TYPE uint8_t
+#define RING_BUF_INDEX_TYPE volatile uint16_t
+#define RING_BUF_SIZE SYNC_QUEUE_SIZE
+#define RING_BUF_ATOMIC_COPY 1
+#include "ringbuf.def"
+
+
+static struct {
+  bool active;
+  uint16_t id;
+  uint32_t last_empty;
+  volatile uint16_t count;
+  float position[AXES];
+} cmd = {0,};
+
+
+// Define command callbacks
+#define CMD(CODE, NAME, SYNC)                   \
+  stat_t command_##NAME(char *);                \
+  IF(SYNC)(unsigned command_##NAME##_size();)   \
+  IF(SYNC)(void command_##NAME##_exec(void *);)
+#include "command.def"
+#undef CMD
+
+
+// Name
+#define CMD(CODE, NAME, SYNC)                                   \
+  static const char command_##NAME##_name[] PROGMEM = #NAME;
+#include "command.def"
+#undef CMD
+
+
+static bool _is_synchronous(char code) {
+  switch (code) {
+#define CMD(CODE, NAME, SYNC, ...) case COMMAND_##NAME: return SYNC;
+#include "command.def"
+#undef CMD
+  }
+  return false;
+}
+
+
+static stat_t _dispatch(char *s) {
+  switch (*s) {
+#define CMD(CODE, NAME, SYNC, ...)              \
+    case COMMAND_##NAME: return command_##NAME(s);
+#include "command.def"
+#undef CMD
+  }
+
+  return STAT_INVALID_COMMAND;
+}
+
+
+static unsigned _size(char code) {
+  switch (code) {
+#define CMD(CODE, NAME, SYNC, ...)                                  \
+    IF(SYNC)(case COMMAND_##NAME: return command_##NAME##_size();)
+#include "command.def"
+#undef CMD
+  }
+
+  return 0;
+}
+
+
+static void _exec_cb(char code, uint8_t *data) {
+  switch (code) {
+#define CMD(CODE, NAME, SYNC, ...)                                      \
+    IF(SYNC)(case COMMAND_##NAME: command_##NAME##_exec(data); break;)
+#include "command.def"
+#undef CMD
+  }
+}
+
+
+static void _i2c_cb(uint8_t *data, uint8_t length) {
+  stat_t status = _dispatch((char *)data);
+  if (status) STATUS_ERROR(status, "i2c: %s", data);
+}
+
+
+void command_init() {i2c_set_read_callback(_i2c_cb);}
+bool command_is_active() {return cmd.active;}
+unsigned command_get_count() {return cmd.count;}
+
+
+void command_print_json() {
+  bool first = true;
+  static const char fmt[] PROGMEM = "\"%c\":{\"name\":\"%" PRPSTR "\"}";
+
+#define CMD(CODE, NAME, SYNC)                                           \
+  if (first) first = false; else putchar(',');                          \
+  printf_P(fmt, CODE, command_##NAME##_name);
+
+#include "command.def"
+#undef CMD
+}
+
+
+void command_flush_queue() {
+  sync_q_init();
+  cmd.count = 0;
+  command_reset_position();
+}
+
+
+void command_push(char code, void *_data) {
+  uint8_t *data = (uint8_t *)_data;
+  unsigned size = _size(code);
+
+  if (!_is_synchronous(code)) estop_trigger(STAT_Q_INVALID_PUSH);
+  if (sync_q_space() <= size) estop_trigger(STAT_Q_OVERRUN);
+
+  sync_q_push(code);
+  for (unsigned i = 0; i < size; i++) sync_q_push(*data++);
+
+  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) cmd.count++;
+}
+
+
+bool command_callback() {
+  static char *block = 0;
+
+  if (!block) block = usart_readline();
+  if (!block) return false; // No command
+
+  stat_t status = STAT_OK;
+
+  // Special processing for synchronous commands
+  if (_is_synchronous(*block)) {
+    if (estop_triggered()) status = STAT_MACHINE_ALARMED;
+    else if (state_is_flushing()) status = STAT_NOP; // Flush command
+    else if (state_is_resuming() || sync_q_space() <= _size(*block))
+      return false; // Wait
+  }
+
+  // Dispatch non-empty commands
+  if (*block && status == STAT_OK) {
+    status = _dispatch(block);
+    if (status == STAT_OK) cmd.active = true; // Disables LCD booting message
+  }
+
+  switch (status) {
+  case STAT_OK: break;
+  case STAT_NOP: break;
+  case STAT_MACHINE_ALARMED: STATUS_WARNING(status, ""); break;
+  default: STATUS_ERROR(status, "%s", block); break;
+  }
+
+  block = 0; // Command consumed
+
+  return true;
+}
+
+
+void command_set_axis_position(int axis, const float p) {
+  cmd.position[axis] = p;
+}
+
+
+void command_set_position(const float position[AXES]) {
+  memcpy(cmd.position, position, sizeof(cmd.position));
+}
+
+
+void command_get_position(float position[AXES]) {
+  memcpy(position, cmd.position, sizeof(cmd.position));
+}
+
+
+void command_reset_position() {
+  float position[AXES];
+  exec_get_position(position);
+  command_set_position(position);
+}
+
+
+char command_peek() {return (char)(cmd.count ? sync_q_peek() : 0);}
+
+
+uint8_t *command_next() {
+  if (!cmd.count) return 0;
+  cmd.count--;
+
+  if (sync_q_empty()) estop_trigger(STAT_Q_UNDERRUN);
+
+  static uint8_t data[INPUT_BUFFER_LEN];
+
+  data[0] = sync_q_next();
+
+  if (!_is_synchronous((char)data[0])) estop_trigger(STAT_INVALID_QCMD);
+
+  unsigned size = _size((char)data[0]);
+  for (unsigned i = 0; i < size; i++)
+    data[i + 1] = sync_q_next();
+
+  return data;
+}
+
+
+// Returns true if command queued
+// Called by exec.c from low-level interrupt
+bool command_exec() {
+  if (!cmd.count) {
+    cmd.last_empty = rtc_get_time();
+    state_idle();
+    return false;
+  }
+
+  // On restart wait a bit to give queue a chance to fill
+  if (cmd.count < EXEC_FILL_TARGET &&
+      !rtc_expired(cmd.last_empty + EXEC_DELAY)) return false;
+
+  uint8_t *data = command_next();
+  state_running();
+
+  _exec_cb((char)*data, data + 1);
+
+  return true;
+}
+
+
+// Var callbacks
+uint16_t get_id() {return cmd.id;}
+void set_id(uint16_t id) {cmd.id = id;}

src/avr/src/command.def

@@ -0,0 +1,50 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+//(CODE, NAME,      SYNC)
+CMD('$', var,          0) // Set or get variable
+CMD('#', sync_var,     1) // Set variable synchronous
+CMD('s', seek,         1) // [switch][flags:active|error]
+CMD('a', set_axis,     1) // [axis][position] Set axis position
+CMD('l', line,         1) // [targetVel][maxJerk][axes][times]
+CMD('%', sync_speed,   1) // [offset][speed] Command synchronized speed
+CMD('p', speed,        1) // [speed] Spindle speed
+CMD('I', input,        1) // [a|d][port][mode][timeout] Read input
+CMD('d', dwell,        1) // [seconds]
+CMD('P', pause,        1) // [type] Pause control
+CMD('S', stop,         0) // Stop move, spindle and load outputs
+CMD('U', unpause,      0) // Unpause
+CMD('j', jog,          0) // [axes]
+CMD('r', report,       0) // <0|1>[var] Enable or disable var reporting
+CMD('R', reboot,       0) // Reboot the controller
+CMD('c', resume,       0) // Continue processing after a flush
+CMD('E', estop,        0) // Emergency stop
+CMD('X', shutdown,     0) // Power shutdown
+CMD('C', clear,        0) // Clear estop
+CMD('F', flush,        0) // Flush command queue
+CMD('D', dump,         0) // Report all variables
+CMD('h', help,         0) // Print this help screen

src/avr/src/command.h

@@ -0,0 +1,58 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "config.h"
+#include "status.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+
+// Commands
+typedef enum {
+#define CMD(CODE, NAME, ...) COMMAND_##NAME = CODE,
+#include "command.def"
+#undef CMD
+} command_t;
+
+
+void command_init();
+bool command_is_active();
+unsigned command_get_count();
+void command_print_json();
+void command_flush_queue();
+void command_push(char code, void *data);
+bool command_callback();
+void command_set_axis_position(int axis, const float p);
+void command_set_position(const float position[AXES]);
+void command_get_position(float position[AXES]);
+void command_reset_position();
+char command_peek();
+uint8_t *command_next();
+bool command_exec();

src/avr/src/command.json.in

@@ -0,0 +1,11 @@
+#include "cpp_magic.h"
+{
+#define CMD(CODE, NAME, SYNC) \
+  #NAME: {                                      \
+    "code": CODE,                               \
+    "sync": IF_ELSE(SYNC)(true, false)          \
+  },
+#include "command.def"
+#undef CMD
+  "_": {}
+}

src/avr/src/commands.c

@@ -0,0 +1,83 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "config.h"
+#include "stepper.h"
+#include "command.h"
+#include "vars.h"
+#include "base64.h"
+#include "hardware.h"
+#include "report.h"
+#include "exec.h"
+
+#include <stdio.h>
+
+
+stat_t command_dwell(char *cmd) {
+  float seconds;
+  if (!b64_decode_float(cmd + 1, &seconds)) return STAT_BAD_FLOAT;
+  command_push(*cmd, &seconds);
+  return STAT_OK;
+}
+
+
+static stat_t _dwell_exec() {
+  exec_set_cb(0); // Immediately clear the callback
+  return STAT_OK;
+}
+
+
+unsigned command_dwell_size() {return sizeof(float);}
+
+
+void command_dwell_exec(void *seconds) {
+  st_prep_dwell(*(float *)seconds);
+  exec_set_cb(_dwell_exec); // Command must set an exec callback
+}
+
+
+stat_t command_help(char *cmd) {
+  printf_P(PSTR("\n{\"commands\":{"));
+  command_print_json();
+  printf_P(PSTR("},\"variables\":{"));
+  vars_print_json();
+  printf_P(PSTR("}}\n"));
+
+  return STAT_OK;
+}
+
+
+stat_t command_reboot(char *cmd) {
+  hw_request_hard_reset();
+  return STAT_OK;
+}
+
+
+stat_t command_dump(char *cmd) {
+  report_request_full();
+  return STAT_OK;
+}

src/avr/src/config.h

@@ -0,0 +1,222 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "pins.h"
+
+#include <avr/interrupt.h>
+
+
+// Pins
+enum {
+  STALL_0_PIN = PIN_ID(PORT_A, 0),
+  STALL_1_PIN,
+  STALL_2_PIN,
+  STALL_3_PIN,
+  TOOL_DIR_PIN,
+  TOOL_ENABLE_PIN,
+  ANALOG_1_PIN,
+  ANALOG_2_PIN,
+
+  MIN_0_PIN = PIN_ID(PORT_B, 0),
+  MAX_0_PIN,
+  MIN_3_PIN,
+  MAX_3_PIN,
+  MIN_1_PIN,
+  MAX_1_PIN,
+  MIN_2_PIN,
+  MAX_2_PIN,
+
+  SDA_PIN = PIN_ID(PORT_C, 0),
+  SCL_PIN,
+  SERIAL_RX_PIN,
+  SERIAL_TX_PIN,
+  SERIAL_CTS_PIN,
+  SPI_CLK_PIN,
+  SPI_MISO_PIN,
+  SPI_MOSI_PIN,
+
+  STEP_0_PIN = PIN_ID(PORT_D, 0),
+  SPI_CS_0_PIN,
+  SPI_CS_3_PIN,
+  SPI_CS_2_PIN,
+  PWM_PIN,
+  SWITCH_2_PIN,
+  RS485_RO_PIN,
+  RS485_DI_PIN,
+
+  STEP_1_PIN = PIN_ID(PORT_E, 0),
+  SPI_CS_1_PIN,
+  DIR_0_PIN,
+  DIR_1_PIN,
+  STEP_3_PIN,
+  SWITCH_1_PIN,
+  DIR_2_PIN,
+  DIR_3_PIN,
+
+  STEP_2_PIN = PIN_ID(PORT_F, 0),
+  RS485_RW_PIN,
+  FAULT_PIN,
+  ESTOP_PIN,
+  MOTOR_FAULT_PIN,
+  MOTOR_ENABLE_PIN,
+  TEST_PIN,
+  PROBE_PIN,
+};
+
+#define SPI_SS_PIN               SERIAL_CTS_PIN // Needed for SPI configuration
+
+
+#define AXES                     6 // number of axes
+#define MOTORS                   4 // number of motors on the board
+#define OUTS                     6 // number of supported pin outputs
+#define ANALOG                   2 // number of supported analog inputs
+#define VFDREG                  32 // number of supported VFD modbus registers
+
+// Switch settings.  See switch.c
+#define SWITCH_DEBOUNCE          5 // ms, default value
+#define SWITCH_LOCKOUT         250 // ms, default value
+#define SWITCH_MAX_DEBOUNCE   5000 // ms
+#define SWITCH_MAX_LOCKOUT   60000 // ms
+
+
+// Motor ISRs
+#define STALL_ISR_vect           PORTA_INT1_vect
+#define FAULT_ISR_vect           PORTF_INT1_vect
+
+
+/* Interrupt usage:
+ *
+ *    HI    Step timers                          stepper.c
+ *    HI    Serial RX                            usart.c
+ *   MED    Serial TX                            usart.c (* see note)
+ *   MED    Modbus serial interrupts             modbus.c
+ *    LO    Segment execution SW interrupt       stepper.c
+ *    LO    I2C Slave                            i2c.c
+ *    LO    Real-time clock interrupt            rtc.c
+ *    LO    DRV8711 SPI                          drv8711.c
+ *    LO    A2D interrupts                       analog.c
+ *
+ *    (*) The TX cannot run at LO level or exception reports and other prints
+ *        called from a LO interrupt (as in prep_line()) will kill the system
+ *        in a permanent loop call in usart_putc() (usart.c).
+ */
+
+// Timer assignments
+// NOTE, TCC1 is unused
+#define TIMER_STEP               TCC0 // Step timer (see stepper.h)
+#define TIMER_PWM                TCD1 // PWM timer  (see pwm.c)
+
+
+// Timer setup for stepper and dwells
+#define STEP_TIMER_DIV           8
+#define STEP_TIMER_FREQ          (F_CPU / STEP_TIMER_DIV)
+#define STEP_TIMER_POLL          ((uint16_t)(STEP_TIMER_FREQ * 0.001)) // 1ms
+#define STEP_TIMER_ISR           TCC0_OVF_vect
+#define STEP_LOW_LEVEL_ISR       ADCB_CH0_vect
+#define STEP_PULSE_WIDTH         (F_CPU * 0.000002) // 2uS w/ clk/1
+#define SEGMENT_MS               4
+#define SEGMENT_TIME             (SEGMENT_MS / 60000.0) // mins
+
+
+// DRV8711 settings
+// NOTE, PWM frequency = 1 / (2 * DTIME + TBLANK + TOFF)
+// We have PWM frequency = 1 / (2 * 850nS + 1uS + 6.5uS) ~= 110kHz
+#define DRV8711_OFF              12
+#define DRV8711_BLANK            (0x32 | DRV8711_BLANK_ABT_bm)
+#define DRV8711_DECAY            (DRV8711_DECAY_DECMOD_MIXED | 16)
+
+#define DRV8711_DRIVE            (DRV8711_DRIVE_IDRIVEP_50  | \
+                                  DRV8711_DRIVE_IDRIVEN_100 | \
+                                  DRV8711_DRIVE_TDRIVEP_500 | \
+                                  DRV8711_DRIVE_TDRIVEN_500 | \
+                                  DRV8711_DRIVE_OCPDEG_1    | \
+                                  DRV8711_DRIVE_OCPTH_500)
+#define DRV8711_TORQUE            DRV8711_TORQUE_SMPLTH_200
+// NOTE, Datasheet suggests 850ns DTIME with the optional gate resistor
+// installed.  See page 30 section 8.1.2 of DRV8711 datasheet.
+#define DRV8711_CTRL             (DRV8711_CTRL_ISGAIN_5 | \
+                                  DRV8711_CTRL_DTIME_850)
+
+
+// RS485 settings
+#define RS485_PORT               USARTD1
+#define RS485_DRE_vect           USARTD1_DRE_vect
+#define RS485_TXC_vect           USARTD1_TXC_vect
+#define RS485_RXC_vect           USARTD1_RXC_vect
+
+
+// Modbus settings
+#define MODBUS_TIMEOUT           100 // ms. response timeout
+#define MODBUS_RETRIES           4   // Number of retries before failure
+#define MODBUS_BUF_SIZE          18  // Max bytes in rx/tx buffers
+#define VFD_QUERY_DELAY          100 // ms
+
+
+// Serial settings
+#define SERIAL_BAUD              USART_BAUD_230400 // 115200
+#define SERIAL_PORT              USARTC0
+#define SERIAL_DRE_vect          USARTC0_DRE_vect
+#define SERIAL_RXC_vect          USARTC0_RXC_vect
+#define SERIAL_CTS_THRESH        4
+
+
+// PWM settings
+#define POWER_MAX_UPDATES        SEGMENT_MS
+
+// Input
+#define INPUT_BUFFER_LEN         128 // text buffer size (255 max)
+
+
+// Report
+#define REPORT_RATE              250 // ms
+
+
+// I2C
+#define I2C_DEV                  TWIC
+#define I2C_ISR                  TWIC_TWIS_vect
+#define I2C_ADDR                 0x2b
+#define I2C_MAX_DATA             8
+
+
+// Motor
+#define MOTOR_IDLE_TIMEOUT       0.25  // secs, motor off after this time
+#define MIN_STEP_CORRECTION      2
+
+#define MIN_VELOCITY             10            // mm/min
+#define CURRENT_SENSE_RESISTOR   0.05          // ohms
+#define CURRENT_SENSE_REF        2.75          // volts
+#define MAX_CURRENT              6             // amps
+#define MAX_IDLE_CURRENT         2             // amps
+#define VELOCITY_MULTIPLIER      1000.0
+#define ACCEL_MULTIPLIER         1000000.0
+#define JERK_MULTIPLIER          1000000.0
+#define SYNC_QUEUE_SIZE          4096
+#define EXEC_FILL_TARGET         8
+#define EXEC_DELAY               250 // ms
+#define JOG_STOPPING_UNDERSHOOT  1   // % of stopping distance

src/avr/src/cpp_magic.h

@@ -0,0 +1,507 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+/* This header file contains a library of advanced C Pre-Processor (CPP) macros
+ * which implement various useful functions, such as iteration, in the
+ * pre-processor.
+ *
+ * Though the file name (quite validly) labels this as magic, there should be
+ * enough documentation in the comments for a reader only casually familiar
+ * with the CPP to be able to understand how everything works.
+ *
+ * The majority of the magic tricks used in this file are based on those
+ * described by pfultz2 in his "Cloak" library:
+ *
+ * https://github.com/pfultz2/Cloak/wiki/C-Preprocessor-tricks,-tips,-and-idioms
+ *
+ * Major differences are a greater level of detailed explanation in this
+ * implementation and also a refusal to include any macros which require a O(N)
+ * macro definitions to handle O(N) arguments (with the exception of DEFERn).
+ */
+
+#pragma once
+
+/**
+ * Force the pre-processor to expand the macro a large number of times.
+ * Usage:
+ *
+ *   EVAL(expression)
+ *
+ * This is useful when you have a macro which evaluates to a valid
+ * macro expression which is not subsequently expanded in the same
+ * pass. A contrived, but easy to understand, example of such a macro
+ * follows. Note that though this example is contrived, this behaviour
+ * is abused to implement bounded recursion in macros such as FOR.
+ *
+ *   #define A(x) x+1
+ *   #define EMPTY
+ *   #define NOT_QUITE_RIGHT(x) A EMPTY (x)
+ *   NOT_QUITE_RIGHT(999)
+ *
+ * Here's what happens inside the C preprocessor:
+ *
+ * 1. It sees a macro "NOT_QUITE_RIGHT" and performs a single macro
+ *    expansion pass on its arguments. Since the argument is "999" and
+ *    this isn't a macro, this is a boring step resulting in no
+ *    change.
+ *
+ * 2. The NOT_QUITE_RIGHT macro is substituted for its definition
+ *    giving "A EMPTY() (x)".
+ *
+ * 3. The expander moves from left-to-right trying to expand the
+ *    macro: The first token, A, cannot be expanded since there are no
+ *    brackets immediately following it. The second token EMPTY(),
+ *    however, can be expanded (recursively in this manner) and is
+ *    replaced with "".
+ *
+ * 4. Expansion continues from the start of the substituted test
+ *    (which in this case is just empty), and sees "(999)" but since
+ *    no macro name is present, nothing is done. This results in a
+ *    final expansion of "A (999)".
+ *
+ * Unfortunately, this doesn't quite meet expectations since you may
+ * expect that "A (999)" would have been expanded into
+ * "999+1". Unfortunately this requires a second expansion pass but
+ * luckily we can force the macro processor to make more passes by
+ * abusing the first step of macro expansion: the preprocessor expands
+ * arguments in their own pass. If we define a macro which does
+ * nothing except produce its arguments e.g.:
+ *
+ *   #define PASS_THROUGH(...) __VA_ARGS__
+ *
+ * We can now do "PASS_THROUGH(NOT_QUITE_RIGHT(999))" causing
+ * "NOT_QUITE_RIGHT" to be expanded to "A (999)", as described above,
+ * when the arguments are expanded.  Now when the body of PASS_THROUGH
+ * is expanded, "A (999)" gets expanded to "999+1".
+ *
+ * The EVAL defined below is essentially equivalent to a large nesting
+ * of "PASS_THROUGH(PASS_THROUGH(PASS_THROUGH(..." which results in
+ * the preprocessor making a large number of expansion passes over the
+ * given expression.
+ */
+#define EVAL(...) EVAL1024(__VA_ARGS__)
+#define EVAL1024(...) EVAL512(EVAL512(__VA_ARGS__))
+#define EVAL512(...) EVAL256(EVAL256(__VA_ARGS__))
+#define EVAL256(...) EVAL128(EVAL128(__VA_ARGS__))
+#define EVAL128(...) EVAL64(EVAL64(__VA_ARGS__))
+#define EVAL64(...) EVAL32(EVAL32(__VA_ARGS__))
+#define EVAL32(...) EVAL16(EVAL16(__VA_ARGS__))
+#define EVAL16(...) EVAL8(EVAL8(__VA_ARGS__))
+#define EVAL8(...) EVAL4(EVAL4(__VA_ARGS__))
+#define EVAL4(...) EVAL2(EVAL2(__VA_ARGS__))
+#define EVAL2(...) EVAL1(EVAL1(__VA_ARGS__))
+#define EVAL1(...) __VA_ARGS__
+
+
+// Macros which expand to common values
+#define PASS(...) __VA_ARGS__
+#define EMPTY()
+#define COMMA() ,
+#define SEMI() ;
+#define PLUS() +
+#define ZERO() 0
+#define ONE() 1
+
+/**
+ * Causes a function-style macro to require an additional pass to be expanded.
+ *
+ * This is useful, for example, when trying to implement recursion since the
+ * recursive step must not be expanded in a single pass as the pre-processor
+ * will catch it and prevent it.
+ *
+ * Usage:
+ *
+ *   DEFER1(IN_NEXT_PASS)(args, to, the, macro)
+ *
+ * How it works:
+ *
+ * 1. When DEFER1 is expanded, first its arguments are expanded which are
+ *    simply IN_NEXT_PASS. Since this is a function-style macro and it has no
+ *    arguments, nothing will happen.
+ * 2. The body of DEFER1 will now be expanded resulting in EMPTY() being
+ *    deleted. This results in "IN_NEXT_PASS (args, to, the macro)". Note that
+ *    since the macro expander has already passed IN_NEXT_PASS by the time it
+ *    expands EMPTY() and so it won't spot that the brackets which remain can be
+ *    applied to IN_NEXT_PASS.
+ * 3. At this point the macro expansion completes. If one more pass is made,
+ *    IN_NEXT_PASS(args, to, the, macro) will be expanded as desired.
+ */
+#define DEFER1(id) id EMPTY()
+
+/**
+ * As with DEFER1 except here n additional passes are required for DEFERn.
+ *
+ * The mechanism is analogous.
+ *
+ * Note that there doesn't appear to be a way of combining DEFERn macros in
+ * order to achieve exponentially increasing defers e.g. as is done by EVAL.
+ */
+#define DEFER2(id) id EMPTY EMPTY()()
+#define DEFER3(id) id EMPTY EMPTY EMPTY()()()
+#define DEFER4(id) id EMPTY EMPTY EMPTY EMPTY()()()()
+#define DEFER5(id) id EMPTY EMPTY EMPTY EMPTY EMPTY()()()()()
+#define DEFER6(id) id EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY()()()()()()
+#define DEFER7(id) id EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY()()()()()()()
+#define DEFER8(id) \
+  id EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY()()()()()()()()
+
+
+/**
+ * Indirection around the standard ## concatenation operator. This simply
+ * ensures that the arguments are expanded (once) before concatenation.
+ */
+#define CAT(a, ...) a ## __VA_ARGS__
+#define CAT3(a, b, ...) a ## b ## __VA_ARGS__
+
+
+/**
+ * Get the first argument and ignore the rest.
+ */
+#define FIRST(a, ...) a
+
+/**
+ * Get the second argument and ignore the rest.
+ */
+#define SECOND(a, b, ...) b
+
+/**
+ * Expects a single input (not containing commas). Returns 1 if the input is
+ * PROBE() and otherwise returns 0.
+ *
+ * This can be useful as the basis of a NOT function.
+ *
+ * This macro abuses the fact that PROBE() contains a comma while other valid
+ * inputs must not.
+ */
+#define IS_PROBE(...) SECOND(__VA_ARGS__, 0)
+#define PROBE() ~, 1
+
+
+/**
+ * Logical negation. 0 is defined as false and everything else as true.
+ *
+ * When 0, _NOT_0 will be found which evaluates to the PROBE. When 1
+ * (or any other value) is given, an appropriately named macro won't
+ * be found and the concatenated string will be produced. IS_PROBE
+ * then simply checks to see if the PROBE was returned, cleanly
+ * converting the argument into a 1 or 0.
+ */
+#define NOT(x) IS_PROBE(CAT(_NOT_, x))
+#define _NOT_0 PROBE()
+
+/**
+ * Macro version of C's famous "cast to bool" operator (i.e. !!) which takes
+ * anything and casts it to 0 if it is 0 and 1 otherwise.
+ */
+#define BOOL(x) NOT(NOT(x))
+
+/**
+ * Logical OR. Simply performs a lookup.
+ */
+#define OR(a,b) CAT3(_OR_, a, b)
+#define _OR_00 0
+#define _OR_01 1
+#define _OR_10 1
+#define _OR_11 1
+
+/**
+ * Logical AND. Simply performs a lookup.
+ */
+#define AND(a,b) CAT3(_AND_, a, b)
+#define _AND_00 0
+#define _AND_01 0
+#define _AND_10 0
+#define _AND_11 1
+
+
+/**
+ * Macro if statement. Usage:
+ *
+ *   IF(c)(expansion when true)
+ *
+ * Here's how:
+ *
+ * 1. The preprocessor expands the arguments to _IF casting the condition to '0'
+ *    or '1'.
+ * 2. The casted condition is concatencated with _IF_ giving _IF_0 or _IF_1.
+ * 3. The _IF_0 and _IF_1 macros either returns the argument or doesn't (e.g.
+ *    they implement the "choice selection" part of the macro).
+ * 4. Note that the "true" clause is in the extra set of brackets; thus these
+ *    become the arguments to _IF_0 or _IF_1 and thus a selection is made!
+ */
+#define IF(c) _IF(BOOL(c))
+#define _IF(c) CAT(_IF_,c)
+#define _IF_0(...)
+#define _IF_1(...) __VA_ARGS__
+
+/**
+ * Macro if/else statement. Usage:
+ *
+ *   IF_ELSE(c)( \
+ *     expansion when true, \
+ *     expansion when false \
+ *   )
+ *
+ * The mechanism is analogous to IF.
+ */
+#define IF_ELSE(c) _IF_ELSE(BOOL(c))
+#define _IF_ELSE(c) CAT(_IF_ELSE_,c)
+#define _IF_ELSE_0(t,f) f
+#define _IF_ELSE_1(t,f) t
+
+
+/**
+ * Macro which checks if it has any arguments. Returns '0' if there are no
+ * arguments, '1' otherwise.
+ *
+ * Limitation: HAS_ARGS(,1,2,3) returns 0 -- this check essentially only checks
+ * that the first argument exists.
+ *
+ * This macro works as follows:
+ *
+ * 1. _END_OF_ARGUMENTS_ is concatenated with the first argument.
+ * 2. If the first argument is not present then only "_END_OF_ARGUMENTS_" will
+ *    remain, otherwise "_END_OF_ARGUMENTS something_here" will remain.
+ * 3. In the former case, the _END_OF_ARGUMENTS_() macro expands to a
+ *    0 when it is expanded. In the latter, a non-zero result remains.
+ * 4. BOOL is used to force non-zero results into 1 giving the clean 0 or 1
+ *    output required.
+ */
+#define HAS_ARGS(...) BOOL(FIRST(_END_OF_ARGUMENTS_ __VA_ARGS__)())
+#define _END_OF_ARGUMENTS_() 0
+
+
+/**
+ * Macro map/list comprehension. Usage:
+ *
+ *   MAP(op, sep, ...)
+ *
+ * Produces a 'sep()'-separated list of the result of op(arg) for each arg.
+ *
+ * Example Usage:
+ *
+ *   #define MAKE_HAPPY(x) happy_##x
+ *   #define COMMA() ,
+ *   MAP(MAKE_HAPPY, COMMA, 1,2,3)
+ *
+ * Which expands to:
+ *
+ *    happy_1 , happy_2 , happy_3
+ *
+ * How it works:
+ *
+ * 1. The MAP macro simply maps the inner MAP_INNER function in an EVAL which
+ *    forces it to be expanded a large number of times, thus enabling many steps
+ *    of iteration (see step 6).
+ * 2. The MAP_INNER macro is substituted for its body.
+ * 3. In the body, op(cur_val) is substituted giving the value for this
+ *    iteration.
+ * 4. The IF macro expands according to whether further iterations are required.
+ *    This expansion either produces _IF_0 or _IF_1.
+ * 5. Since the IF is followed by a set of brackets containing the "if true"
+ *    clause, these become the argument to _IF_0 or _IF_1. At this point, the
+ *    macro in the brackets will be expanded giving the separator followed by
+ *    _MAP_INNER EMPTY()()(op, sep, __VA_ARGS__).
+ * 5. If the IF was not taken, the above will simply be discarded and everything
+ *    stops. If the IF is taken, The expression is then processed a second time
+ *    yielding "_MAP_INNER()(op, sep, __VA_ARGS__)". Note that this call looks
+ *    very similar to the  essentially the same as the original call except the
+ *    first argument has been dropped.
+ * 6. At this point expansion of MAP_INNER will terminate. However, since we can
+ *    force more rounds of expansion using EVAL1. In the argument-expansion pass
+ *    of the EVAL1, _MAP_INNER() is expanded to MAP_INNER which is then expanded
+ *    using the arguments which follow it as in step 2-5. This is followed by a
+ *    second expansion pass as the substitution of EVAL1() is expanded executing
+ *    2-5 a second time. This results in up to two iterations occurring. Using
+ *    many nested EVAL1 macros, i.e. the very-deeply-nested EVAL macro, will in
+ *    this manner produce further iterations, hence the outer MAP macro doing
+ *    this for us.
+ *
+ * Important tricks used:
+ *
+ * * If we directly produce "MAP_INNER" in an expansion of MAP_INNER,
+ *   a special case in the preprocessor will prevent it being expanded
+ *   in the future, even if we EVAL.  As a result, the MAP_INNER macro
+ *   carefully only expands to something containing "_MAP_INNER()"
+ *   which requires a further expansion step to invoke MAP_INNER and
+ *   thus implementing the recursion.
+ *
+ * * To prevent _MAP_INNER being expanded within the macro we must
+ *   first defer its expansion during its initial pass as an argument
+ *   to _IF_0 or _IF_1. We must then defer its expansion a second time
+ *   as part of the body of the _IF_0. As a result hence the DEFER2.
+ *
+ * * _MAP_INNER seemingly gets away with producing itself because it
+ *   actually only produces MAP_INNER. It just happens that when
+ *   _MAP_INNER() is expanded in this case it is followed by some
+ *   arguments which get consumed by MAP_INNER and produce a
+ *   _MAP_INNER.  As such, the macro expander never marks _MAP_INNER
+ *   as expanding to itself and thus it will still be expanded in
+ *   future productions of itself.
+ */
+#define MAP(...) \
+   IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_INNER(__VA_ARGS__)))
+#define MAP_INNER(op,sep,cur_val, ...) \
+  op(cur_val) \
+  IF(HAS_ARGS(__VA_ARGS__))( \
+    sep() DEFER2(_MAP_INNER)()(op, sep, ##__VA_ARGS__) \
+  )
+#define _MAP_INNER() MAP_INNER
+
+
+/**
+ * This is a variant of the MAP macro which also includes as an argument to the
+ * operation a valid C variable name which is different for each iteration.
+ *
+ * Usage:
+ *   MAP_WITH_ID(op, sep, ...)
+ *
+ * Where op is a macro op(val, id) which takes a list value and an ID. This ID
+ * will simply be a unary number using the digit "I", that is, I, II, III, IIII,
+ * and so on.
+ *
+ * Example:
+ *
+ *   #define MAKE_STATIC_VAR(type, name) static type name;
+ *   MAP_WITH_ID(MAKE_STATIC_VAR, EMPTY, int, int, int, bool, char)
+ *
+ * Which expands to:
+ *
+ *   static int I; static int II; static int III; static bool IIII;
+ *   static char IIIII;
+ *
+ * The mechanism is analogous to the MAP macro.
+ */
+#define MAP_WITH_ID(op,sep,...) \
+  IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_WITH_ID_INNER(op,sep,I, ##__VA_ARGS__)))
+#define MAP_WITH_ID_INNER(op,sep,id,cur_val, ...) \
+  op(cur_val,id) \
+  IF(HAS_ARGS(__VA_ARGS__))( \
+    sep() DEFER2(_MAP_WITH_ID_INNER)()(op, sep, CAT(id,I), ##__VA_ARGS__) \
+  )
+#define _MAP_WITH_ID_INNER() MAP_WITH_ID_INNER
+
+
+/**
+ * This is a variant of the MAP macro which iterates over pairs rather than
+ * singletons.
+ *
+ * Usage:
+ *   MAP_PAIRS(op, sep, ...)
+ *
+ * Where op is a macro op(val_1, val_2) which takes two list values.
+ *
+ * Example:
+ *
+ *   #define MAKE_STATIC_VAR(type, name) static type name;
+ *   MAP_PAIRS(MAKE_STATIC_VAR, EMPTY, char, my_char, int, my_int)
+ *
+ * Which expands to:
+ *
+ *   static char my_char; static int my_int;
+ *
+ * The mechanism is analogous to the MAP macro.
+ */
+#define MAP_PAIRS(op,sep,...) \
+  IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_PAIRS_INNER(op,sep,__VA_ARGS__)))
+#define MAP_PAIRS_INNER(op,sep,cur_val_1, cur_val_2, ...) \
+  op(cur_val_1,cur_val_2) \
+  IF(HAS_ARGS(__VA_ARGS__))( \
+    sep() DEFER2(_MAP_PAIRS_INNER)()(op, sep, __VA_ARGS__) \
+  )
+#define _MAP_PAIRS_INNER() MAP_PAIRS_INNER
+
+/**
+ * This is a variant of the MAP macro which iterates over a two-element sliding
+ * window.
+ *
+ * Usage:
+ *   MAP_SLIDE(op, last_op, sep, ...)
+ *
+ * Where op is a macro op(val_1, val_2) which takes the two list values
+ * currently in the window. last_op is a macro taking a single value which is
+ * called for the last argument.
+ *
+ * Example:
+ *
+ *   #define SIMON_SAYS_OP(simon, next) IF(NOT(simon()))(next)
+ *   #define SIMON_SAYS_LAST_OP(val) last_but_not_least_##val
+ *   #define SIMON_SAYS() 0
+ *
+ *   MAP_SLIDE(SIMON_SAYS_OP, SIMON_SAYS_LAST_OP, EMPTY, wiggle, SIMON_SAYS,
+ *     dance, move, SIMON_SAYS, boogie, stop)
+ *
+ * Which expands to:
+ *
+ *   dance boogie last_but_not_least_stop
+ *
+ * The mechanism is analogous to the MAP macro.
+ */
+#define MAP_SLIDE(op,last_op,sep,...) \
+  IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_SLIDE_INNER(op,last_op,sep,__VA_ARGS__)))
+#define MAP_SLIDE_INNER(op,last_op,sep,cur_val, ...) \
+  IF(HAS_ARGS(__VA_ARGS__))(op(cur_val,FIRST(__VA_ARGS__))) \
+  IF(NOT(HAS_ARGS(__VA_ARGS__)))(last_op(cur_val)) \
+  IF(HAS_ARGS(__VA_ARGS__))( \
+    sep() DEFER2(_MAP_SLIDE_INNER)()(op, last_op, sep, __VA_ARGS__) \
+  )
+#define _MAP_SLIDE_INNER() MAP_SLIDE_INNER
+
+
+/**
+ * Strip any excess commas from a set of arguments.
+ */
+#define REMOVE_TRAILING_COMMAS(...) \
+	MAP(PASS, COMMA, __VA_ARGS__)
+
+
+/**
+ * Evaluates an array of macros passing 1 argument
+ */
+#define EMAP1(...)                                          \
+  IF(HAS_ARGS(__VA_ARGS__))(EVAL(EMAP1_INNER(__VA_ARGS__)))
+
+#define EMAP1_INNER(ARG1, OP, ...)                                      \
+  _##OP(ARG1)                                                           \
+  IF(HAS_ARGS(__VA_ARGS__))                                             \
+  (DEFER2(_EMAP1_INNER)()(ARG1, ##__VA_ARGS__))
+
+#define _EMAP1_INNER() EMAP1_INNER
+
+
+/**
+ * Evaluates an array of macros passing 2 arguments
+ */
+#define EMAP2(...)                                          \
+  IF(HAS_ARGS(__VA_ARGS__))(EVAL(EMAP2_INNER(__VA_ARGS__)))
+
+#define EMAP2_INNER(ARG1, ARG2, OP, ...)                                \
+  _##OP(ARG1, ARG2)                                                     \
+  IF(HAS_ARGS(__VA_ARGS__))                                             \
+  (DEFER2(_EMAP2_INNER)()(ARG1, ARG2, ##__VA_ARGS__))
+
+#define _EMAP2_INNER() EMAP2_INNER

src/avr/src/drv8711.c

@@ -0,0 +1,546 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "drv8711.h"
+#include "status.h"
+#include "stepper.h"
+#include "switch.h"
+#include "estop.h"
+#include "seek.h"
+
+#include <avr/interrupt.h>
+#include <util/delay.h>
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+
+
+#define DRIVERS MOTORS
+
+
+#define DRV8711_WORD_BYTE_PTR(WORD, LOW) (((uint8_t *)&(WORD)) + !(LOW))
+
+
+typedef enum {
+  SS_WRITE_OFF,
+  SS_WRITE_BLANK,
+  SS_WRITE_DECAY,
+  SS_WRITE_STALL,
+  SS_WRITE_DRIVE,
+  SS_WRITE_TORQUE,
+  SS_WRITE_CTRL,
+  SS_READ_OFF,
+  SS_READ_STATUS,
+  SS_CLEAR_STATUS,
+} spi_state_t;
+
+
+bool motor_fault = false;
+
+
+typedef struct {
+  float current;
+  uint8_t torque;
+} current_t;
+
+
+typedef struct {
+  uint8_t cs_pin;
+  switch_id_t stall_sw;
+
+  uint8_t status;
+  uint16_t flags;
+  bool reset_flags;
+  bool stalled;
+
+  drv8711_state_t state;
+  current_t drive;
+  current_t idle;
+  current_t stall;
+  uint16_t stall_vdiv;
+  uint8_t stall_thresh;
+  uint8_t last_thresh;
+  uint16_t sample_time;
+  //float stall_current;
+
+  uint8_t microstep;
+  stall_callback_t stall_cb;
+
+  uint8_t last_torque;
+  uint8_t last_microstep;
+
+  spi_state_t spi_state;
+} drv8711_driver_t;
+
+
+static drv8711_driver_t drivers[DRIVERS] = {
+  {.cs_pin = SPI_CS_0_PIN, .stall_sw = SW_STALL_0},
+  {.cs_pin = SPI_CS_1_PIN, .stall_sw = SW_STALL_1},
+  {.cs_pin = SPI_CS_2_PIN, .stall_sw = SW_STALL_2},
+  {.cs_pin = SPI_CS_3_PIN, .stall_sw = SW_STALL_3},
+};
+
+
+typedef struct {
+  bool advance;
+  uint8_t disable_cs_pin;
+
+  uint16_t command;
+  uint16_t response;
+  uint8_t driver;
+  bool low_byte;
+} spi_t;
+
+static spi_t spi = {0};
+
+
+static void _current_set(current_t *c, float current) {
+  const float gain = DRV8711_CTRL_GAIN(DRV8711_CTRL);
+  const float max_current = CURRENT_SENSE_REF / (CURRENT_SENSE_RESISTOR * gain);
+
+  // Limit to max configurable current (11A)
+  if (max_current < current) current = max_current;
+
+  c->current = current;
+  c->torque = round(current * CURRENT_SENSE_RESISTOR / CURRENT_SENSE_REF *
+                    gain * 255);
+}
+
+
+static bool _driver_fault(drv8711_driver_t *drv) {return drv->flags & 0x1f;}
+
+
+static bool _driver_active(drv8711_driver_t *drv) {
+  return drv->state == DRV8711_ACTIVE;
+}
+
+
+static float _driver_get_current(drv8711_driver_t *drv) {
+  if (_driver_fault(drv)) return 0;
+
+  switch (drv->state) {
+  case DRV8711_IDLE: return drv->idle.current;
+  case DRV8711_ACTIVE: return drv->drive.current;
+  default: return 0; // Off
+  }
+}
+
+
+static uint8_t _driver_get_torque(drv8711_driver_t *drv) {
+  if (estop_triggered()) return 0;
+
+  if(seek_active()) return drv->stall.torque;
+
+  switch (drv->state) {
+  case DRV8711_IDLE:   return drv->idle.torque;
+  case DRV8711_ACTIVE: return drv->drive.torque;
+  default: return 0; // Off
+  }
+}
+
+
+static uint16_t _driver_spi_command(drv8711_driver_t *drv) {
+  switch (drv->spi_state) {
+  case SS_WRITE_OFF:   return DRV8711_WRITE(DRV8711_OFF_REG,   DRV8711_OFF);
+  case SS_WRITE_BLANK: return DRV8711_WRITE(DRV8711_BLANK_REG, DRV8711_BLANK);
+  case SS_WRITE_DECAY: return DRV8711_WRITE(DRV8711_DECAY_REG, DRV8711_DECAY);
+
+  case SS_WRITE_STALL: {
+    //uint16_t reg = drv->stall_thresh | DRV8711_STALL_SDCNT_2 | drv->stall_vdiv; //ORIGINAL
+    //uint16_t reg = 50 | DRV8711_STALL_SDCNT_8 | DRV8711_STALL_VDIV_4; //WORKS
+    uint16_t reg = drv->stall_thresh | DRV8711_STALL_SDCNT_8 | drv->stall_vdiv;
+    drv->last_thresh = drv->stall_thresh;
+    return DRV8711_WRITE(DRV8711_STALL_REG, reg);
+  }
+
+  case SS_WRITE_DRIVE: return DRV8711_WRITE(DRV8711_DRIVE_REG, DRV8711_DRIVE);
+
+  case SS_WRITE_TORQUE:
+    drv->last_torque = _driver_get_torque(drv);
+    return DRV8711_WRITE(DRV8711_TORQUE_REG, DRV8711_TORQUE | drv->last_torque);
+
+  case SS_WRITE_CTRL: {
+    // NOTE, we disable the driver if it's not active.  The chip gets hot
+    // idling with the driver enabled.
+    bool enable = _driver_get_torque(drv);
+    drv->last_microstep = drv->microstep;
+    return DRV8711_WRITE(DRV8711_CTRL_REG, DRV8711_CTRL |
+                         (drv->microstep << 3) |
+                         (enable ? DRV8711_CTRL_ENBL_bm : 0));
+  }
+
+  case SS_READ_OFF:    return DRV8711_READ(DRV8711_OFF_REG);
+  case SS_READ_STATUS: return DRV8711_READ(DRV8711_STATUS_REG);
+
+  case SS_CLEAR_STATUS:
+    drv->reset_flags = false;
+    drv->flags = 0;
+    return DRV8711_WRITE(DRV8711_STATUS_REG, 0x0fff & ~drv->status);
+  }
+
+  return 0; // Should not get here
+}
+
+
+static spi_state_t _driver_spi_next(drv8711_driver_t *drv) {
+  // Process response
+  switch (drv->spi_state) {
+  case SS_READ_OFF:
+    // We read back the OFF register to test for communication failure.
+    if ((spi.response & 0x1ff) != DRV8711_OFF)
+      drv->flags |= DRV8711_COMM_ERROR_bm;
+    else drv->flags &= ~DRV8711_COMM_ERROR_bm;
+    break;
+
+  case SS_READ_STATUS: {
+    drv->status = spi.response;
+
+    // NOTE If there is a power fault and the drivers are not powered
+    // then the status flags will read 0xff but the motor fault line will
+    // not be asserted.  So, fault flags are not valid with out motor fault.
+    // Also, a real stall cannot occur if the driver is inactive.
+    bool active = _driver_active(drv);
+    uint8_t mask =
+      ((motor_fault && !drv->reset_flags) ? 0xff : 0) | (active ? 0xc0 : 0);
+    drv->flags = (drv->flags & 0xff00) | (mask & drv->status);
+
+    // EStop on fatal driver faults
+    if (_driver_fault(drv)) estop_trigger(STAT_MOTOR_FAULT);
+    break;
+  }
+
+  default: break;
+  }
+
+  switch (drv->spi_state) {
+  case SS_READ_OFF:
+    if (drv->flags & DRV8711_COMM_ERROR_bm) return SS_WRITE_OFF; // Retry
+    break;
+
+  case SS_READ_STATUS:
+    if (drv->reset_flags) return SS_CLEAR_STATUS;
+    if (drv->last_torque != _driver_get_torque(drv)) return SS_WRITE_TORQUE;
+    if (drv->last_microstep != drv->microstep) return SS_WRITE_CTRL;
+    if (drv->last_thresh != drv->stall_thresh) return SS_WRITE_STALL;
+    // Fall through
+
+  case SS_CLEAR_STATUS: return SS_READ_OFF;
+
+  default: break;
+  }
+
+  return (spi_state_t)(drv->spi_state + 1); // Next
+}
+
+
+static void _spi_send() {
+  drv8711_driver_t *drv = &drivers[spi.driver];
+
+  // Flush any status errors (TODO check SPI errors)
+  uint8_t x = SPIC.STATUS;
+  x = x;
+
+  // Read byte
+  *DRV8711_WORD_BYTE_PTR(spi.response, !spi.low_byte) = SPIC.DATA;
+
+  // Advance state and process response
+  if (spi.advance) {
+    spi.advance = false;
+
+    // Handle response and set next state
+    drv->spi_state = _driver_spi_next(drv);
+
+    // Next driver
+    if (++spi.driver == DRIVERS) spi.driver = 0; // Wrap around
+    drv = &drivers[spi.driver];
+  }
+
+  // Disable CS
+  if (spi.disable_cs_pin) {
+    OUTCLR_PIN(spi.disable_cs_pin); // Set low (inactive)
+    _delay_us(1);
+    spi.disable_cs_pin = 0;
+  }
+
+  if (spi.low_byte) {
+    spi.disable_cs_pin = drv->cs_pin; // Schedule next CS disable
+    spi.advance = true; // Word complete
+
+  } else {
+    // Enable CS
+    OUTSET_PIN(drv->cs_pin); // Set high (active)
+    _delay_us(1);
+
+    // Get next command
+    spi.command = _driver_spi_command(drv);
+  }
+
+  // Write byte and prep next read
+  SPIC.DATA = *DRV8711_WORD_BYTE_PTR(spi.command, spi.low_byte);
+
+  // Next byte
+  spi.low_byte = !spi.low_byte;
+}
+
+
+ISR(SPIC_INT_vect) {_spi_send();}
+
+
+static void _stall_change(int driver, bool stalled) {
+  drivers[driver].stalled = stalled;
+
+  // Call stall callback
+  if (stalled && drivers[driver].stall_cb)
+    drivers[driver].stall_cb(driver);
+}
+
+
+static void _stall_switch_cb(switch_id_t sw, bool active) {
+  switch (sw) {
+  case SW_STALL_0: _stall_change(0, active); break;
+  case SW_STALL_1: _stall_change(1, active); break;
+  case SW_STALL_2: _stall_change(2, active); break;
+  case SW_STALL_3: _stall_change(3, active); break;
+  default: break;
+  }
+}
+
+
+static void _motor_fault_switch_cb(switch_id_t sw, bool active) {
+  motor_fault = active;
+}
+
+
+void drv8711_init() {
+  // Setup pins
+  // Must set the SS pin either in/high or out/any for master mode to work
+  // Note, this pin is also used by the USART as the CTS line
+  DIRSET_PIN(SPI_SS_PIN);   // Output
+  OUTSET_PIN(SPI_CLK_PIN);  // High
+  DIRSET_PIN(SPI_CLK_PIN);  // Output
+  DIRCLR_PIN(SPI_MISO_PIN); // Input
+  OUTSET_PIN(SPI_MOSI_PIN); // High
+  DIRSET_PIN(SPI_MOSI_PIN); // Output
+
+  // Motor driver enable
+  OUTSET_PIN(MOTOR_ENABLE_PIN); // Active high
+  DIRSET_PIN(MOTOR_ENABLE_PIN); // Output
+
+  for (int i = 0; i < DRIVERS; i++) {
+    uint8_t cs_pin = drivers[i].cs_pin;
+    OUTSET_PIN(cs_pin);     // High
+    DIRSET_PIN(cs_pin);     // Output
+
+    switch_id_t stall_sw = drivers[i].stall_sw;
+    switch_set_type(stall_sw, SW_NORMALLY_OPEN);
+    switch_set_callback(stall_sw, _stall_switch_cb);
+
+    drivers[i].reset_flags = true; // Reset flags once on startup
+  }
+
+  switch_set_type(SW_MOTOR_FAULT, SW_NORMALLY_OPEN);
+  switch_set_callback(SW_MOTOR_FAULT, _motor_fault_switch_cb);
+
+  // Configure SPI
+  PR.PRPC &= ~PR_SPI_bm; // Disable power reduction
+  SPIC.CTRL = SPI_ENABLE_bm | SPI_MASTER_bm | SPI_MODE_0_gc |
+    SPI_PRESCALER_DIV16_gc; // enable, big endian, master, mode, clock div
+  PIN_PORT(SPI_CLK_PIN)->REMAP = PORT_SPI_bm; // Swap SCK and MOSI
+  SPIC.INTCTRL = SPI_INTLVL_LO_gc; // interupt level
+
+  _spi_send(); // Kick it off
+}
+
+
+drv8711_state_t drv8711_get_state(int driver) {
+  if (driver < 0 || DRIVERS <= driver) return DRV8711_DISABLED;
+  return drivers[driver].state;
+}
+
+
+void drv8711_set_state(int driver, drv8711_state_t state) {
+  if (driver < 0 || DRIVERS <= driver) return;
+  drivers[driver].state = state;
+}
+
+
+void drv8711_set_microsteps(int driver, uint16_t msteps) {
+  if (driver < 0 || DRIVERS <= driver) return;
+  switch (msteps) {
+  case 1: case 2: case 4: case 8: case 16: case 32: case 64: case 128: case 256:
+    break;
+  default: return; // Invalid
+  }
+
+  drivers[driver].microstep = round(logf(msteps) / logf(2));
+}
+
+
+
+void drv8711_set_stall_callback(int driver, stall_callback_t cb) {
+  drivers[driver].stall_cb = cb;
+}
+
+
+float get_drive_current(int driver) {
+  if (driver < 0 || DRIVERS <= driver) return 0;
+  return drivers[driver].drive.current;
+}
+
+
+void set_drive_current(int driver, float value) {
+  if (driver < 0 || DRIVERS <= driver || value < 0) return;
+  if (MAX_CURRENT < value) value = MAX_CURRENT;
+  _current_set(&drivers[driver].drive, value);
+}
+
+
+float get_idle_current(int driver) {
+  if (driver < 0 || DRIVERS <= driver) return 0;
+  return drivers[driver].idle.current;
+}
+
+
+void set_idle_current(int driver, float value) {
+  if (driver < 0 || DRIVERS <= driver || value < 0 || MAX_IDLE_CURRENT < value)
+    return;
+
+  _current_set(&drivers[driver].idle, value);
+}
+
+
+float get_active_current(int driver) {
+  if (driver < 0 || DRIVERS <= driver) return 0;
+  return _driver_get_current(&drivers[driver]);
+}
+
+
+bool get_motor_fault() {return motor_fault;}
+
+
+void set_driver_flags(int driver, uint16_t flags) {
+  drivers[driver].reset_flags = true;
+}
+
+
+uint16_t get_driver_flags(int driver) {return drivers[driver].flags;}
+bool get_driver_stalled(int driver) {return drivers[driver].stalled;}
+
+
+float get_stall_volts(int driver) {
+  if (driver < 0 || DRIVERS <= driver) return 0;
+
+  float vdiv;
+  switch (drivers[driver].stall_vdiv) {
+  case DRV8711_STALL_VDIV_4:  vdiv =  4; break;
+  case DRV8711_STALL_VDIV_8:  vdiv =  8; break;
+  case DRV8711_STALL_VDIV_16: vdiv = 16; break;
+  default:                    vdiv = 32; break;
+  }
+
+  return 1.8 / 256 * vdiv * drivers[driver].stall_thresh;
+}
+
+
+void set_stall_volts(int driver, float volts) {
+  if (driver < 0 || DRIVERS <= driver) return;
+
+  uint16_t vdiv = DRV8711_STALL_VDIV_32;
+  uint16_t thresh = (uint16_t)(volts * 256 / 1.8);
+
+  if (thresh < 4 << 8) {
+    thresh >>= 2;
+    vdiv = DRV8711_STALL_VDIV_4;
+
+  } else if (thresh < 8 << 8) {
+    thresh >>= 3;
+    vdiv = DRV8711_STALL_VDIV_8;
+
+  } else if (thresh < 16 << 8) {
+    thresh >>= 4;
+    vdiv = DRV8711_STALL_VDIV_16;
+
+  } else {
+    if (thresh < 32 << 8) thresh >>= 5;
+    else thresh = 255;
+  }
+
+  drivers[driver].stall_vdiv = vdiv;
+  drivers[driver].stall_thresh = thresh;
+}
+
+void set_stall_sample_time(int driver, uint16_t sample_time)
+{
+  if (driver < 0 || DRIVERS <= driver) return;
+
+  switch (sample_time) {
+  case 50: drivers[driver].sample_time = DRV8711_TORQUE_SMPLTH_50; break;
+  case 100: drivers[driver].sample_time = DRV8711_TORQUE_SMPLTH_100; break;
+  case 200: drivers[driver].sample_time = DRV8711_TORQUE_SMPLTH_200; break;
+  case 300: drivers[driver].sample_time = DRV8711_TORQUE_SMPLTH_300; break;
+  case 400: drivers[driver].sample_time = DRV8711_TORQUE_SMPLTH_400; break;
+  case 600: drivers[driver].sample_time = DRV8711_TORQUE_SMPLTH_600; break;
+  case 800: drivers[driver].sample_time = DRV8711_TORQUE_SMPLTH_800; break;
+  case 1000: drivers[driver].sample_time = DRV8711_TORQUE_SMPLTH_1000; break;
+  default: return; // Invalid
+  }
+
+}
+
+uint16_t get_stall_sample_time(int driver)
+{
+  if (driver < 0 || DRIVERS <= driver) return 0;
+
+  uint16_t sample_time = 0;
+  switch (drivers[driver].sample_time) {
+  case DRV8711_TORQUE_SMPLTH_50:   sample_time = 50; break;
+  case DRV8711_TORQUE_SMPLTH_100:  sample_time = 100; break;
+  case DRV8711_TORQUE_SMPLTH_200:  sample_time = 200; break;
+  case DRV8711_TORQUE_SMPLTH_300:  sample_time = 300; break;
+  case DRV8711_TORQUE_SMPLTH_400:  sample_time = 400; break;
+  case DRV8711_TORQUE_SMPLTH_600:  sample_time = 600; break;
+  case DRV8711_TORQUE_SMPLTH_800:  sample_time = 800; break;
+  case DRV8711_TORQUE_SMPLTH_1000:  sample_time = 1000; break;
+  default: sample_time = 50; break;
+  }
+
+  return sample_time;
+}
+
+float get_stall_current(int driver) {
+  if (driver < 0 || DRIVERS <= driver) return 0;
+  return drivers[driver].stall.current;
+}
+
+
+void set_stall_current(int driver, float value) {
+  if (driver < 0 || DRIVERS <= driver || value < 0) return;
+  if (MAX_CURRENT < value) value = MAX_CURRENT;
+  _current_set(&drivers[driver].stall, value);
+}

src/avr/src/drv8711.h

@@ -0,0 +1,188 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include "config.h"
+#include "status.h"
+#include "motor.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+
+enum {
+  DRV8711_CTRL_REG,
+  DRV8711_TORQUE_REG,
+  DRV8711_OFF_REG,
+  DRV8711_BLANK_REG,
+  DRV8711_DECAY_REG,
+  DRV8711_STALL_REG,
+  DRV8711_DRIVE_REG,
+  DRV8711_STATUS_REG,
+};
+
+
+enum {
+  DRV8711_CTRL_ENBL_bm            = 1 << 0,
+  DRV8711_CTRL_RDIR_bm            = 1 << 1,
+  DRV8711_CTRL_RSTEP_bm           = 1 << 2,
+  DRV8711_CTRL_MODE_1             = 0 << 3,
+  DRV8711_CTRL_MODE_2             = 1 << 3,
+  DRV8711_CTRL_MODE_4             = 2 << 3,
+  DRV8711_CTRL_MODE_8             = 3 << 3,
+  DRV8711_CTRL_MODE_16            = 4 << 3,
+  DRV8711_CTRL_MODE_32            = 5 << 3,
+  DRV8711_CTRL_MODE_64            = 6 << 3,
+  DRV8711_CTRL_MODE_128           = 7 << 3,
+  DRV8711_CTRL_MODE_256           = 8 << 3,
+  DRV8711_CTRL_EXSTALL_bm         = 1 << 7,
+  DRV8711_CTRL_ISGAIN_5           = 0 << 8,
+  DRV8711_CTRL_ISGAIN_10          = 1 << 8,
+  DRV8711_CTRL_ISGAIN_20          = 2 << 8,
+  DRV8711_CTRL_ISGAIN_40          = 3 << 8,
+  DRV8711_CTRL_DTIME_400          = 0 << 10,
+  DRV8711_CTRL_DTIME_450          = 1 << 10,
+  DRV8711_CTRL_DTIME_650          = 2 << 10,
+  DRV8711_CTRL_DTIME_850          = 3 << 10,
+};
+
+
+enum {
+  DRV8711_TORQUE_SMPLTH_50        = 0 << 8,
+  DRV8711_TORQUE_SMPLTH_100       = 1 << 8,
+  DRV8711_TORQUE_SMPLTH_200       = 2 << 8,
+  DRV8711_TORQUE_SMPLTH_300       = 3 << 8,
+  DRV8711_TORQUE_SMPLTH_400       = 4 << 8,
+  DRV8711_TORQUE_SMPLTH_600       = 5 << 8,
+  DRV8711_TORQUE_SMPLTH_800       = 6 << 8,
+  DRV8711_TORQUE_SMPLTH_1000      = 7 << 8,
+};
+
+
+enum {
+  DRV8711_OFF_PWMMODE_bm          = 1 << 8,
+};
+
+
+enum {
+  DRV8711_BLANK_ABT_bm            = 1 << 8,
+};
+
+
+enum {
+  DRV8711_DECAY_DECMOD_SLOW       = 0 << 8,
+  DRV8711_DECAY_DECMOD_OPT        = 1 << 8,
+  DRV8711_DECAY_DECMOD_FAST       = 2 << 8,
+  DRV8711_DECAY_DECMOD_MIXED      = 3 << 8,
+  DRV8711_DECAY_DECMOD_AUTO_OPT   = 4 << 8,
+  DRV8711_DECAY_DECMOD_AUTO_MIXED = 5 << 8,
+};
+
+
+enum {
+  DRV8711_STALL_SDCNT_1           = 0 << 8,
+  DRV8711_STALL_SDCNT_2           = 1 << 8,
+  DRV8711_STALL_SDCNT_4           = 2 << 8,
+  DRV8711_STALL_SDCNT_8           = 3 << 8,
+  DRV8711_STALL_VDIV_32           = 0 << 10,
+  DRV8711_STALL_VDIV_16           = 1 << 10,
+  DRV8711_STALL_VDIV_8            = 2 << 10,
+  DRV8711_STALL_VDIV_4            = 3 << 10,
+};
+
+
+enum {
+  DRV8711_DRIVE_OCPTH_250         = 0 << 0,
+  DRV8711_DRIVE_OCPTH_500         = 1 << 0,
+  DRV8711_DRIVE_OCPTH_750         = 2 << 0,
+  DRV8711_DRIVE_OCPTH_1000        = 3 << 0,
+  DRV8711_DRIVE_OCPDEG_1          = 0 << 2,
+  DRV8711_DRIVE_OCPDEG_2          = 1 << 2,
+  DRV8711_DRIVE_OCPDEG_4          = 2 << 2,
+  DRV8711_DRIVE_OCPDEG_8          = 3 << 2,
+  DRV8711_DRIVE_TDRIVEN_250       = 0 << 4,
+  DRV8711_DRIVE_TDRIVEN_500       = 1 << 4,
+  DRV8711_DRIVE_TDRIVEN_1000      = 2 << 4,
+  DRV8711_DRIVE_TDRIVEN_2000      = 3 << 4,
+  DRV8711_DRIVE_TDRIVEP_250       = 0 << 6,
+  DRV8711_DRIVE_TDRIVEP_500       = 1 << 6,
+  DRV8711_DRIVE_TDRIVEP_1000      = 2 << 6,
+  DRV8711_DRIVE_TDRIVEP_2000      = 3 << 6,
+  DRV8711_DRIVE_IDRIVEN_100       = 0 << 8,
+  DRV8711_DRIVE_IDRIVEN_200       = 1 << 8,
+  DRV8711_DRIVE_IDRIVEN_300       = 2 << 8,
+  DRV8711_DRIVE_IDRIVEN_400       = 3 << 8,
+  DRV8711_DRIVE_IDRIVEP_50        = 0 << 10,
+  DRV8711_DRIVE_IDRIVEP_100       = 1 << 10,
+  DRV8711_DRIVE_IDRIVEP_150       = 2 << 10,
+  DRV8711_DRIVE_IDRIVEP_200       = 3 << 10,
+};
+
+
+enum {
+  DRV8711_STATUS_OTS_bm           = 1 << 0,
+  DRV8711_STATUS_AOCP_bm          = 1 << 1,
+  DRV8711_STATUS_BOCP_bm          = 1 << 2,
+  DRV8711_STATUS_APDF_bm          = 1 << 3,
+  DRV8711_STATUS_BPDF_bm          = 1 << 4,
+  DRV8711_STATUS_UVLO_bm          = 1 << 5,
+  DRV8711_STATUS_STD_bm           = 1 << 6,
+  DRV8711_STATUS_STDLAT_bm        = 1 << 7,
+  DRV8711_COMM_ERROR_bm           = 1 << 8,
+};
+
+
+#define DRV8711_READ(ADDR) ((1 << 15) | ((ADDR) << 12))
+#define DRV8711_WRITE(ADDR, DATA) (((ADDR) << 12) | ((DATA) & 0xfff))
+#define DRV8711_CMD_ADDR(CMD) (((CMD) >> 12) & 7)
+#define DRV8711_CMD_IS_READ(CMD) ((1 << 15) & (CMD))
+
+#define DRV8711_CTRL_ISGAIN_BM (3 << 8)
+#define DRV8711_CTRL_GET_GAIN(CTRL) ((CTRL) & DRV8711_CTRL_ISGAIN_BM)
+
+#define DRV8711_CTRL_GAIN(CTRL)                                         \
+  (DRV8711_CTRL_GET_GAIN(CTRL) == DRV8711_CTRL_ISGAIN_5 ? 5 :           \
+   (DRV8711_CTRL_GET_GAIN(CTRL) == DRV8711_CTRL_ISGAIN_10 ? 10 :        \
+    (DRV8711_CTRL_GET_GAIN(CTRL) == DRV8711_CTRL_ISGAIN_20 ? 20 : 40)))
+
+
+typedef enum {
+  DRV8711_DISABLED,
+  DRV8711_IDLE,
+  DRV8711_ACTIVE,
+} drv8711_state_t;
+
+
+typedef void (*stall_callback_t)(int driver);
+
+
+void drv8711_init();
+drv8711_state_t drv8711_get_state(int driver);
+void drv8711_set_state(int driver, drv8711_state_t state);
+void drv8711_set_microsteps(int driver, uint16_t msteps);
+void drv8711_set_stall_callback(int driver, stall_callback_t cb);

src/avr/src/emu.h

@@ -0,0 +1,36 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#ifdef __AVR__
+#define emu_init()
+#define emu_callback()
+
+#else
+void emu_init();
+void emu_callback();
+
+#endif

src/avr/src/estop.c

@@ -0,0 +1,126 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "estop.h"
+#include "motor.h"
+#include "stepper.h"
+#include "spindle.h"
+#include "switch.h"
+#include "hardware.h"
+#include "config.h"
+#include "state.h"
+#include "outputs.h"
+#include "jog.h"
+#include "exec.h"
+
+
+static stat_t estop_reason = STAT_OK;
+
+
+static void _switch_callback(switch_id_t id, bool active) {
+  if (active) estop_trigger(STAT_ESTOP_SWITCH);
+  else estop_clear();
+}
+
+
+void estop_init() {
+  switch_set_callback(SW_ESTOP, _switch_callback);
+  if (switch_is_active(SW_ESTOP)) estop_trigger(STAT_ESTOP_SWITCH);
+}
+
+
+bool estop_triggered() {return estop_reason != STAT_OK;}
+
+
+void estop_trigger(stat_t reason) {
+  if (estop_triggered()) return;
+  estop_reason = reason;
+
+  // Set fault signal
+  outputs_set_active(FAULT_PIN, true);
+
+  // Shutdown peripherals
+  st_shutdown();
+  spindle_estop();
+  jog_stop();
+  outputs_stop();
+
+  // Set machine state
+  state_estop();
+}
+
+
+void estop_clear() {
+  // It is important that we don't clear the estop if it's not set because
+  // it can cause a reboot loop.
+  if (!estop_triggered()) return;
+
+  // Check if estop switch is set
+  if (switch_is_active(SW_ESTOP)) {
+    estop_reason = STAT_ESTOP_SWITCH;
+    return; // Can't clear while estop switch is still active
+  }
+
+  // Clear fault signal
+  outputs_set_active(FAULT_PIN, false);
+
+  estop_reason = STAT_OK;
+
+  // Reboot
+  // Note, hardware.c waits until any spindle stop command has been delivered
+  hw_request_hard_reset();
+}
+
+
+// Var callbacks
+bool get_estop() {return estop_triggered();}
+
+
+void set_estop(bool value) {
+  if (value == estop_triggered()) return;
+  if (value) estop_trigger(STAT_ESTOP_USER);
+  else estop_clear();
+}
+
+
+PGM_P get_estop_reason() {return status_to_pgmstr(estop_reason);}
+
+
+// Command callbacks
+stat_t command_estop(char *cmd) {
+  estop_trigger(STAT_ESTOP_USER);
+  return STAT_OK;
+}
+
+
+stat_t command_shutdown(char *cmd) {
+  estop_trigger(STAT_POWER_SHUTDOWN);
+  return STAT_OK;
+}
+
+
+stat_t command_clear(char *cmd) {estop_clear(); return STAT_OK;}

src/avr/src/estop.h

@@ -0,0 +1,42 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "status.h"
+
+#include <stdbool.h>
+
+
+void estop_init();
+bool estop_triggered();
+void estop_trigger(stat_t reason);
+void estop_clear();
+
+
+#define ESTOP_ASSERT(COND, CODE)                    \
+  do {if (!(COND)) estop_trigger(CODE);} while (0)

src/avr/src/exec.c

@@ -0,0 +1,306 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "exec.h"
+
+#include "stepper.h"
+#include "motor.h"
+#include "axis.h"
+#include "util.h"
+#include "command.h"
+#include "switch.h"
+#include "seek.h"
+#include "estop.h"
+#include "state.h"
+#include "spindle.h"
+#include "config.h"
+#include "SCurve.h"
+
+
+static struct {
+  exec_cb_t cb;
+
+  float position[AXES];
+  float velocity;
+  float accel;
+  float jerk;
+  float peak_vel;
+  float peak_accel;
+
+  float feed_override;
+
+  struct {
+    float target[AXES];
+    float time;
+    float vel;
+    float accel;
+    float max_accel;
+    float max_jerk;
+    power_update_t power_updates[2 * POWER_MAX_UPDATES];
+    exec_cb_t cb;
+  } seg;
+} ex;
+
+
+static void _limit_switch_cb(switch_id_t sw, bool active) {
+  if (sw == seek_get_switch()) return;
+  if (ex.velocity && active) estop_trigger(STAT_ESTOP_SWITCH);
+}
+
+
+void exec_init() {
+  memset(&ex, 0, sizeof(ex));
+  ex.feed_override = 1; // TODO implement feed override
+
+  // Set callback for limit switches
+  for (int sw = SW_MIN_0; sw <= SW_MAX_3; sw++)
+    switch_set_callback((switch_id_t)sw, _limit_switch_cb);
+}
+
+
+void exec_get_position(float p[AXES]) {
+  memcpy(p, ex.position, sizeof(ex.position));
+}
+
+
+float exec_get_axis_position(int axis) {return ex.position[axis];}
+
+
+void exec_set_velocity(float v) {
+  ex.velocity = v;
+  if (ex.peak_vel < v) ex.peak_vel = v;
+}
+
+
+float exec_get_velocity() {return ex.velocity;}
+
+
+void exec_set_acceleration(float a) {
+  ex.accel = a;
+  if (ex.peak_accel < a) ex.peak_accel = a;
+}
+
+
+float exec_get_acceleration() {return ex.accel;}
+void exec_set_jerk(float j) {ex.jerk = j;}
+
+
+void exec_set_cb(exec_cb_t cb) {ex.cb = cb;}
+
+
+void exec_move_to_target(const float target[]) {
+  ESTOP_ASSERT(isfinite(target[AXIS_X]) && isfinite(target[AXIS_Y]) &&
+               isfinite(target[AXIS_Z]) && isfinite(target[AXIS_A]) &&
+               isfinite(target[AXIS_B]) && isfinite(target[AXIS_C]),
+               STAT_BAD_FLOAT);
+
+  // Prep power updates
+  st_prep_power(ex.seg.power_updates);
+
+  // Shift power updates
+  for (unsigned i = 0; i < POWER_MAX_UPDATES; i++) {
+    ex.seg.power_updates[i] = ex.seg.power_updates[i + POWER_MAX_UPDATES];
+    ex.seg.power_updates[i + POWER_MAX_UPDATES].state = POWER_IGNORE;
+  }
+
+  // Update position
+  copy_vector(ex.position, target);
+
+  // Call the stepper prep function
+  st_prep_line(target);
+}
+
+
+stat_t _segment_exec() {
+  float t = ex.seg.time;
+  float v = ex.seg.vel;
+  float a = ex.seg.accel;
+
+  // Handle pause
+  if (state_get() == STATE_STOPPING) {
+    a = SCurve::nextAccel(SEGMENT_TIME, 0, ex.velocity, ex.accel,
+                          ex.seg.max_accel, ex.seg.max_jerk);
+    v = ex.velocity + SEGMENT_TIME * a;
+    t *= ex.seg.vel / v;
+
+    if (v < MIN_VELOCITY || seek_switch_found()) {
+      t = v = 0;
+      ex.seg.cb = 0;
+      command_reset_position();
+      state_holding();
+      seek_end();
+      spindle_update_speed();
+    }
+  }
+
+  // Wait for next seg if time is too short and we are still moving
+  if (t < SEGMENT_TIME && (!t || v)) {
+    if (!v) {
+      exec_set_velocity(0);
+      exec_set_acceleration(0);
+      exec_set_jerk(0);
+      ex.seg.time = 0;
+    }
+    ex.cb = ex.seg.cb;
+    return STAT_AGAIN;
+  }
+
+  // Update velocity and accel
+  exec_set_velocity(v);
+  exec_set_acceleration(a);
+
+  if (t <= SEGMENT_TIME) {
+    // Move
+    exec_move_to_target(ex.seg.target);
+    ex.seg.time = 0;
+
+  } else {
+    // Compute next target
+    float ratio = SEGMENT_TIME / t;
+    float target[AXES];
+    for (int axis = 0; axis < AXES; axis++) {
+      float diff = ex.seg.target[axis] - ex.position[axis];
+      target[axis] = ex.position[axis] + ratio * diff;
+    }
+
+    // Move
+    exec_move_to_target(target);
+
+    // Update time
+    if (t == ex.seg.time) ex.seg.time -= SEGMENT_TIME;
+    else ex.seg.time -= SEGMENT_TIME * v / ex.seg.vel;
+  }
+
+  // Check switch
+  if (seek_switch_found()) state_seek_hold();
+
+  return STAT_OK;
+}
+
+
+stat_t exec_segment(float time, const float target[], float vel, float accel,
+                    float maxAccel, float maxJerk,
+                    const power_update_t power_updates[]) {
+  // Copy power updates in to the correct position given the time offset
+  float nextT = ex.seg.time + time;
+  const float stepT = 1.0 / 60000; // 1ms in mins
+  float t = 0.5 / 60000; // 0.5ms in mins
+  unsigned j = 0;
+  for (unsigned i = 0; t < nextT && j < POWER_MAX_UPDATES; i++) {
+    if (ex.seg.time < t) ex.seg.power_updates[i] = power_updates[j++];
+    t += stepT;
+  }
+
+  copy_vector(ex.seg.target, target);
+  ex.seg.time = nextT;
+  ex.seg.vel = vel;
+  ex.seg.accel = accel;
+  ex.seg.max_accel = maxAccel;
+  ex.seg.max_jerk = maxJerk;
+  ex.seg.cb = ex.cb;
+  ex.cb = _segment_exec;
+
+  // TODO To be precise, seek_end() should not be called until the current
+  // segment has completed execution.
+  if (!ex.seg.cb) seek_end(); // No callback when at line end
+
+  return _segment_exec();
+}
+
+
+// Called by stepper.c from low-level interrupt
+stat_t exec_next() {
+  // Hold if we've reached zero velocity between commands and stopping
+  if (!ex.cb && !exec_get_velocity() && state_get() == STATE_STOPPING)
+    state_holding();
+
+  if (state_get() == STATE_HOLDING) return STAT_NOP;
+  if (!ex.cb && !command_exec()) return STAT_NOP; // Queue empty
+  if (!ex.cb) return STAT_AGAIN; // Non-exec command
+  return ex.cb(); // Exec
+}
+
+
+// Variable callbacks
+float get_axis_position(int axis) {return ex.position[axis];}
+float get_velocity() {return ex.velocity / VELOCITY_MULTIPLIER;}
+float get_acceleration() {return ex.accel / ACCEL_MULTIPLIER;}
+float get_jerk() {return ex.jerk / JERK_MULTIPLIER;}
+float get_peak_vel() {return ex.peak_vel / VELOCITY_MULTIPLIER;}
+void set_peak_vel(float x) {ex.peak_vel = 0;}
+float get_peak_accel() {return ex.peak_accel / ACCEL_MULTIPLIER;}
+void set_peak_accel(float x) {ex.peak_accel = 0;}
+uint16_t get_feed_override() {return ex.feed_override * 1000;}
+void set_feed_override(uint16_t value) {ex.feed_override = value / 1000.0;}
+
+
+// Command callbacks
+typedef struct {
+  uint8_t axis;
+  float position;
+} set_axis_t;
+
+
+stat_t command_set_axis(char *cmd) {
+  cmd++; // Skip command name
+
+  // Decode axis
+  int axis = axis_get_id(*cmd++);
+  if (axis < 0) return STAT_INVALID_ARGUMENTS;
+
+  // Decode position
+  float position;
+  if (!decode_float(&cmd, &position)) return STAT_BAD_FLOAT;
+
+  // Check for end of command
+  if (*cmd) return STAT_INVALID_ARGUMENTS;
+
+  // Update command
+  command_set_axis_position((uint8_t)axis, position);
+
+  // Queue
+  set_axis_t set_axis = {(uint8_t)axis, position};
+  command_push(COMMAND_set_axis, &set_axis);
+
+  return STAT_OK;
+}
+
+
+unsigned command_set_axis_size() {return sizeof(set_axis_t);}
+
+
+void command_set_axis_exec(void *data) {
+  set_axis_t *cmd = (set_axis_t *)data;
+
+  // Update exec
+  ex.position[cmd->axis] = cmd->position;
+
+  // Update motors
+  for (int motor = 0; motor < MOTORS; motor++)
+    if (motor_get_axis(motor) == cmd->axis)
+      motor_set_position(motor, cmd->position);
+}

src/avr/src/exec.h

@@ -0,0 +1,59 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include "config.h"
+#include "spindle.h"
+#include "status.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+
+typedef stat_t (*exec_cb_t)();
+
+
+void exec_init();
+
+void exec_get_position(float p[AXES]);
+float exec_get_axis_position(int axis);
+float exec_get_power_scale();
+void exec_set_velocity(float v);
+float exec_get_velocity();
+void exec_set_acceleration(float a);
+float exec_get_acceleration();
+void exec_set_jerk(float j);
+
+void exec_set_cb(exec_cb_t cb);
+
+void exec_move_to_target(const float target[]);
+stat_t exec_segment(float time, const float target[], float vel, float accel,
+                    float maxAccel, float maxJerk,
+                    const power_update_t power_updates[]);
+stat_t exec_next();

src/avr/src/hardware.c

@@ -0,0 +1,158 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "hardware.h"
+#include "rtc.h"
+#include "usart.h"
+#include "config.h"
+#include "pgmspace.h"
+
+#include <avr/interrupt.h>
+#include <avr/eeprom.h>
+#include <avr/wdt.h>
+
+#include <stdbool.h>
+#include <stddef.h>
+
+
+typedef struct {
+  char id[26];
+  bool hard_reset;         // flag to perform a hard reset
+} hw_t;
+
+static hw_t hw = {{0}};
+
+
+#define PROD_SIGS (*(NVM_PROD_SIGNATURES_t *)0x0000)
+#define HEXNIB(x) "0123456789abcdef"[(x) & 0xf]
+
+
+static void _init_clock()  {
+#if 0 // 32Mhz Int RC
+  OSC.CTRL |= OSC_RC32MEN_bm | OSC_RC32KEN_bm;  // Enable 32MHz & 32KHz osc
+  while (!(OSC.STATUS & OSC_RC32KRDY_bm));      // Wait for 32Khz oscillator
+  while (!(OSC.STATUS & OSC_RC32MRDY_bm));      // Wait for 32MHz oscillator
+
+  // Defaults to calibrate against internal 32Khz clock
+  DFLLRC32M.CTRL = DFLL_ENABLE_bm;              // Enable DFLL
+  CCP = CCP_IOREG_gc;                           // Disable register security
+  CLK.CTRL = CLK_SCLKSEL_RC32M_gc;              // Switch to 32MHz clock
+
+#else
+  // 12-16 MHz crystal; 0.4-16 MHz XTAL w/ 16K CLK startup
+  OSC.XOSCCTRL = OSC_FRQRANGE_12TO16_gc | OSC_XOSCSEL_XTAL_16KCLK_gc;
+  OSC.CTRL = OSC_XOSCEN_bm;                // enable external crystal oscillator
+  while (!(OSC.STATUS & OSC_XOSCRDY_bm));  // wait for oscillator ready
+
+  OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | 2;    // PLL source, 2x (32 MHz sys clock)
+  OSC.CTRL = OSC_PLLEN_bm | OSC_XOSCEN_bm; // Enable PLL & External Oscillator
+  while (!(OSC.STATUS & OSC_PLLRDY_bm));   // wait for PLL ready
+
+  CCP = CCP_IOREG_gc;
+  CLK.CTRL = CLK_SCLKSEL_PLL_gc;           // switch to PLL clock
+#endif
+
+  OSC.CTRL &= ~OSC_RC2MEN_bm;              // disable internal 2 MHz clock
+}
+
+
+#ifdef __AVR__
+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.id[i] = HEXNIB(byte >> 4);
+  hw.id[i + 1] = HEXNIB(byte);
+}
+#endif // __AVR__
+
+
+static void _read_hw_id() {
+#ifdef __AVR__
+  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;
+
+#else // __AVR__
+  for (int i = 0; i < 25; i++)
+    hw.id[i] = '0';
+  hw.id[25] = 0;
+#endif // __AVR__
+}
+
+
+/// Lowest level hardware init
+void hw_init() {
+  _init_clock();                           // set system clock
+  rtc_init();                              // real time counter
+  _read_hw_id();
+
+  // 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() {hw.hard_reset = true;}
+
+
+static void _hard_reset() {
+  usart_flush();
+  cli();
+  CCP = CCP_IOREG_gc;
+  RST.CTRL = RST_SWRST_bm;
+}
+
+
+/// Controller's rest handler
+void hw_reset_handler() {
+  if (!hw.hard_reset) return;
+
+  // Flush serial port and wait for EEPROM writes to finish
+  while (!usart_tx_empty() || !eeprom_is_ready())
+    continue;
+
+  _hard_reset();
+}
+
+
+const char *get_hw_id() {return hw.id;}

src/avr/src/hardware.h

@@ -0,0 +1,40 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "status.h"
+
+#include <stdint.h>
+
+
+void hw_init();
+void hw_request_hard_reset();
+void hw_reset_handler();
+
+uint8_t hw_disable_watchdog();
+void hw_restore_watchdog(uint8_t state);

src/avr/src/huanyang.c

@@ -0,0 +1,301 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "huanyang.h"
+#include "config.h"
+#include "modbus.h"
+#include "estop.h"
+
+#include <util/atomic.h>
+
+#include <string.h>
+#include <math.h>
+
+
+/*
+  Huanyang is not quite Modbus compliant.
+
+  Message format is:
+
+    [id][func][length][data][checksum]
+
+  Where:
+
+     id       - 1-byte Peer ID
+     func     - 1-byte One of hy_func_t
+     length   - 1-byte Length data in bytes
+     data     - length bytes - Command arguments
+     checksum - 16-bit CRC: x^16 + x^15 + x^2 + 1 (0xa001) initial: 0xffff
+*/
+
+
+// See VFD manual pg56 3.1.3
+typedef enum {
+  HUANYANG_FUNC_READ = 1, // [len=1][hy_addr_t]
+  HUANYANG_FUNC_WRITE,    // [len=3][hy_addr_t][data]
+  HUANYANG_CTRL_WRITE,    // [len=1][hy_ctrl_state_t]
+  HUANYANG_CTRL_READ,     // [len=1][hy_ctrl_addr_t]
+  HUANYANG_FREQ_WRITE,    // [len=2][freq]
+  HUANYANG_RESERVED_1,
+  HUANYANG_RESERVED_2,
+  HUANYANG_LOOP_TEST,
+} hy_func_t;
+
+
+// Sent in HUANYANG_CTRL_WRITE
+// See VFD manual pg57 3.1.3.c
+typedef enum {
+  HUANYANG_RUN         = 1 << 0,
+  HUANYANG_FORWARD     = 1 << 1,
+  HUANYANG_REVERSE     = 1 << 2,
+  HUANYANG_STOP        = 1 << 3,
+  HUANYANG_REV_FWD     = 1 << 4,
+  HUANYANG_JOG         = 1 << 5,
+  HUANYANG_JOG_FORWARD = 1 << 6,
+  HUANYANG_JOG_REVERSE = 1 << 7,
+} hy_ctrl_state_t;
+
+
+// Returned by HUANYANG_CTRL_WRITE
+// See VFD manual pg57 3.1.3.c
+typedef enum {
+  HUANYANG_STATUS_RUN         = 1 << 0,
+  HUANYANG_STATUS_JOG         = 1 << 1,
+  HUANYANG_STATUS_COMMAND_REV = 1 << 2,
+  HUANYANG_STATUS_RUNNING     = 1 << 3,
+  HUANYANG_STATUS_JOGGING     = 1 << 4,
+  HUANYANG_STATUS_JOGGING_REV = 1 << 5,
+  HUANYANG_STATUS_BRAKING     = 1 << 6,
+  HUANYANG_STATUS_TRACK_START = 1 << 7,
+} hy_ctrl_status_t;
+
+
+// Sent in HUANYANG_CTRL_READ
+// See VFD manual pg57 3.1.3.d
+typedef enum {
+  HUANYANG_TARGET_FREQ,
+  HUANYANG_ACTUAL_FREQ,
+  HUANYANG_ACTUAL_CURRENT,
+  HUANYANG_ACTUAL_RPM,
+  HUANYANG_DCV,
+  HUANYANG_ACV,
+  HUANYANG_COUNTER,
+  HUANYANG_TEMPERATURE,
+} hy_ctrl_addr_t;
+
+
+static struct {
+  uint8_t state;
+
+  deinit_cb_t deinit_cb;
+  bool shutdown;
+  bool changed;
+  float power;
+
+  float actual_freq;
+  float actual_current;
+  uint16_t actual_rpm;
+  uint16_t temperature;
+
+  float max_freq;
+  float min_freq;
+  uint16_t rated_rpm;
+
+  uint8_t status;
+} hy;
+
+
+static void _func_read_response(hy_addr_t addr, uint16_t value) {
+  switch (addr) {
+  case HY_PD005_MAX_FREQUENCY:         hy.max_freq  = value * 0.01; break;
+  case HY_PD011_FREQUENCY_LOWER_LIMIT: hy.min_freq  = value * 0.01; break;
+  case HY_PD144_RATED_MOTOR_RPM:       hy.rated_rpm = value;        break;
+  default: break;
+  }
+}
+
+
+static void _ctrl_read_response(hy_ctrl_addr_t addr, uint16_t value) {
+  switch (addr) {
+  case HUANYANG_ACTUAL_FREQ:    hy.actual_freq    = value * 0.01; break;
+  case HUANYANG_ACTUAL_CURRENT: hy.actual_current = value * 0.01; break;
+  case HUANYANG_ACTUAL_RPM:     hy.actual_rpm     = value;        break;
+  case HUANYANG_TEMPERATURE:    hy.temperature    = value;        break;
+  default: break;
+  }
+}
+
+
+static uint16_t _read_word(const uint8_t *data) {
+  return (uint16_t)data[0] << 8 | data[1];
+}
+
+
+static void _handle_response(hy_func_t func, const uint8_t *data) {
+  switch (func) {
+  case HUANYANG_FUNC_READ:
+    _func_read_response((hy_addr_t)*data, _read_word(data + 1));
+    break;
+
+  case HUANYANG_FUNC_WRITE: break;
+  case HUANYANG_CTRL_WRITE: hy.status = *data; break;
+
+  case HUANYANG_CTRL_READ:
+    _ctrl_read_response((hy_ctrl_addr_t)*data, _read_word(data + 1));
+    break;
+
+  case HUANYANG_FREQ_WRITE: break;
+  default: break;
+  }
+}
+
+
+static void _next_command();
+
+
+static bool _shutdown() {
+  if (!hy.shutdown && !estop_triggered()) return false;
+  modbus_deinit();
+  if (hy.deinit_cb) hy.deinit_cb();
+  return true;
+}
+
+
+static void _modbus_cb(uint8_t func, uint8_t bytes, const uint8_t *data) {
+  if (!data) { // Modbus command failed
+    if (_shutdown()) return;
+    hy.state = 0;
+
+  } else if (bytes == *data + 1) {
+    _handle_response((hy_func_t)func, data + 1);
+
+    if (func == HUANYANG_CTRL_WRITE && _shutdown()) return;
+
+    // Next command
+    if (++hy.state == 9) {
+      if (hy.shutdown || hy.changed) hy.state = 0;
+      else hy.state = 5;
+      hy.changed = false;
+    }
+ }
+
+  _next_command();
+}
+
+
+static void _func_read(hy_addr_t addr) {
+  uint8_t data[2] = {1, addr};
+  modbus_func(HUANYANG_FUNC_READ, 2, data, 4, _modbus_cb);
+}
+
+
+static void _ctrl_write(hy_ctrl_state_t state) {
+  uint8_t data[2] = {1, state};
+  modbus_func(HUANYANG_CTRL_WRITE, 2, data, 2, _modbus_cb);
+}
+
+
+static void _ctrl_read(hy_ctrl_addr_t addr) {
+  uint8_t data[2] = {1, addr};
+  modbus_func(HUANYANG_CTRL_READ, 2, data, 4, _modbus_cb);
+}
+
+
+static void _freq_write(uint16_t freq) {
+  uint8_t data[3] = {2, (uint8_t)(freq >> 8), (uint8_t)freq};
+  modbus_func(HUANYANG_FREQ_WRITE, 3, data, 3, _modbus_cb);
+}
+
+
+static void _next_command() {
+  switch (hy.state) {
+  case 0: { // Update direction
+    hy_ctrl_state_t state = HUANYANG_STOP;
+    if (!hy.shutdown && !estop_triggered()) {
+      if (0 < hy.power)
+        state = (hy_ctrl_state_t)(HUANYANG_RUN | HUANYANG_FORWARD);
+      else if (hy.power < 0)
+        state = (hy_ctrl_state_t)(HUANYANG_RUN | HUANYANG_REV_FWD);
+    }
+
+    _ctrl_write(state);
+    break;
+  }
+
+  case 1: _func_read(HY_PD005_MAX_FREQUENCY); break;
+  case 2: _func_read(HY_PD011_FREQUENCY_LOWER_LIMIT); break;
+  case 3: _func_read(HY_PD144_RATED_MOTOR_RPM); break;
+
+  case 4: { // Update freqency
+    // Compute frequency in Hz
+    float freq = fabs(hy.power * hy.max_freq);
+
+    // Frequency write command
+    _freq_write(freq * 100);
+    break;
+  }
+
+  case 5: _ctrl_read(HUANYANG_ACTUAL_FREQ);    break;
+  case 6: _ctrl_read(HUANYANG_ACTUAL_CURRENT); break;
+  case 7: _ctrl_read(HUANYANG_ACTUAL_RPM);     break;
+  case 8: _ctrl_read(HUANYANG_TEMPERATURE);    break;
+  }
+}
+
+
+void huanyang_init() {
+  modbus_init();
+  memset(&hy, 0, sizeof(hy));
+  _next_command();
+}
+
+
+void huanyang_deinit(deinit_cb_t cb) {
+  hy.deinit_cb = cb;
+  hy.shutdown = true;
+}
+
+
+void huanyang_set(float power) {
+  if (hy.power != power && !hy.shutdown)
+    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+      hy.power = power;
+      hy.changed = true;
+    }
+}
+
+
+float huanyang_get() {return hy.actual_freq / hy.max_freq;}
+uint8_t huanyang_get_status() {return hy.status;}
+
+// Variable callbacks
+float get_hy_freq() {return hy.actual_freq;}
+float get_hy_current() {return hy.actual_current;}
+uint16_t get_hy_temp() {return hy.temperature;}
+float get_hy_max_freq() {return hy.max_freq;}
+float get_hy_min_freq() {return hy.min_freq;}
+uint16_t get_hy_rated_rpm() {return hy.rated_rpm;}

src/avr/src/huanyang.h

@@ -0,0 +1,226 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "spindle.h"
+
+
+void huanyang_init();
+void huanyang_deinit(deinit_cb_t cb);
+void huanyang_set(float power);
+float huanyang_get();
+uint8_t huanyang_get_status();
+
+
+/// See Huanyang VFD user manual
+typedef enum {
+  HY_PD000_PARAMETER_LOCK,
+  HY_PD001_SOURCE_OF_OPERATION_COMMANDS,
+  HY_PD002_SOURCE_OF_OPERATING_FREQUENCY,
+  HY_PD003_MAIN_FREQUENCY,
+  HY_PD004_BASE_FREQUENCY,
+  HY_PD005_MAX_FREQUENCY,
+  HY_PD006_INTERMEDIATE_FREQUENCY,
+  HY_PD007_MIN_FREQUENCY,
+  HY_PD008_MAX_VOLTAGE,
+  HY_PD009_INTERMEDIATE_VOLTAGE,
+  HY_PD010_MIN_VOLTAGE,
+  HY_PD011_FREQUENCY_LOWER_LIMIT,
+  HY_PD012_RESERVED,
+  HY_PD013_PARAMETER_RESET,
+  HY_PD014_ACCEL_TIME_1,
+  HY_PD015_DECEL_TIME_1,
+  HY_PD016_ACCEL_TIME_2,
+  HY_PD017_DECEL_TIME_2,
+  HY_PD018_ACCEL_TIME_3,
+  HY_PD019_DECEL_TIME_3,
+  HY_PD020_ACCEL_TIME_4,
+  HY_PD021_DECEL_TIME_4,
+  HY_PD022_FACTORY_RESERVED,
+  HY_PD023_REV_ROTATION_SELECT,
+  HY_PD024_STOP_KEY,
+  HY_PD025_STARTING_MODE,
+  HY_PD026_STOPPING_MODE,
+  HY_PD027_STARTING_FREQUENCY,
+  HY_PD028_STOPPING_FREQUENCY,
+  HY_PD029_DC_BRAKING_TIME_AT_START,
+  HY_PD030_DC_BRAKING_TIME_AT_STOP,
+  HY_PD031_DC_BRAKING_VOLTAGE_LEVEL,
+  HY_PD032_FREQUENCY_TRACK_TIME,
+  HY_PD033_CURRENT_LEVEL_FOR_FREQUENCY_TRACK,
+  HY_PD034_VOLTAGE_RISE_TIME_FOR_FREQUENCY_TRACK,
+  HY_PD035_FREQUENCY_STEP_LENGTH,
+  HY_PD036,
+  HY_PD037,
+  HY_PD038,
+  HY_PD039,
+  HY_PD040,
+  HY_PD041_CARRIER_FREQUENCY,
+  HY_PD042_JOGGING_FREQUENCY,
+  HY_PD043_S_CURVE_TIME,
+  HY_PD044_MULTI_INPUT_FOR,
+  HY_PD045_MULTI_INPUT_REV,
+  HY_PD046_MULTI_INPUT_RST,
+  HY_PD047_MULTI_INPUT_SPH,
+  HY_PD048_MULTI_INPUT_SPM,
+  HY_PD049_MULTI_INPUT_SPL,
+  HY_PD050_MULTI_OUTPUT_DRV,
+  HY_PD051_MULTI_OUTPUT_UPF,
+  HY_PD052_MULTI_OUTPUT_FA_FB_FC,
+  HY_PD053_MULTI_OUTPUT_KA_KB,
+  HY_PD054_MULTI_OUTPUT_AM,
+  HY_PD055_AM_ANALOG_OUTPUT_GAIN,
+  HY_PD056_SKIP_FREQUENCY_1,
+  HY_PD057_SKIP_FREQUENCY_2,
+  HY_PD058_SKIP_FREQUENCY_3,
+  HY_PD059_SKIP_FREQUENCY_RANGE,
+  HY_PD060_UNIFORM_FREQUENCY_1,
+  HY_PD061_UNIFORM_FREQUENCY_2,
+  HY_PD062_UNIFORM_FREQUENCY_RANGE,
+  HY_PD063_TIMER_1_TIME,
+  HY_PD064_TIMER_2_TIME,
+  HY_PD065_COUNTING_VALUE,
+  HY_PD066_INTERMEDIATE_COUNTER,
+  HY_PD067,
+  HY_PD068,
+  HY_PD069,
+  HY_PD070_ANALOG_INPUT,
+  HY_PD071_ANALOG_FILTERING_CONSTANT,
+  HY_PD072_HIGHER_ANALOG_FREQUENCY,
+  HY_PD073_LOWER_ANALOG_FREQUENCY,
+  HY_PD074_BIAS_DIRECTION_AT_HIGHER_FREQUENCY,
+  HY_PD075_BIAS_DIRECTION_AT_LOWER_FREQUENCY,
+  HY_PD076_ANALOG_NEGATIVE_BIAS_REVERSE,
+  HY_PD077_UP_DOWN_FUNCTION,
+  HY_PD078_UP_DOWN_SPEED,
+  HY_PD079,
+  HY_PD080_PLC_OPERATION,
+  HY_PD081_AUTO_PLC,
+  HY_PD082_PLC_RUNNING_DIRECTION,
+  HY_PD083,
+  HY_PD084_PLC_RAMP_TIME,
+  HY_PD085,
+  HY_PD086_FREQUENCY_2,
+  HY_PD087_FREQUENCY_3,
+  HY_PD088_FREQUENCY_4,
+  HY_PD089_FREQUENCY_5,
+  HY_PD090_FREQUENCY_6,
+  HY_PD091_FREQUENCY_7,
+  HY_PD092_FREQUENCY_8,
+  HY_PD093,
+  HY_PD094,
+  HY_PD095,
+  HY_PD096,
+  HY_PD097,
+  HY_PD098,
+  HY_PD099,
+  HY_PD100,
+  HY_PD101_TIMER_1,
+  HY_PD102_TIMER_2,
+  HY_PD103_TIMER_3,
+  HY_PD104_TIMER_4,
+  HY_PD105_TIMER_5,
+  HY_PD106_TIMER_6,
+  HY_PD107_TIMER_7,
+  HY_PD108_TIMER_8,
+  HY_PD109,
+  HY_PD110,
+  HY_PD111,
+  HY_PD112,
+  HY_PD113,
+  HY_PD114,
+  HY_PD115,
+  HY_PD116,
+  HY_PD117_AUTOPLC_MEMORY_FUNCTION,
+  HY_PD118_OVER_VOLTAGE_STALL_PREVENTION,
+  HY_PD119_STALL_PREVENTION_LEVEL_AT_RAMP_UP,
+  HY_PD120_STALL_PREVENTION_LEVEL_AT_CONSTANT_SPEED,
+  HY_PD121_DECEL_TIME_FOR_STALL_PREVENTION_AT_CONSTANT_SPEED,
+  HY_PD122_STALL_PREVENTION_LEVEL_AT_DECELERATION,
+  HY_PD123_OVER_TORQUE_DETECT_MODE,
+  HY_PD124_OVER_TORQUE_DETECT_LEVEL,
+  HY_PD125_OVER_TORQUE_DETECT_TIME,
+  HY_PD126,
+  HY_PD127,
+  HY_PD128,
+  HY_PD129,
+  HY_PD130_NUMBER_OF_AUXILIARY_PUMP,
+  HY_PD131_CONTINUOUS_RUNNING_TIME_OF_AUXILIARY_PUMPS,
+  HY_PD132_INTERLOCKING_TIME_OF_AUXILIARY_PUMP,
+  HY_PD133_HIGH_SPEED_RUNNING_TIME,
+  HY_PD134_LOW_SPEED_RUNNING_TIME,
+  HY_PD135_STOPPING_VOLTAGE_LEVEL,
+  HY_PD136_LASTING_TIME_OF_STOPPING_VOLTAGE_LEVEL,
+  HY_PD137_WAKEUP_VOLTAGE_LEVEL,
+  HY_PD138_SLEEP_FREQUENCY,
+  HY_PD139_LASTING_TIME_OF_SLEEP_FREQUENCY,
+  HY_PD140,
+  HY_PD141_RATED_MOTOR_VOLTAGE,
+  HY_PD142_RATED_MOTOR_CURRENT,
+  HY_PD143_MOTOR_POLE_NUMBER,
+  HY_PD144_RATED_MOTOR_RPM,
+  HY_PD145_AUTO_TORQUE_COMPENSATION,
+  HY_PD146_MOTOR_NO_LOAD_CURRENT,
+  HY_PD147_MOTOR_SLIP_COMPENSATION,
+  HY_PD148,
+  HY_PD149,
+  HY_PD150_AUTO_VOLTAGE_REGULATION,
+  HY_PD151_AUTO_ENERGY_SAVING,
+  HY_PD152_FAULT_RESTART_TIME,
+  HY_PD153_RESTART_AFTER_INSTANTANEOUS_STOP,
+  HY_PD154_ALLOWABLE_POWER_BREAKDOWN_TIME,
+  HY_PD155_NUMBER_OF_ABNORMAL_RESTART,
+  HY_PD156_PROPORTIONAL_CONSTANT,
+  HY_PD157_INTEGRAL_TIME,
+  HY_PD158_DIFFERENTIAL_TIME,
+  HY_PD159_TARGET_VALUE,
+  HY_PD160_PID_TARGET_VALUE,
+  HY_PD161_PID_UPPER_LIMIT,
+  HY_PD162_PID_LOWER_LIMIT,
+  HY_PD163_COMMUNICATION_ADDRESSES,
+  HY_PD164_COMMUNICATION_BAUD_RATE,
+  HY_PD165_COMMUNICATION_DATA_METHOD,
+  HY_PD166,
+  HY_PD167,
+  HY_PD168,
+  HY_PD169,
+  HY_PD170_DISPLAY_ITEMS,
+  HY_PD171_DISPLAY_ITEMS_OPEN,
+  HY_PD172_FAULT_CLEAR,
+  HY_PD173,
+  HY_PD174_RATED_CURRENT_OF_INVERTER,
+  HY_PD175_INVERTER_MODEL,
+  HY_PD176_INVERTER_FREQUENCY_STANDARD,
+  HY_PD177_FAULT_RECORD_1,
+  HY_PD178_FAULT_RECORD_2,
+  HY_PD179_FAULT_RECORD_3,
+  HY_PD180_FAULT_RECORD_4,
+  HY_PD181_SOFTWARE_VERSION,
+  HY_PD182_MANUFACTURE_DATE,
+  HY_PD183_SERIAL_NO,
+} hy_addr_t;

src/avr/src/i2c.c

@@ -0,0 +1,129 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "i2c.h"
+
+#include <avr/interrupt.h>
+
+#include <stdbool.h>
+
+
+typedef struct {
+  i2c_read_cb_t read_cb;
+  i2c_write_cb_t write_cb;
+  uint8_t data[I2C_MAX_DATA + 1];
+  uint8_t length;
+  bool done;
+  bool write;
+} i2c_t;
+
+static i2c_t i2c = {0};
+
+
+static void _i2c_reset_command() {
+  i2c.length = 0;
+  i2c.done = true;
+  i2c.write = false;
+}
+
+
+static void _i2c_end_command() {
+  if (i2c.length && !i2c.write && i2c.read_cb) {
+    i2c.data[i2c.length] = 0; // Null terminate
+    i2c.read_cb(i2c.data, i2c.length);
+  }
+
+  _i2c_reset_command();
+}
+
+
+static void _i2c_command_byte(uint8_t byte) {
+  i2c.data[i2c.length++] = byte;
+}
+
+
+ISR(I2C_ISR) {
+  uint8_t status = I2C_DEV.SLAVE.STATUS;
+
+  // Error or collision
+  if (status & (TWI_SLAVE_BUSERR_bm | TWI_SLAVE_COLL_bm)) {
+    _i2c_reset_command();
+    return; // Ignore
+
+  } else if ((status & TWI_SLAVE_APIF_bm) && (status & TWI_SLAVE_AP_bm)) {
+    // START + address match
+    I2C_DEV.SLAVE.CTRLB = TWI_SLAVE_CMD_RESPONSE_gc; // ACK address byte
+    _i2c_end_command(); // Handle repeated START
+
+  } else if (status & TWI_SLAVE_APIF_bm) {
+    // STOP
+    I2C_DEV.SLAVE.STATUS = TWI_SLAVE_APIF_bm; // Clear interrupt flag
+    _i2c_end_command();
+
+  } else if (status & TWI_SLAVE_DIF_bm) {
+    i2c.write = status & TWI_SLAVE_DIR_bm;
+
+    // DATA
+    if (i2c.write) { // Write
+      // Check if master ACKed last byte sent
+      if (i2c.length && (status & TWI_SLAVE_RXACK_bm || i2c.done))
+        I2C_DEV.SLAVE.CTRLB = TWI_SLAVE_CMD_COMPTRANS_gc; // End transaction
+
+      else {
+        // Send some data
+        i2c.done = false;
+        I2C_DEV.SLAVE.DATA = i2c.write_cb(i2c.length++, &i2c.done);
+        I2C_DEV.SLAVE.CTRLB = TWI_SLAVE_CMD_RESPONSE_gc; // Continue transaction
+      }
+
+    } else { // Read
+      _i2c_command_byte(I2C_DEV.SLAVE.DATA);
+
+      // ACK and continue transaction
+      I2C_DEV.SLAVE.CTRLB = TWI_SLAVE_CMD_RESPONSE_gc;
+    }
+  }
+}
+
+
+static uint8_t _i2c_default_write_cb(uint8_t offset, bool *done) {
+  *done = true;
+  return 0;
+}
+
+
+void i2c_init() {
+  i2c_set_write_callback(_i2c_default_write_cb);
+
+  I2C_DEV.SLAVE.CTRLA = TWI_SLAVE_INTLVL_LO_gc | TWI_SLAVE_DIEN_bm |
+    TWI_SLAVE_ENABLE_bm | TWI_SLAVE_APIEN_bm | TWI_SLAVE_PIEN_bm;
+  I2C_DEV.SLAVE.ADDR = I2C_ADDR << 1;
+}
+
+
+void i2c_set_read_callback(i2c_read_cb_t cb) {i2c.read_cb = cb;}
+void i2c_set_write_callback(i2c_write_cb_t cb) {i2c.write_cb = cb;}

src/avr/src/i2c.h

@@ -0,0 +1,41 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "config.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+
+typedef void (*i2c_read_cb_t)(uint8_t *data, uint8_t length);
+typedef uint8_t (*i2c_write_cb_t)(uint8_t offset, bool *done);
+
+
+void i2c_init();
+void i2c_set_read_callback(i2c_read_cb_t cb);
+void i2c_set_write_callback(i2c_write_cb_t cb);

src/avr/src/io.c

@@ -0,0 +1,118 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "io.h"
+
+#include "status.h"
+#include "util.h"
+#include "command.h"
+#include "exec.h"
+#include "rtc.h"
+#include "analog.h"
+
+#include <ctype.h>
+#include <stdbool.h>
+#include <stdio.h>
+
+
+typedef struct {
+  int8_t port;
+  bool digital;
+  input_mode_t mode;
+  float timeout;
+} input_cmd_t;
+
+
+static input_cmd_t active_cmd = {-1,};
+static uint32_t timeout;
+
+
+void io_callback() {
+  if (active_cmd.port == -1) return;
+
+  bool done = false;
+  float result = 0;
+  if (active_cmd.mode == INPUT_IMMEDIATE || rtc_expired(timeout)) done = true;
+
+  // TODO handle modes
+
+  if (done) {
+    if (active_cmd.digital) { // TODO
+    } else result = analog_get(active_cmd.port);
+
+    printf_P(PSTR("{\"result\": %f}\n"), (double)result);
+    active_cmd.port = -1;
+  }
+}
+
+
+static stat_t _exec_cb() {
+  if (active_cmd.port == -1) exec_set_cb(0);
+  return STAT_NOP;
+}
+
+
+// Command callbacks
+stat_t command_input(char *cmd) {
+  input_cmd_t input_cmd;
+  cmd++; // Skip command
+
+  // Analog or digital
+  if (*cmd == 'd') input_cmd.digital = true;
+  else if (*cmd == 'a') input_cmd.digital = false;
+  else return STAT_INVALID_ARGUMENTS;
+  cmd++;
+
+  // Port index
+  if (!isdigit(*cmd)) return STAT_INVALID_ARGUMENTS;
+  input_cmd.port = *cmd - '0';
+  cmd++;
+
+  // Mode
+  if (!isdigit(*cmd)) return STAT_INVALID_ARGUMENTS;
+  input_cmd.mode = (input_mode_t)(*cmd - '0');
+  if (INPUT_LOW < input_cmd.mode) return STAT_INVALID_ARGUMENTS;
+  cmd++;
+
+  // Timeout
+  if (!decode_float(&cmd, &input_cmd.timeout)) return STAT_BAD_FLOAT;
+
+  command_push(COMMAND_input, &input_cmd);
+
+  return STAT_OK;
+}
+
+
+unsigned command_input_size() {return sizeof(input_cmd_t);}
+
+
+void command_input_exec(void *data) {
+  active_cmd = *(input_cmd_t *)data;
+
+  timeout = rtc_get_time() + active_cmd.timeout * 1000;
+  exec_set_cb(_exec_cb);
+}

src/avr/src/io.h

@@ -0,0 +1,40 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+typedef enum {
+  INPUT_IMMEDIATE,
+  INPUT_RISE,
+  INPUT_FALL,
+  INPUT_HIGH,
+  INPUT_LOW,
+} input_mode_t;
+
+
+void io_callback();

src/avr/src/jog.c

@@ -0,0 +1,169 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "jog.h"
+
+#include "axis.h"
+#include "util.h"
+#include "exec.h"
+#include "state.h"
+#include "command.h"
+#include "config.h"
+#include "SCurve.h"
+
+#include <stdbool.h>
+#include <math.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+
+typedef struct {
+  bool holding;
+  bool writing;
+
+  SCurve scurves[AXES];
+  float next[AXES];
+  float targetV[AXES];
+} jr_t;
+
+jr_t jr = {0};
+
+
+stat_t jog_exec() {
+  bool done = true;
+
+  // Compute per axis velocities and target positions
+  float target[AXES] = {0,};
+  float velocity_sqr = 0;
+
+  for (int axis = 0; axis < AXES; axis++) {
+    if (!axis_is_enabled(axis)) continue;
+
+    // Load next velocity
+    if (!jr.writing)
+      jr.targetV[axis] = jr.next[axis] * axis_get_velocity_max(axis);
+
+    float p = exec_get_axis_position(axis);
+    float vel = jr.scurves[axis].getVelocity();
+    float targetV = jr.targetV[axis];
+    float min = axis_get_soft_limit(axis, true);
+    float max = axis_get_soft_limit(axis, false);
+    bool softLimited = min != max && axis_get_homed(axis);
+
+    // Apply soft limits, if enabled and homed
+    if (softLimited && MIN_VELOCITY < fabs(targetV)) {
+      float dist = jr.scurves[axis].getStoppingDist() *
+        (1 + (JOG_STOPPING_UNDERSHOOT / 100.0));
+
+      if (vel < 0 && p - dist <= min) targetV = -MIN_VELOCITY;
+      if (0 < vel && max <= p + dist) targetV = MIN_VELOCITY;
+    }
+
+    // Compute next velocity
+    float v = jr.scurves[axis].next(SEGMENT_TIME, targetV);
+
+    // Don't overshoot soft limits
+    float deltaP = v * SEGMENT_TIME;
+    if (softLimited && 0 < deltaP && max < p + deltaP) p = max;
+    else if (softLimited && deltaP < 0 && p + deltaP < min) p = min;
+    else p += deltaP;
+
+    // Not done jogging if still moving
+    if (MIN_VELOCITY < fabs(v) || MIN_VELOCITY < fabs(targetV)) done = false;
+
+    velocity_sqr += square(v);
+    target[axis] = p;
+  }
+
+  // Check if we are done
+  if (done) {
+    command_reset_position();
+    exec_set_velocity(0);
+    exec_set_cb(0);
+    if (jr.holding) state_holding();
+    else state_idle();
+
+    return STAT_NOP; // Done, no move executed
+  }
+
+  // Set velocity and target
+  exec_set_velocity(sqrt(velocity_sqr));
+  exec_move_to_target(target);
+
+  return STAT_OK;
+}
+
+
+void jog_stop() {
+  if (state_get() != STATE_JOGGING) return;
+  jr.writing = true;
+  for (int axis = 0; axis < AXES; axis++) jr.next[axis] = 0;
+  jr.writing = false;
+}
+
+
+stat_t command_jog(char *cmd) {
+  // Ignore jog commands when not READY, HOLDING or JOGGING
+  if (state_get() != STATE_READY && state_get() != STATE_HOLDING &&
+      state_get() != STATE_JOGGING)
+    return STAT_NOP;
+
+  // Skip over command code
+  cmd++;
+
+  // Get velocities
+  float velocity[AXES] = {0,};
+  stat_t status = decode_axes(&cmd, velocity);
+  if (status) return status;
+
+  // Check for end of command
+  if (*cmd) return STAT_INVALID_ARGUMENTS;
+
+  // Start jogging
+  if (state_get() != STATE_JOGGING) {
+    memset(&jr, 0, sizeof(jr));
+
+    jr.holding = state_get() == STATE_HOLDING;
+
+    for (int axis = 0; axis < AXES; axis++)
+      if (axis_is_enabled(axis))
+        jr.scurves[axis] =
+          SCurve(axis_get_velocity_max(axis), axis_get_accel_max(axis),
+                 axis_get_jerk_max(axis));
+
+    state_jogging();
+    exec_set_cb(jog_exec);
+  }
+
+  // Set next velocities
+  jr.writing = true;
+  for (int axis = 0; axis < AXES; axis++) jr.next[axis] = velocity[axis];
+  jr.writing = false;
+
+  return STAT_OK;
+}

src/avr/src/jog.h

@@ -0,0 +1,34 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "status.h"
+
+
+stat_t jog_exec();
+void jog_stop();

src/avr/src/lcd.c

@@ -0,0 +1,143 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "lcd.h"
+#include "rtc.h"
+#include "hardware.h"
+#include "command.h"
+
+#include <avr/io.h>
+#include <avr/wdt.h>
+#include <util/delay.h>
+
+#include <stdbool.h>
+
+
+void lcd_init(uint8_t addr) {
+  // Enable I2C master
+  TWIC.MASTER.BAUD = F_CPU / 2 / 100000 - 5; // 100 KHz
+  TWIC.MASTER.CTRLA = TWI_MASTER_ENABLE_bm;
+  TWIC.MASTER.CTRLB = TWI_MASTER_TIMEOUT_DISABLED_gc;
+  TWIC.MASTER.STATUS = TWI_MASTER_BUSSTATE_IDLE_gc;
+
+  _delay_ms(50);
+  lcd_nibble(addr, 3 << 4); // Home
+  _delay_ms(50);
+  lcd_nibble(addr, 3 << 4); // Home
+  _delay_ms(50);
+  lcd_nibble(addr, 3 << 4); // Home
+  lcd_nibble(addr, 2 << 4); // 4-bit
+
+  lcd_write(addr,
+            LCD_FUNCTION_SET | LCD_2_LINE | LCD_5x8_DOTS | LCD_4_BIT_MODE, 0);
+  lcd_write(addr, LCD_DISPLAY_CONTROL | LCD_DISPLAY_ON, 0);
+  lcd_write(addr, LCD_ENTRY_MODE_SET | LCD_ENTRY_SHIFT_INC, 0);
+
+  lcd_write(addr, LCD_CLEAR_DISPLAY, 0);
+  lcd_write(addr, LCD_RETURN_HOME, 0);
+}
+
+
+static void _master_wait() {
+#ifdef __AVR__
+  while (!(TWIC.MASTER.STATUS & TWI_MASTER_WIF_bm)) continue;
+#endif
+}
+
+
+static void _write_i2c(uint8_t addr, uint8_t data) {
+  data |= BACKLIGHT_BIT;
+
+  TWIC.MASTER.ADDR = addr << 1;
+  _master_wait();
+
+  TWIC.MASTER.DATA = data;
+  _master_wait();
+
+  TWIC.MASTER.CTRLC = TWI_MASTER_CMD_STOP_gc;
+
+  _delay_us(100);
+}
+
+
+void lcd_nibble(uint8_t addr, uint8_t data) {
+  _write_i2c(addr, data);
+  _write_i2c(addr, data | ENABLE_BIT);
+  _delay_us(500);
+  _write_i2c(addr, data & ~ENABLE_BIT);
+  _delay_us(100);
+}
+
+
+void lcd_write(uint8_t addr, uint8_t cmd, uint8_t flags) {
+  lcd_nibble(addr, flags | (cmd & 0xf0));
+  lcd_nibble(addr, flags | ((cmd << 4) & 0xf0));
+}
+
+
+void lcd_goto(uint8_t addr, uint8_t x, uint8_t y) {
+  static uint8_t row[] = {0, 64, 20, 84};
+  lcd_write(addr, LCD_SET_DDRAM_ADDR | (row[y] + x), 0);
+}
+
+
+void lcd_putchar(uint8_t addr, uint8_t c) {
+  lcd_write(addr, c, REG_SELECT_BIT);
+}
+
+
+void lcd_pgmstr(uint8_t addr, const char *s) {
+  while (true) {
+    char c = pgm_read_byte(s++);
+    if (!c) break;
+    lcd_putchar(addr, c);
+  }
+}
+
+
+void _splash(uint8_t addr) {
+  lcd_init(addr);
+  lcd_goto(addr, 1, 1);
+  lcd_pgmstr(addr, PSTR("Controller booting"));
+  lcd_goto(addr, 3, 2);
+  lcd_pgmstr(addr, PSTR("Please wait..."));
+}
+
+
+void lcd_splash() {
+  wdt_disable();
+  _splash(0x27);
+  _splash(0x3f);
+  wdt_enable(WDTO_250MS);
+}
+
+
+void lcd_rtc_callback() {
+  // Display the splash if we haven't gotten any commands in 1sec since boot
+  if (!command_is_active() && rtc_get_time() == 1000)
+    lcd_splash();
+}

src/avr/src/lcd.h

@@ -0,0 +1,103 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "pgmspace.h"
+
+#include <stdint.h>
+
+
+// Control flags
+enum {
+  REG_SELECT_BIT          = 1 << 0,
+  READ_BIT                = 1 << 1,
+  ENABLE_BIT              = 1 << 2,
+  BACKLIGHT_BIT           = 1 << 3,
+};
+
+
+// Commands
+enum {
+  LCD_CLEAR_DISPLAY       = 1 << 0,
+  LCD_RETURN_HOME         = 1 << 1,
+  LCD_ENTRY_MODE_SET      = 1 << 2,
+  LCD_DISPLAY_CONTROL     = 1 << 3,
+  LCD_CURSOR_SHIFT        = 1 << 4,
+  LCD_FUNCTION_SET        = 1 << 5,
+  LCD_SET_CGRAM_ADDR      = 1 << 6,
+  LCD_SET_DDRAM_ADDR      = 1 << 7,
+};
+
+
+// Entry Mode Set flags
+#define LCD_ENTRY_SHIFT_DISPLAY (1 << 0)
+#define LCD_ENTRY_SHIFT_INC     (1 << 1)
+#define LCD_ENTRY_SHIFT_DEC     (0 << 1)
+
+
+// Display Control flags
+#define LCD_BLINK_ON            (1 << 0)
+#define LCD_BLINK_OFF           (0 << 0)
+#define LCD_CURSOR_ON           (1 << 1)
+#define LCD_CURSOR_OFF          (0 << 1)
+#define LCD_DISPLAY_ON          (1 << 2)
+#define LCD_DISPLAY_OFF         (0 << 2)
+
+
+// Cursor Shift flags
+#define LCD_SHIFT_RIGHT         (1 << 2)
+#define LCD_SHIFT_LEFT          (0 << 2)
+#define LCD_SHIFT_DISPLAY       (1 << 3)
+#define LCD_SHIFT_CURSOR        (0 << 3)
+
+
+// Function Set flags
+#define LCD_5x11_DOTS           (1 << 2)
+#define LCD_5x8_DOTS            (0 << 2)
+#define LCD_2_LINE              (1 << 3)
+#define LCD_1_LINE              (0 << 3)
+#define LCD_8_BIT_MODE          (1 << 4)
+#define LCD_4_BIT_MODE          (0 << 4)
+
+
+// Text justification flags
+enum {
+  JUSTIFY_LEFT            = 0,
+  JUSTIFY_RIGHT           = 1,
+  JUSTIFY_CENTER          = 2,
+};
+
+
+void lcd_init(uint8_t addr);
+void lcd_nibble(uint8_t addr, uint8_t data);
+void lcd_write(uint8_t addr, uint8_t cmd, uint8_t flags);
+void lcd_goto(uint8_t addr, uint8_t x, uint8_t y);
+void lcd_putchar(uint8_t addr, uint8_t c);
+void lcd_pgmstr(uint8_t addr, const char *s);
+void lcd_splash();
+void lcd_rtc_callback();

src/avr/src/line.c

@@ -0,0 +1,274 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "config.h"
+#include "exec.h"
+#include "command.h"
+#include "spindle.h"
+#include "util.h"
+#include "SCurve.h"
+
+#include <math.h>
+#include <float.h>
+#include <string.h>
+
+
+typedef struct {
+  float start[AXES];
+  float target[AXES];
+  float times[7];
+  float target_vel;
+  float max_accel;
+  float max_jerk;
+
+  float unit[AXES];
+  float length;
+} line_t;
+
+
+static struct {
+  line_t line;
+
+  int section;
+  int seg;
+
+  float iD; // Initial section distance
+  float iV; // Initial section velocity
+  float iA; // Initial section acceleration
+  float jerk;
+  float lV; // Last velocity
+  float lD; // Last distance
+
+  power_update_t power_updates[POWER_MAX_UPDATES];
+} l;
+
+
+static void _segment_target(float target[AXES], float d) {
+  for (int axis = 0; axis < AXES; axis++)
+    target[axis] = l.line.start[axis] + l.line.unit[axis] * d;
+}
+
+
+static float _segment_distance(float t) {
+  return l.iD + SCurve::distance(t, l.iV, l.iA, l.jerk);
+}
+
+
+static float _segment_velocity(float t) {
+  return l.iV + SCurve::velocity(t, l.iA, l.jerk);
+}
+
+
+static float _segment_accel(float t) {
+  return l.iA + SCurve::acceleration(t, l.jerk);
+}
+
+
+static bool _section_next() {
+  while (++l.section < 7) {
+    if (!l.line.times[l.section]) continue;
+
+    // Jerk
+    switch (l.section) {
+    case 0: case 6: l.jerk = l.line.max_jerk; break;
+    case 2: case 4: l.jerk = -l.line.max_jerk; break;
+    default: l.jerk = 0;
+    }
+    exec_set_jerk(l.jerk);
+
+    // Acceleration
+    switch (l.section) {
+    case 1: case 2: l.iA = l.line.max_jerk * l.line.times[0]; break;
+    case 5: case 6: l.iA = -l.line.max_jerk * l.line.times[4]; break;
+    default: l.iA = 0;
+    }
+
+    return true;
+  }
+
+  return false;
+}
+
+
+static stat_t _exec_segment(float time, const float target[], float vel,
+                            float accel) {
+  return exec_segment(time, target, vel, accel, l.line.max_accel,
+                      l.line.max_jerk, l.power_updates);
+}
+
+
+static stat_t _line_exec() {
+  // Compute times
+  float section_time = l.line.times[l.section];
+  float seg_time = SEGMENT_TIME;
+  float t = ++l.seg * SEGMENT_TIME;
+
+  // Don't exceed section time
+  if (section_time < t) {
+    seg_time = section_time - (l.seg - 1) * SEGMENT_TIME;
+    t = section_time;
+  }
+
+  // Compute distance and velocity
+  float d = _segment_distance(t);
+  float v = _segment_velocity(t);
+  float a = _segment_accel(t);
+
+  // Don't allow overshoot
+  if (l.line.length < d) d = l.line.length;
+
+  // Handle synchronous speeds
+  spindle_load_power_updates(l.power_updates, l.lD, d);
+  l.lD = d;
+
+  // Check if section complete
+  if (t == section_time) {
+    if (_section_next()) {
+      // Setup next section
+      l.seg = 0;
+      l.iD = d;
+      l.iV = v;
+
+    } else {
+      exec_set_cb(0);
+
+      // Last segment of last section
+      // Use exact target values to correct for floating-point errors
+      return _exec_segment(seg_time, l.line.target, l.line.target_vel, a);
+    }
+  }
+
+  // Compute target position from distance
+  float target[AXES];
+  _segment_target(target, d);
+
+  // Segment move
+  return _exec_segment(seg_time, target, v, a);
+}
+
+
+stat_t command_line(char *cmd) {
+  line_t line = {};
+
+  cmd++; // Skip command code
+
+  // Get start position
+  command_get_position(line.start);
+
+  // Get target velocity
+  if (!decode_float(&cmd, &line.target_vel)) return STAT_BAD_FLOAT;
+  if (line.target_vel < 0) return STAT_INVALID_ARGUMENTS;
+
+  // Get max accel
+  if (!decode_float(&cmd, &line.max_accel)) return STAT_BAD_FLOAT;
+  if (line.max_accel < 0) return STAT_INVALID_ARGUMENTS;
+
+  // Get max jerk
+  if (!decode_float(&cmd, &line.max_jerk)) return STAT_BAD_FLOAT;
+  if (line.max_jerk < 0) return STAT_INVALID_ARGUMENTS;
+
+  // Get target position
+  copy_vector(line.target, line.start);
+  stat_t status = decode_axes(&cmd, line.target);
+  if (status) return status;
+
+  // Get times
+  bool has_time = false;
+  while (*cmd) {
+    if (*cmd < '0' || '6' < *cmd) break;
+    int section = *cmd - '0';
+    cmd++;
+
+    float time;
+    if (!decode_float(&cmd, &time)) return STAT_BAD_FLOAT;
+
+    if (time < 0) return STAT_NEGATIVE_SCURVE_TIME;
+    line.times[section] = time;
+    if (time) has_time = true;
+   }
+
+  if (!has_time) return STAT_ALL_ZERO_SCURVE_TIMES;
+
+  // Check for end of command
+  if (*cmd) return STAT_INVALID_ARGUMENTS;
+
+  // Set next start position
+  command_set_position(line.target);
+
+  // Compute direction vector
+  for (int axis = 0; axis < AXES; axis++) {
+    line.unit[axis] = line.target[axis] - line.start[axis];
+    line.length += line.unit[axis] * line.unit[axis];
+  }
+
+  line.length = sqrt(line.length);
+  for (int axis = 0; axis < AXES; axis++)
+    if (line.unit[axis]) line.unit[axis] /= line.length;
+
+  // Queue
+  command_push(COMMAND_line, &line);
+
+  return STAT_OK;
+}
+
+
+unsigned command_line_size() {return sizeof(line_t);}
+
+
+void command_line_exec(void *data) {
+  l.line = *(line_t *)data;
+
+  // Setup first section
+  l.seg = 0;
+  l.iD = 0;
+  l.lD = 0;
+  // If current velocity is non-zero use last target velocity
+  l.iV = exec_get_velocity() ? l.lV : 0;
+  l.lV = l.line.target_vel;
+
+  // Find first section
+  l.section = -1;
+  if (!_section_next()) return;
+
+#if 0
+  // Compare start position to actual position
+  float diff[AXES];
+  bool report = false;
+  exec_get_position(diff);
+  for (int i = 0; i < AXES; i++) {
+    diff[i] -= l.line.start[i];
+    if (0.1 < fabs(diff[i])) report = true;
+  }
+
+  if (report)
+    STATUS_DEBUG("diff: %.4f %.4f %.4f %.4f",
+                 diff[0], diff[1], diff[2], diff[3]);
+#endif
+
+  // Set callback
+  exec_set_cb(_line_exec);
+}

src/avr/src/main.c

@@ -0,0 +1,101 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "hardware.h"
+#include "stepper.h"
+#include "motor.h"
+#include "switch.h"
+#include "usart.h"
+#include "drv8711.h"
+#include "vars.h"
+#include "rtc.h"
+#include "report.h"
+#include "command.h"
+#include "estop.h"
+#include "i2c.h"
+#include "pgmspace.h"
+#include "outputs.h"
+#include "analog.h"
+#include "modbus.h"
+#include "io.h"
+#include "exec.h"
+#include "state.h"
+#include "emu.h"
+
+#include <avr/wdt.h>
+
+#include <stdio.h>
+#include <stdbool.h>
+
+
+// For emu
+int __argc;
+char **__argv;
+
+
+int main(int argc, char *argv[]) {
+  __argc = argc;
+  __argv = argv;
+
+  wdt_enable(WDTO_250MS);
+
+  // Init
+  cli();                          // disable interrupts
+
+  emu_init();                     // Init emulator
+  hw_init();                      // hardware setup - must be first
+  outputs_init();                 // output pins
+  switch_init();                  // switches
+  estop_init();                   // emergency stop handler
+  analog_init();                  // analog input pins
+  usart_init();                   // serial port
+  i2c_init();                     // i2c port
+  drv8711_init();                 // motor drivers
+  stepper_init();                 // steppers
+  motor_init();                   // motors
+  exec_init();                    // motion exec
+  vars_init();                    // configuration variables
+  command_init();                 // command queue
+
+  sei();                          // enable interrupts
+
+  // Splash
+  printf_P(PSTR("\n{\"firmware\":\"Buildbotics AVR\"}\n"));
+
+  // Main loop
+  while (true) {
+    emu_callback();               // Emulator callback
+    hw_reset_handler();           // handle hard reset requests
+    state_callback();             // manage state
+    command_callback();           // process next command
+    modbus_callback();            // handle modbus events
+    io_callback();                // handle io input
+    report_callback();            // report changes
+  }
+
+  return 0;
+}

src/avr/src/messages.def

@@ -0,0 +1,62 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+STAT_MSG(OK,                    "OK")
+STAT_MSG(AGAIN,                 "Run command again")
+STAT_MSG(NOP,                   "No op")
+STAT_MSG(INTERNAL_ERROR,        "Internal error")
+STAT_MSG(ESTOP_USER,            "User triggered EStop")
+STAT_MSG(ESTOP_SWITCH,          "Switch triggered EStop")
+STAT_MSG(POWER_SHUTDOWN,        "Power shutdown")
+STAT_MSG(UNRECOGNIZED_NAME,     "Unrecognized command or variable name")
+STAT_MSG(INVALID_COMMAND,       "Invalid command")
+STAT_MSG(INVALID_ARGUMENTS,     "Invalid arguments")
+STAT_MSG(TOO_MANY_ARGUMENTS,    "Too many arguments")
+STAT_MSG(TOO_FEW_ARGUMENTS,     "Too few arguments")
+STAT_MSG(MACHINE_ALARMED,       "Machine alarmed - Command not processed")
+STAT_MSG(EXPECTED_MOVE,         "A move expected but none queued")
+STAT_MSG(BAD_FLOAT,             "Failed to parse float")
+STAT_MSG(BAD_INT,               "Failed to parse integer")
+STAT_MSG(INVALID_VALUE,         "Invalid value")
+STAT_MSG(INVALID_TYPE,          "Invalid type")
+STAT_MSG(READ_ONLY,             "Variable is read only")
+STAT_MSG(ALL_ZERO_SCURVE_TIMES, "All zero s-curve times")
+STAT_MSG(NEGATIVE_SCURVE_TIME,  "Negative s-curve time")
+STAT_MSG(SEEK_NOT_ENABLED,      "Switch not enabled")
+STAT_MSG(SEEK_NOT_FOUND,        "Switch not found")
+STAT_MSG(MOTOR_ID_INVALID,      "Invalid motor ID")
+STAT_MSG(MOTOR_NOT_PREPPED,     "Motor move not prepped")
+STAT_MSG(MOTOR_NOT_READY,       "Motor not ready for move")
+STAT_MSG(MOTOR_FAULT,           "Motor fault")
+STAT_MSG(STEPPER_NULL_MOVE,     "Null move in stepper driver")
+STAT_MSG(STEPPER_NOT_READY,     "Stepper driver not ready for move")
+STAT_MSG(LONG_SEG_TIME,         "Long segment time")
+STAT_MSG(MODBUS_BUF_LENGTH,     "Modbus invalid buffer length")
+STAT_MSG(INVALID_QCMD,          "Invalid command in queue")
+STAT_MSG(Q_OVERRUN,             "Command queue overrun")
+STAT_MSG(Q_UNDERRUN,            "Command queue underrun")
+STAT_MSG(Q_INVALID_PUSH,        "Invalid command pushed to queue")

src/avr/src/messages.json.in

@@ -0,0 +1,7 @@
+#include "cpp_magic.h"
+[
+#define STAT_MSG(NAME, MSG) [#NAME, MSG],
+#include "messages.def"
+#undef CMD
+  []
+]

src/avr/src/modbus.c

@@ -0,0 +1,486 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "modbus.h"
+#include "usart.h"
+#include "status.h"
+#include "rtc.h"
+#include "util.h"
+#include "estop.h"
+#include "config.h"
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <util/crc16.h>
+
+#include <string.h>
+#include <stdio.h>
+
+
+typedef enum {
+  MODBUS_DISCONNECTED,
+  MODBUS_OK,
+  MODBUS_CRC,
+  MODBUS_INVALID,
+  MODBUS_TIMEDOUT,
+} modbus_status_t;
+
+
+static struct {
+  bool debug;
+  uint8_t id;
+  baud_t baud;
+  parity_t parity;
+} cfg = {false, 1, USART_BAUD_9600, USART_NONE};
+
+
+static struct {
+  uint8_t bytes;
+  uint8_t command[MODBUS_BUF_SIZE];
+  uint8_t command_length;
+  uint8_t response[MODBUS_BUF_SIZE];
+  uint8_t response_length;
+
+  uint16_t addr;
+  modbus_rw_cb_t rw_cb;
+  modbus_cb_t receive_cb;
+
+  uint32_t last_write;
+  uint32_t last_read;
+  uint8_t retry;
+  uint8_t status;
+  uint16_t crc_errs;
+  bool write_ready;
+  bool response_ready;
+  bool transmit_complete;
+  bool busy;
+} state = {0};
+
+
+static uint16_t _crc16(const uint8_t *buffer, unsigned length) {
+  uint16_t crc = 0xffff;
+
+  for (unsigned i = 0; i < length; i++)
+    crc = _crc16_update(crc, buffer[i]);
+
+  return crc;
+}
+
+
+static void _set_write(bool x) {SET_PIN(RS485_RW_PIN, x);}
+
+
+#define INTLVL_SET(REG, NAME, LEVEL)                        \
+  REG = ((REG) & ~NAME##INTLVL_gm) | NAME##INTLVL_##LEVEL##_gc
+
+
+#define INTLVL_ENABLE(REG, NAME, LEVEL, ENABLE)  do {   \
+    if (ENABLE) INTLVL_SET(REG, NAME, LEVEL);           \
+    else INTLVL_SET(REG, NAME, OFF);                    \
+  } while (0)
+
+
+static void _set_dre_interrupt(bool enable) {
+  INTLVL_ENABLE(RS485_PORT.CTRLA, USART_DRE, MED, enable);
+}
+
+
+static void _set_txc_interrupt(bool enable) {
+  INTLVL_ENABLE(RS485_PORT.CTRLA, USART_TXC, MED, enable);
+}
+
+
+static void _set_rxc_interrupt(bool enable) {
+  INTLVL_ENABLE(RS485_PORT.CTRLA, USART_RXC, MED, enable);
+}
+
+
+static void _write_word(uint8_t *dst, uint16_t value, bool little_endian) {
+  dst[!little_endian] = value;
+  dst[little_endian]  = value >> 8;
+}
+
+
+static uint16_t _read_word(const uint8_t *data, bool little_endian) {
+  return (uint16_t)data[little_endian] << 8 | data[!little_endian];
+}
+
+
+static bool _check_response() {
+  // Check CRC
+  uint16_t computed = _crc16(state.response, state.response_length - 2);
+  uint16_t expected =
+    _read_word(state.response + state.response_length - 2, true);
+
+  if (computed != expected) {
+    if (cfg.debug) {
+      char sent[state.command_length * 2 + 1];
+      char response[state.response_length * 2 + 1];
+      format_hex_buf(sent, state.command, state.command_length);
+      format_hex_buf(response, state.response, state.response_length);
+
+      STATUS_WARNING(STAT_OK, "modbus: invalid CRC, received=0x%04x "
+                     "computed=0x%04x sent=0x%s received=0x%s",
+                     expected, computed, sent, response);
+    }
+
+    state.crc_errs++;
+    state.status = MODBUS_CRC;
+    return false;
+  }
+
+  // Check that slave id matches
+  if (state.command[0] != state.response[0]) {
+    STATUS_WARNING(STAT_OK, "modbus: unexpected slave id, expected=%u got=%u",
+                   state.command[0], state.response[0]);
+    state.status = MODBUS_INVALID;
+    return false;
+  }
+
+  // Check that function code matches
+  if (state.command[1] != state.response[1]) {
+    STATUS_WARNING(STAT_OK, "modbus: invalid function code, expected=%u got=%u",
+                   state.command[1], state.response[1]);
+    state.status = MODBUS_INVALID;
+    return false;
+  }
+
+  return true;
+}
+
+
+static void _notify(uint8_t func, uint8_t bytes, const uint8_t *data) {
+  if (!state.receive_cb) return;
+
+  modbus_cb_t cb = state.receive_cb;
+  state.receive_cb = 0; // May be set in callback
+
+  cb(func, bytes, data);
+}
+
+
+static void _handle_response() {
+  if (!state.response_ready) return;
+  state.response_ready = false;
+
+  if (!_check_response()) return;
+
+  state.last_write = 0;             // Clear timeout timer
+  state.last_read = rtc_get_time(); // Set delay timer
+  state.retry = 0;                  // Reset retry counter
+  state.status = MODBUS_OK;
+  state.busy = false;
+
+  _notify(state.response[1], state.response_length - 4, state.response + 2);
+}
+
+
+/// Data register empty interrupt
+ISR(RS485_DRE_vect) {
+  RS485_PORT.DATA = state.command[state.bytes++];
+
+  if (state.bytes == state.command_length) {
+    _set_dre_interrupt(false);
+    _set_txc_interrupt(true);
+    state.bytes = 0;
+  }
+}
+
+
+/// Transmit complete interrupt
+ISR(RS485_TXC_vect) {
+  _set_txc_interrupt(false);
+  _set_rxc_interrupt(true);
+  _set_write(false); // Switch to read mode
+  state.transmit_complete = true;
+}
+
+
+/// Data received interrupt
+ISR(RS485_RXC_vect) {
+  state.response[state.bytes] = RS485_PORT.DATA;
+
+  // Ignore leading zeros
+  if (state.bytes || state.response[0]) state.bytes++;
+
+  if (state.bytes == state.response_length) {
+    _set_rxc_interrupt(false);
+    _set_write(true); // Back to write mode
+    state.bytes = 0;
+    state.response_ready = true;
+  }
+}
+
+
+static void _read_cb(uint8_t func, uint8_t bytes, const uint8_t *data) {
+  if (func == MODBUS_READ_OUTPUT_REG && data[0] == bytes - 1) {
+    if (state.rw_cb)
+      for (uint8_t i = 0; i < bytes >> 1; i++)
+        state.rw_cb(true, state.addr + i, _read_word(data + i * 2 + 1, false));
+    return;
+  }
+
+  if (state.rw_cb) state.rw_cb(false, state.addr, 0);
+}
+
+
+static void _write_cb(uint8_t func, uint8_t bytes, const uint8_t *data) {
+  if ((func == MODBUS_WRITE_OUTPUT_REG ||
+       func == MODBUS_WRITE_OUTPUT_REGS) && bytes == 4 &&
+      _read_word(data, false) == state.addr) {
+    if (state.rw_cb)
+      state.rw_cb(true, state.addr, _read_word(state.command + 4, false));
+    return;
+  }
+
+  if (state.rw_cb) state.rw_cb(false, state.addr, 0);
+}
+
+
+static void _reset() {
+  _set_dre_interrupt(false);
+  _set_txc_interrupt(false);
+  _set_rxc_interrupt(false);
+  _set_write(true); // RS485 write mode
+
+  // Flush USART
+  uint8_t x = RS485_PORT.DATA;
+  x = RS485_PORT.STATUS;
+  x = x;
+
+  // Clear state
+  state.write_ready = false;
+  state.busy = false;
+}
+
+
+static void _retry() {
+  state.last_write = 0;
+  state.bytes = 0;
+  state.retry++;
+
+  _set_write(true); // RS485 write mode
+
+  _set_txc_interrupt(false);
+  _set_rxc_interrupt(false);
+  _set_dre_interrupt(true);
+
+  // Try changing pin polarity
+  if (state.retry == MODBUS_RETRIES) {
+    PINCTRL_PIN(RS485_RO_PIN) ^= PORT_INVEN_bm;
+    PINCTRL_PIN(RS485_DI_PIN) ^= PORT_INVEN_bm;
+  }
+
+  if (cfg.debug) STATUS_DEBUG("modbus: retry %d", state.retry);
+}
+
+
+static void _timeout() {
+  if (cfg.debug) STATUS_DEBUG("modbus: timedout");
+
+  if (state.status == MODBUS_OK || state.status == MODBUS_DISCONNECTED)
+    state.status = MODBUS_TIMEDOUT;
+
+  _reset();
+  _notify(state.command[1], 0, 0);
+}
+
+
+static stop_t _get_stop() {
+  // RTU mode characters must always be 11-bits long
+  return cfg.parity == USART_NONE ? USART_2STOP : USART_1STOP;
+}
+
+
+void modbus_init() {
+  PR.PRPD &= ~PR_USART1_bm; // Disable power reduction
+
+  DIRCLR_PIN(RS485_RO_PIN); // Input
+  OUTSET_PIN(RS485_DI_PIN); // High
+  DIRSET_PIN(RS485_DI_PIN); // Output
+  OUTSET_PIN(RS485_RW_PIN); // High
+  DIRSET_PIN(RS485_RW_PIN); // Output
+
+  _reset();
+  memset(&state, 0, sizeof(state));
+  state.status = MODBUS_DISCONNECTED;
+
+  usart_init_port(&RS485_PORT, cfg.baud, cfg.parity, USART_8BITS, _get_stop());
+}
+
+
+void modbus_deinit() {
+  _reset();
+  memset(&state, 0, sizeof(state));
+  state.status = MODBUS_DISCONNECTED;
+
+  // Disable USART
+  RS485_PORT.CTRLB &= ~(USART_RXEN_bm | USART_TXEN_bm);
+
+  // Float write pins
+  DIRCLR_PIN(RS485_DI_PIN);
+  DIRCLR_PIN(RS485_RW_PIN);
+}
+
+
+bool modbus_busy() {return state.busy;}
+
+
+static void _start_write() {
+  if (!state.write_ready) return;
+
+  // The minimum delay between modbus messages is 3.5 characters.  There are
+  // 11-bits per character in RTU mode.  Character time is calculated as
+  // follows:
+  //
+  //     char time = 11-bits / baud * 3.5
+  //
+  // At 9600 baud the minimum delay is 4.01ms.  At 19200 it's 2.005ms.  All
+  // supported higher baud rates require the 1.75ms minimum delay.  We round up
+  // and add 1ms to ensure the delay is never less than the required minimum.
+  if (state.last_read &&
+      !rtc_expired(state.last_read + (cfg.baud == USART_BAUD_9600 ? 5 : 3)))
+    return;
+  state.last_read = 0;
+
+  state.write_ready = false;
+  _set_dre_interrupt(true);
+}
+
+
+void modbus_func(uint8_t func, uint8_t send, const uint8_t *data,
+                 uint8_t receive, modbus_cb_t receive_cb) {
+  state.bytes = 0;
+  state.command_length = send + 4;
+  state.response_length = receive + 4;
+  state.receive_cb = receive_cb;
+  state.last_write = 0;
+  state.retry = 0;
+
+  ESTOP_ASSERT(state.command_length <= MODBUS_BUF_SIZE, STAT_MODBUS_BUF_LENGTH);
+  ESTOP_ASSERT(state.response_length <= MODBUS_BUF_SIZE,
+               STAT_MODBUS_BUF_LENGTH);
+
+  state.command[0] = cfg.id;
+  state.command[1] = func;
+  memcpy(state.command + 2, data, send);
+  _write_word(state.command + send + 2, _crc16(state.command, send + 2), true);
+
+  state.busy = true;
+  state.write_ready = true;
+  _start_write();
+}
+
+
+void modbus_read(uint16_t addr, uint16_t count, modbus_rw_cb_t cb) {
+  state.rw_cb = cb;
+  state.addr = addr;
+  uint8_t cmd[4];
+  _write_word(cmd, addr, false);
+  _write_word(cmd + 2, count, false);
+  modbus_func(MODBUS_READ_OUTPUT_REG, 4, cmd, 2 * count + 1, _read_cb);
+}
+
+
+void modbus_write(uint16_t addr, uint16_t value, modbus_rw_cb_t cb) {
+  state.rw_cb = cb;
+  state.addr = addr;
+  uint8_t cmd[4];
+  _write_word(cmd, addr, false);
+  _write_word(cmd + 2, value, false);
+  modbus_func(MODBUS_WRITE_OUTPUT_REG, 4, cmd, 4, _write_cb);
+}
+
+
+void modbus_multi_write(uint16_t addr, uint16_t value, modbus_rw_cb_t cb) {
+  state.rw_cb = cb;
+  state.addr = addr;
+  uint8_t cmd[7];
+  _write_word(cmd, addr, false);      // Start address
+  _write_word(cmd + 2, 1, false);     // Number of regs
+  cmd[4] = 2;                         // Number of bytes
+  _write_word(cmd + 5, value, false); // Value
+  modbus_func(MODBUS_WRITE_OUTPUT_REGS, 7, cmd, 4, _write_cb);
+}
+
+
+void modbus_callback() {
+  if (state.transmit_complete) {
+    state.last_write = rtc_get_time();
+    state.transmit_complete = false;
+  }
+
+  _handle_response();
+  _start_write();
+
+  // Timeout out writes
+  if (!state.last_write || !rtc_expired(state.last_write + MODBUS_TIMEOUT))
+    return;
+  state.last_write = 0;
+
+  if (cfg.debug && state.bytes) {
+    char sent[state.command_length * 2 + 1];
+    char received[state.bytes * 2 + 1];
+
+    format_hex_buf(sent, state.command, state.command_length);
+    format_hex_buf(received, state.response, state.bytes);
+
+    STATUS_DEBUG("modbus: sent 0x%s received 0x%s expected %u bytes",
+                 sent, received, state.response_length);
+  }
+
+  if (state.retry < 2 * MODBUS_RETRIES) _retry();
+  else _timeout();
+}
+
+
+// Variable callbacks
+bool get_mb_debug() {return cfg.debug;}
+void set_mb_debug(bool value) {cfg.debug = value;}
+uint8_t get_mb_id() {return cfg.id;}
+void set_mb_id(uint8_t id) {cfg.id = id;}
+uint8_t get_mb_baud() {return cfg.baud;}
+
+
+void set_mb_baud(uint8_t baud) {
+  cfg.baud = (baud_t)baud;
+  usart_set_baud(&RS485_PORT, cfg.baud);
+}
+
+
+uint8_t get_mb_parity() {return cfg.parity;}
+
+
+void set_mb_parity(uint8_t parity) {
+  cfg.parity = (parity_t)parity;
+  usart_set_parity(&RS485_PORT, cfg.parity);
+  usart_set_stop(&RS485_PORT, _get_stop());
+}
+
+
+uint8_t get_mb_status() {return state.status;}
+uint16_t get_mb_crc_errs() {return state.crc_errs;}

src/avr/src/modbus.h

@@ -0,0 +1,112 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+/******************************************************************************\
+
+  Modbus RTU command format is as follows:
+
+    Function 01: Read Coil Status
+    Function 02: Read Input Status
+
+           Send: [id][func][addr][count][crc]
+        Receive: [id][func][bytes][flags][crc]
+
+    Function 03: Read Holding Registers
+    Function 04: Read Input Registers
+
+           Send: [id][func][addr][count][crc]
+        Receive: [id][func][bytes][regs][crc]
+
+    Function 05: Force Single Coil
+    Function 06: Preset Single Register
+
+           Send: [id][func][addr][value][crc]
+        Receive: [id][func][addr][value][crc]
+
+    Function 15: Force Multiple Coils
+
+           Send: [id][func][addr][count][bytes][flags][crc]
+        Receive: [id][func][addr][count][crc]
+
+    Function 16: Preset Multiple Registers
+
+           Send: [id][func][addr][count][bytes][regs][crc]
+        Receive: [id][func][addr][count][crc]
+
+  Where:
+
+             id: 1-byte   Slave ID
+           func: 1-byte   Function code
+           addr: 2-byte   Data address
+          count: 2-byte   Address count
+          bytes: 1-byte   Number of bytes to follow
+          value: 2-byte   Value read or written
+           bits: n-bytes  Flags indicating on/off
+           regs: n-bytes  register values to write
+       checksum: 16-bit   CRC: x^16 + x^15 + x^2 + 1 (0x8005) initial: 0xffff
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include <stdint.h>
+#include <stdbool.h>
+
+
+typedef enum {
+  MODBUS_READ_COILS        = 1,
+  MODBUS_READ_CONTACTS     = 2,
+  MODBUS_READ_OUTPUT_REG   = 3,
+  MODBUS_READ_INPUT_REG    = 4,
+  MODBUS_WRITE_COIL        = 5,
+  MODBUS_WRITE_OUTPUT_REG  = 6,
+  MODBUS_WRITE_COILS       = 15,
+  MODBUS_WRITE_OUTPUT_REGS = 16,
+} modbus_function_code_t;
+
+
+typedef enum {
+  MODBUS_COIL_BASE       = 0,
+  MODBUS_CONTACT_BASE    = 10000,
+  MODBUS_INPUT_REG_BASE  = 30000,
+  MODBUS_OUTPUT_REG_BASE = 40000,
+} modbus_base_addrs_t;
+
+
+typedef void (*modbus_cb_t)(uint8_t func, uint8_t bytes, const uint8_t *data);
+typedef void (*modbus_rw_cb_t)(bool ok, uint16_t addr, uint16_t value);
+
+void modbus_init();
+void modbus_deinit();
+bool modbus_busy();
+void modbus_func(uint8_t func, uint8_t send, const uint8_t *data,
+                 uint8_t receive, modbus_cb_t cb);
+void modbus_read(uint16_t addr, uint16_t count, modbus_rw_cb_t cb);
+void modbus_write(uint16_t addr, uint16_t value, modbus_rw_cb_t cb);
+void modbus_multi_write(uint16_t addr, uint16_t value, modbus_rw_cb_t cb);
+void modbus_callback();

src/avr/src/motor.c

@@ -0,0 +1,497 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "motor.h"
+#include "config.h"
+#include "hardware.h"
+#include "cpp_magic.h"
+#include "rtc.h"
+#include "report.h"
+#include "stepper.h"
+#include "drv8711.h"
+#include "estop.h"
+#include "axis.h"
+#include "util.h"
+#include "pgmspace.h"
+#include "exec.h"
+
+#include <util/delay.h>
+
+#include <string.h>
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+
+typedef struct {
+  // Config
+  uint8_t axis;                  // map motor to axis
+  uint8_t step_pin;
+  uint8_t dir_pin;
+  TC0_t *timer;
+  DMA_CH_t *dma;
+  uint8_t dma_trigger;
+
+  bool slave;
+  uint16_t microsteps;           // microsteps per full step
+  uint16_t stall_microsteps;
+  bool reverse;
+  bool enabled;
+  float step_angle;              // degrees per whole step
+  float travel_rev;              // mm or deg of travel per motor revolution
+  float min_soft_limit;
+  float max_soft_limit;
+  bool homed;
+
+  // Computed
+  float steps_per_unit;
+
+  // Runtime state
+  uint32_t power_timeout;
+  int32_t commanded;
+  int32_t encoder;
+  int16_t error;
+  bool last_negative;
+
+  // Move prep
+  bool prepped;
+  uint8_t clock;
+  uint16_t timer_period;
+  bool negative;
+  int32_t position;
+} motor_t;
+
+
+static motor_t motors[MOTORS] = {
+  {
+    .axis            = AXIS_X,
+    .step_pin        = STEP_0_PIN,
+    .dir_pin         = DIR_0_PIN,
+    .timer           = &TCD0,
+    .dma             = &DMA.CH0,
+    .dma_trigger     = DMA_CH_TRIGSRC_TCD0_CCA_gc,
+  }, {
+    .axis            = AXIS_Y,
+    .step_pin        = STEP_1_PIN,
+    .dir_pin         = DIR_1_PIN,
+    .timer           = &TCE0,
+    .dma             = &DMA.CH1,
+    .dma_trigger     = DMA_CH_TRIGSRC_TCE0_CCA_gc,
+  }, {
+    .axis            = AXIS_Z,
+    .step_pin        = STEP_2_PIN,
+    .dir_pin         = DIR_2_PIN,
+    .timer           = &TCF0,
+    .dma             = &DMA.CH2,
+    .dma_trigger     = DMA_CH_TRIGSRC_TCF0_CCA_gc,
+  }, {
+    .axis            = AXIS_A,
+    .step_pin        = STEP_3_PIN,
+    .dir_pin         = DIR_3_PIN,
+    .timer           = (TC0_t *)&TCE1,
+    .dma             = &DMA.CH3,
+    .dma_trigger     = DMA_CH_TRIGSRC_TCE1_CCA_gc,
+  }
+};
+
+
+static uint8_t _dummy;
+
+
+static void _update_config(int motor) {
+  motor_t *m = &motors[motor];
+
+  m->steps_per_unit = 360.0 * m->microsteps / m->travel_rev / m->step_angle;
+}
+
+
+void motor_init() {
+  // Enable DMA
+  DMA.CTRL = DMA_RESET_bm;
+  DMA.CTRL = DMA_ENABLE_bm;
+  DMA.INTFLAGS = 0xff; // clear all pending interrupts
+
+  for (int motor = 0; motor < MOTORS; motor++) {
+    motor_t *m = &motors[motor];
+
+    // Default soft limits
+    m->min_soft_limit = -INFINITY;
+    m->max_soft_limit = INFINITY;
+
+    _update_config(motor);
+
+    // Setup motor timer
+    m->timer->CTRLB = TC_WGMODE_SINGLESLOPE_gc | TC1_CCAEN_bm;
+    m->timer->CCA = STEP_PULSE_WIDTH;
+
+    // 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;
+
+    // Note, the DMA transfer must read CCA to clear the trigger
+    m->dma->SRCADDR0 = (((uintptr_t)&m->timer->CCA) >> 0) & 0xff;
+    m->dma->SRCADDR1 = (((uintptr_t)&m->timer->CCA) >> 8) & 0xff;
+    m->dma->SRCADDR2 = 0;
+
+    m->dma->DESTADDR0 = (((uintptr_t)&_dummy) >> 0) & 0xff;
+    m->dma->DESTADDR1 = (((uintptr_t)&_dummy) >> 8) & 0xff;
+    m->dma->DESTADDR2 = 0;
+
+    m->dma->CTRLA = DMA_CH_SINGLE_bm | DMA_CH_BURSTLEN_1BYTE_gc;
+
+    // IO pins
+    PINCTRL_PIN(m->step_pin) = PORT_INVEN_bm; // Inverted output
+    DIRSET_PIN(m->step_pin); // Output
+    DIRSET_PIN(m->dir_pin);  // Output
+  }
+
+  axis_map_motors();
+}
+
+
+bool motor_is_enabled(int motor) {return motors[motor].enabled;}
+int motor_get_axis(int motor) {return motors[motor].axis;}
+
+
+static int32_t _position_to_steps(int motor, float position) {
+  return (int32_t)round(position * motors[motor].steps_per_unit);
+}
+
+
+void motor_set_position(int motor, float position) {
+  motor_t *m = &motors[motor];
+  m->commanded = m->encoder = m->position = _position_to_steps(motor, position);
+  m->error = 0;
+}
+
+
+float motor_get_soft_limit(int motor, bool min) {
+  return min ? motors[motor].min_soft_limit : motors[motor].max_soft_limit;
+}
+
+
+bool motor_get_homed(int motor) {return motors[motor].homed;}
+
+
+static void _update_power(int motor) {
+  motor_t *m = &motors[motor];
+
+  if (m->enabled) {
+    bool timedout = rtc_expired(m->power_timeout);
+    // NOTE, we have ~5ms to update the driver config
+    drv8711_set_state(motor, timedout ? DRV8711_IDLE : DRV8711_ACTIVE);
+
+  } else drv8711_set_state(motor, DRV8711_DISABLED);
+}
+
+
+/// Callback to manage motor power sequencing and power-down timing.
+stat_t motor_rtc_callback() { // called by controller
+  for (int motor = 0; motor < MOTORS; motor++)
+    _update_power(motor);
+
+  return STAT_OK;
+}
+
+
+void motor_emulate_steps(int motor) {
+  motor_t *m = &motors[motor];
+  m->dma->TRFCNT = 0xffff - abs(m->commanded - m->encoder);
+}
+
+
+void motor_end_move(int motor) {
+  motor_t &m = motors[motor];
+
+  if (!m.timer->CTRLA) return;
+
+  // Stop clock
+  m.timer->CTRLA = 0;
+
+  // Wait for pending DMA transfers
+  while (m.dma->CTRLB & DMA_CH_CHPEND_bm) continue;
+
+  // Get actual step count from DMA channel
+  const int32_t steps = 0xffff - m.dma->TRFCNT;
+
+  // Accumulate encoder & compute error
+  m.encoder += m.last_negative ? -steps : steps;
+  m.error = m.commanded - m.encoder;
+}
+
+
+void motor_load_move(int motor) {
+  motor_t &m = motors[motor];
+
+  // Clear move
+  ESTOP_ASSERT(m.prepped, STAT_MOTOR_NOT_PREPPED);
+  m.prepped = false;
+
+  motor_end_move(motor);
+
+  if (!m.timer_period) return; // Leave clock stopped
+
+  // Set direction, compensating for polarity but only when moving
+  const bool dir = m.negative ^ m.reverse;
+  if (dir != IN_PIN(m.dir_pin)) {
+    SET_PIN(m.dir_pin, dir);
+
+    // We need at least 200ns between direction change and next step.
+    if (m.timer->CCA < m.timer->CNT) m.timer->CNT = m.timer->CCA + 1;
+  }
+
+  // Reset DMA step counter
+  m.dma->CTRLA &= ~DMA_CH_ENABLE_bm;
+  m.dma->TRFCNT = 0xffff;
+  m.dma->CTRLA |= DMA_CH_ENABLE_bm;
+
+  // To avoid causing couter wrap around, it is important to start the clock
+  // before setting PERBUF.  If PERBUF is set before the clock is started PER
+  // updates immediately and possibly mid step.
+
+  // Set clock and period
+  m.timer->CTRLA  = m.clock;         // Start clock
+  m.timer->PERBUF = m.timer_period;  // Set next frequency
+  m.last_negative = m.negative;
+  m.commanded     = m.position;
+}
+
+
+void motor_prep_move(int motor, float target) {
+  // Validate input
+  ESTOP_ASSERT(0 <= motor && motor < MOTORS, STAT_MOTOR_ID_INVALID);
+  ESTOP_ASSERT(isfinite(target), STAT_BAD_FLOAT);
+
+  motor_t &m = motors[motor];
+  ESTOP_ASSERT(!m.prepped, STAT_MOTOR_NOT_READY);
+
+  // Travel in steps
+  int32_t position = _position_to_steps(motor, target);
+  int24_t steps = position - m.position;
+  m.position = position;
+
+  // Error correction
+  int16_t correction = abs(m.error);
+  if (MIN_STEP_CORRECTION <= correction) {
+    // Dampen correction oscillation
+    correction >>= 1;
+
+    // Make correction
+    steps += m.error < 0 ? -correction : correction;
+  }
+
+  // Positive steps from here on
+  m.negative = steps < 0;
+  if (m.negative) steps = -steps;
+
+  // Start with clock / 2
+  const float seg_clocks = SEGMENT_TIME * (F_CPU * 60 / 2);
+  float ticks_per_step = seg_clocks / steps;
+
+  // Use faster clock with faster step rates for increased resolution.
+  if (ticks_per_step < 0x7fff) {
+    ticks_per_step *= 2;
+    m.clock = TC_CLKSEL_DIV1_gc;
+
+    // Limit clock if step rate is too fast
+    // We allow a slight fudge here (i.e. 1.9 instead 2) because the motor
+    // driver is able to handle it and otherwise we could not actually hit
+    // an average rate of 250k usteps/sec.
+    if (ticks_per_step < STEP_PULSE_WIDTH * 1.9)
+      ticks_per_step = STEP_PULSE_WIDTH * 1.9; // Too fast
+
+  } else m.clock = TC_CLKSEL_DIV2_gc; // NOTE, pulse width will be twice as long
+
+  // Disable clock if too slow
+  if (0xffff <= ticks_per_step) ticks_per_step = 0;
+
+  m.timer_period = steps ? round(ticks_per_step) : 0;
+
+  // Power motor
+  if (!m.enabled) {
+    m.timer_period = 0;
+    m.encoder = m.commanded = m.position;
+    m.error = 0;
+
+  } else if (m.timer_period) // Motor is moving so reset power timeout
+    m.power_timeout = rtc_get_time() + MOTOR_IDLE_TIMEOUT * 1000;
+  _update_power(motor);
+
+  // Queue move
+  m.prepped = true;
+}
+
+
+// Var callbacks
+bool get_motor_enabled(int motor) {return motors[motor].enabled;}
+
+
+void set_motor_enabled(int motor, bool enabled) {
+  if (motors[motor].slave) return;
+
+  for (int m = motor; m < MOTORS; m++)
+    if (motors[m].axis == motors[motor].axis)
+      motors[m].enabled = enabled;
+}
+
+
+float get_step_angle(int motor) {return motors[motor].step_angle;}
+
+
+void set_step_angle(int motor, float value) {
+  if (motors[motor].slave) return;
+
+  for (int m = motor; m < MOTORS; m++)
+    if (motors[m].axis == motors[motor].axis) {
+      motors[m].step_angle = value;
+      _update_config(m);
+    }
+}
+
+
+float get_travel(int motor) {return motors[motor].travel_rev;}
+
+
+void set_travel(int motor, float value) {
+  if (motors[motor].slave) return;
+
+  for (int m = motor; m < MOTORS; m++)
+    if (motors[m].axis == motors[motor].axis) {
+      motors[m].travel_rev = value;
+      _update_config(m);
+    }
+}
+
+
+uint16_t get_microstep(int motor) {return motors[motor].microsteps;}
+
+void set_microstep(int motor, uint16_t value) {
+  switch (value) {
+  case 1: case 2: case 4: case 8: case 16: case 32: case 64: case 128: case 256:
+    break;
+  default: return;
+  }
+
+  if (motors[motor].slave) return;
+
+  for (int m = motor; m < MOTORS; m++)
+    if (motors[m].axis == motors[motor].axis) {
+      motors[m].microsteps = value;
+      _update_config(m);
+      drv8711_set_microsteps(m, value);
+    }
+}
+
+uint16_t get_stall_microstep(int motor) {return motors[motor].stall_microsteps;}
+
+void set_stall_microstep(int motor, uint16_t value) {
+  switch (value) {
+  case 1: case 2: case 4: case 8: case 16: case 32: case 64: case 128: case 256:
+    break;
+  default: return;
+  }
+
+  if (motors[motor].slave) return;
+
+  for (int m = motor; m < MOTORS; m++)
+    if (motors[m].axis == motors[motor].axis) {
+      motors[m].stall_microsteps = value;
+      //_update_config(m);
+      //drv8711_set_stall_microsteps(m, value);
+    }
+}
+
+void enable_stall_microstep(int m)
+{
+    //for (int m = 0; m < MOTORS; m++)
+    //{
+        motor_t *motorTemp = &motors[m];
+        motorTemp->steps_per_unit = 360.0 * motorTemp->stall_microsteps / motorTemp->travel_rev / motorTemp->step_angle;
+        drv8711_set_microsteps(m, motors[m].stall_microsteps);
+    //}
+}
+
+void disable_stall_microstep()
+{
+    for(int m =0; m < MOTORS; m++)
+    {
+        _update_config(m);  //Restore the steps_per_unit
+        drv8711_set_microsteps(m, motors[m].microsteps);
+    }
+}
+
+char get_motor_axis(int motor) {return motors[motor].axis;}
+
+
+void set_motor_axis(int motor, uint8_t axis) {
+  if (MOTORS <= motor || AXES <= axis || axis == motors[motor].axis) return;
+  motors[motor].axis = axis;
+  axis_map_motors();
+
+  // Reset encoder
+  motor_set_position(motor, exec_get_axis_position(axis));
+
+  // Check if this is now a slave motor
+  motors[motor].slave = false;
+  for (int m = 0; m < motor; m++)
+    if (motors[m].axis == motors[motor].axis) {
+      // Sync with master
+      set_step_angle(motor, motors[m].step_angle);
+      set_travel(motor, motors[m].travel_rev);
+      set_microstep(motor, motors[m].microsteps);
+      set_stall_microstep(motor, motors[m].stall_microsteps);
+      set_motor_enabled(motor, motors[m].enabled);
+      motors[motor].slave = true; // Must be last
+      break;
+    }
+}
+
+
+bool get_reverse(int motor) {return motors[motor].reverse;}
+void set_reverse(int motor, bool value) {motors[motor].reverse = value;}
+
+
+float get_min_soft_limit(int motor) {return motors[motor].min_soft_limit;}
+float get_max_soft_limit(int motor) {return motors[motor].max_soft_limit;}
+
+
+void set_min_soft_limit(int motor, float limit) {
+  motors[motor].min_soft_limit = limit;
+}
+
+
+void set_max_soft_limit(int motor, float limit) {
+  motors[motor].max_soft_limit = limit;
+}
+
+
+bool get_homed(int motor) {return motors[motor].homed;}
+void set_homed(int motor, bool homed) {motors[motor].homed = homed;}
+
+
+int32_t get_encoder(int m) {return motors[m].encoder;}
+int32_t get_error(int m) {return motors[m].error;}

src/avr/src/motor.h

@@ -0,0 +1,61 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "status.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+
+
+typedef enum {
+  MOTOR_DISABLED,                 // motor enable is deactivated
+  MOTOR_ALWAYS_POWERED,           // motor is always powered while machine is ON
+  MOTOR_POWERED_IN_CYCLE,         // motor fully powered during cycles,
+                                  // de-powered out of cycle
+  MOTOR_POWERED_ONLY_WHEN_MOVING, // idles shortly after stopped, even in cycle
+} motor_power_mode_t;
+
+
+void motor_init();
+
+bool motor_is_enabled(int motor);
+int motor_get_axis(int motor);
+void motor_set_position(int motor, float position);
+float motor_get_soft_limit(int motor, bool min);
+bool motor_get_homed(int motor);
+
+stat_t motor_rtc_callback();
+
+void motor_end_move(int motor);
+void motor_load_move(int motor);
+void motor_prep_move(int motor, float target);
+
+void enable_stall_microstep(int m);
+void disable_stall_microstep();
+

src/avr/src/outputs.c

@@ -0,0 +1,161 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "outputs.h"
+#include "config.h"
+
+
+typedef struct {
+  uint8_t pin;
+  bool active;
+  output_state_t state;
+  output_mode_t mode;
+} output_t;
+
+
+output_t outputs[OUTS] = {
+  {TOOL_ENABLE_PIN},
+  {TOOL_DIR_PIN},
+  {SWITCH_1_PIN},
+  {SWITCH_2_PIN},
+  {FAULT_PIN},
+  {TEST_PIN},
+};
+
+
+static output_t *_get_output(uint8_t pin) {
+  switch (pin) {
+  case TOOL_ENABLE_PIN: return &outputs[0];
+  case TOOL_DIR_PIN:    return &outputs[1];
+  case SWITCH_1_PIN:    return &outputs[2];
+  case SWITCH_2_PIN:    return &outputs[3];
+  case FAULT_PIN:       return &outputs[4];
+  case TEST_PIN:        return &outputs[5];
+  }
+
+  return 0;
+}
+
+
+static void _update_state(output_t *output) {
+  switch (output->mode) {
+  case OUT_DISABLED: output->state = OUT_TRI; break;
+  case OUT_LO_HI:  output->state = output->active ? OUT_HI  : OUT_LO;  break;
+  case OUT_HI_LO:  output->state = output->active ? OUT_LO  : OUT_HI;  break;
+  case OUT_TRI_LO: output->state = output->active ? OUT_LO  : OUT_TRI; break;
+  case OUT_TRI_HI: output->state = output->active ? OUT_HI  : OUT_TRI; break;
+  case OUT_LO_TRI: output->state = output->active ? OUT_TRI : OUT_LO;  break;
+  case OUT_HI_TRI: output->state = output->active ? OUT_TRI : OUT_HI;  break;
+  }
+
+  switch (output->state) {
+  case OUT_TRI: DIRCLR_PIN(output->pin); break;
+  case OUT_HI:  OUTSET_PIN(output->pin); DIRSET_PIN(output->pin); break;
+  case OUT_LO:  OUTCLR_PIN(output->pin); DIRSET_PIN(output->pin); break;
+  }
+}
+
+
+void outputs_init() {
+  for (int i = 0; i < OUTS; i++) _update_state(&outputs[i]);
+}
+
+
+bool outputs_is_active(uint8_t pin) {
+  output_t *output = _get_output(pin);
+  return output ? output->active : false;
+}
+
+
+void outputs_set_active(uint8_t pin, bool active) {
+  output_t *output = _get_output(pin);
+  if (!output) return;
+
+  output->active = active;
+  _update_state(output);
+}
+
+
+bool outputs_toggle(uint8_t pin) {
+  output_t *output = _get_output(pin);
+  if (!output) return false;
+
+  output->active = !output->active;
+  _update_state(output);
+  return output->active;
+}
+
+
+void outputs_set_mode(uint8_t pin, output_mode_t mode) {
+  output_t *output = _get_output(pin);
+  if (!output) return;
+  output->mode = mode;
+  _update_state(output);
+}
+
+
+output_state_t outputs_get_state(uint8_t pin) {
+  output_t *output = _get_output(pin);
+  if (output) return OUT_TRI;
+  return output->state;
+}
+
+
+void outputs_stop() {
+  outputs_set_active(SWITCH_1_PIN, false);
+  outputs_set_active(SWITCH_2_PIN, false);
+}
+
+
+// Var callbacks
+uint8_t get_output_state(int id) {
+  return OUTS <= id ? OUT_TRI : outputs[id].state;
+}
+
+
+bool get_output_active(int id) {
+  return OUTS <= id ? false : outputs[id].active;
+}
+
+
+void set_output_active(int id, bool active) {
+  if (OUTS <= id) return;
+  outputs[id].active = active;
+  _update_state(&outputs[id]);
+}
+
+
+uint8_t get_output_mode(int id) {
+  return OUTS <= id ? OUT_DISABLED : outputs[id].mode;
+}
+
+
+void set_output_mode(int id, uint8_t mode) {
+  if (OUTS <= id) return;
+  outputs[id].mode = (output_mode_t)mode;
+  _update_state(&outputs[id]);
+}

src/avr/src/outputs.h

@@ -0,0 +1,59 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
+
+typedef enum {
+  OUT_LO,
+  OUT_HI,
+  OUT_TRI,
+} output_state_t;
+
+
+/// OUT_<inactive>_<active>
+typedef enum {
+  OUT_DISABLED,
+  OUT_LO_HI,
+  OUT_HI_LO,
+  OUT_TRI_LO,
+  OUT_TRI_HI,
+  OUT_LO_TRI,
+  OUT_HI_TRI,
+} output_mode_t;
+
+
+void outputs_init();
+bool outputs_is_active(uint8_t pin);
+void outputs_set_active(uint8_t pin, bool active);
+bool outputs_toggle(uint8_t pin);
+void outputs_set_mode(uint8_t pin, output_mode_t mode);
+output_state_t outputs_get_state(uint8_t pin);
+void outputs_stop();

src/avr/src/pgmspace.h

@@ -0,0 +1,36 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include <avr/pgmspace.h>
+
+#ifdef __AVR__
+#define PRPSTR "S"
+#else
+#define PRPSTR "s"
+#endif

src/avr/src/pins.c

@@ -0,0 +1,31 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "pins.h"
+
+
+PORT_t *pin_ports[] = {&PORTA, &PORTB, &PORTC, &PORTD, &PORTE, &PORTF};

src/avr/src/pins.h

@@ -0,0 +1,57 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+enum {PORT_A = 1, PORT_B, PORT_C, PORT_D, PORT_E, PORT_F};
+
+#define PIN_INDEX(PIN) (PIN & 7)
+#define PORT_INDEX(PIN) ((PIN >> 3) - 1)
+
+#define PIN_PORT(PIN) pin_ports[PORT_INDEX(PIN)]
+#define PIN_BM(PIN) (1 << PIN_INDEX(PIN))
+
+#define PIN_ID(PORT, PIN) (PORT << 3 | (PIN & 7))
+
+#include <avr/io.h>
+
+extern PORT_t *pin_ports[];
+
+#define DIRSET_PIN(PIN) PIN_PORT(PIN)->DIRSET = PIN_BM(PIN)
+#define DIRCLR_PIN(PIN) PIN_PORT(PIN)->DIRCLR = PIN_BM(PIN)
+#define OUTCLR_PIN(PIN) PIN_PORT(PIN)->OUTCLR = PIN_BM(PIN)
+#define OUTSET_PIN(PIN) PIN_PORT(PIN)->OUTSET = PIN_BM(PIN)
+#define OUTTGL_PIN(PIN) PIN_PORT(PIN)->OUTTGL = PIN_BM(PIN)
+#define OUT_PIN(PIN) (!!(PIN_PORT(PIN)->OUT & PIN_BM(PIN)))
+#define IN_PIN(PIN) (!!(PIN_PORT(PIN)->IN & PIN_BM(PIN)))
+#define PINCTRL_PIN(PIN) ((&PIN_PORT(PIN)->PIN0CTRL)[PIN_INDEX(PIN)])
+
+#define SET_PIN(PIN, X) \
+  do {if (X) OUTSET_PIN(PIN); else OUTCLR_PIN(PIN);} while (0);
+
+#define PIN_EVSYS_CHMUX(PIN) \
+  (EVSYS_CHMUX_PORTA_PIN0_gc + (8 * PORT_INDEX(PIN)) + PIN_INDEX(PIN))

src/avr/src/pwm.c

@@ -0,0 +1,221 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "pwm.h"
+
+#include "config.h"
+#include "estop.h"
+#include "outputs.h"
+
+#include <math.h>
+
+
+typedef struct {
+  bool initialized;
+  float freq; // base frequency for PWM driver, in Hz
+  float min_duty;
+  float max_duty;
+  float power;
+} pwm_t;
+
+
+static pwm_t pwm = {0};
+
+
+static void _set_enable(bool enable) {
+  outputs_set_active(TOOL_ENABLE_PIN, enable);
+}
+
+
+static void _set_dir(bool clockwise) {
+  outputs_set_active(TOOL_DIR_PIN, !clockwise);
+}
+
+
+static void _update_clock(uint16_t period) {
+  if (estop_triggered()) period = 0;
+
+  // Disable
+  if (!period) {
+    TIMER_PWM.CTRLB = 0; // Disable clock control of pin
+    OUTCLR_PIN(PWM_PIN);
+    _set_enable(false);
+    return;
+  }
+  _set_enable(true);
+
+  // 100% duty
+  if (period == 0xffff) {
+    TIMER_PWM.CTRLB = 0; // Disable clock control of pin
+    OUTSET_PIN(PWM_PIN);
+    return;
+  }
+
+  // Configure clock
+  TIMER_PWM.CTRLB = TC1_CCAEN_bm | TC_WGMODE_SINGLESLOPE_gc;
+  TIMER_PWM.CCA = period;
+}
+
+
+static float _compute_duty(float power) {
+  power = fabsf(power);
+  if (!power) return 0; // 0% duty
+  if (power == 1 && pwm.max_duty == 1) return 1; // 100% duty
+  return power * (pwm.max_duty - pwm.min_duty) + pwm.min_duty;
+}
+
+
+static uint16_t _compute_period(float duty) {return TIMER_PWM.PER * duty;}
+
+
+static void _update_pwm() {
+  if (pwm.initialized)
+    _update_clock(_compute_period(_compute_duty(pwm.power)));
+}
+
+
+static void _update_freq() {
+  // Set clock period and optimal prescaler value
+  float prescale = (F_CPU >> 16) / pwm.freq;
+
+  if (prescale <= 1) {
+    TIMER_PWM.PER = F_CPU / pwm.freq;
+    TIMER_PWM.CTRLA = TC_CLKSEL_DIV1_gc;
+
+  } else if (prescale <= 2) {
+    TIMER_PWM.PER = F_CPU / 2 / pwm.freq;
+    TIMER_PWM.CTRLA = TC_CLKSEL_DIV2_gc;
+
+  } else if (prescale <= 4) {
+    TIMER_PWM.PER = F_CPU / 4 / pwm.freq;
+    TIMER_PWM.CTRLA = TC_CLKSEL_DIV4_gc;
+
+  } else if (prescale <= 8) {
+    TIMER_PWM.PER = F_CPU / 8 / pwm.freq;
+    TIMER_PWM.CTRLA = TC_CLKSEL_DIV8_gc;
+
+  } else if (prescale <= 64) {
+    TIMER_PWM.PER = F_CPU / 64 / pwm.freq;
+    TIMER_PWM.CTRLA = TC_CLKSEL_DIV64_gc;
+
+  } else TIMER_PWM.CTRLA = 0;
+
+  _update_pwm();
+}
+
+
+void pwm_init() {
+  pwm.initialized = true;
+  pwm.power = 0;
+
+  // Configure IO
+  _set_dir(true);
+  _set_enable(false);
+  _update_freq();
+
+  // PWM output
+  OUTCLR_PIN(PWM_PIN);
+  DIRSET_PIN(PWM_PIN);
+}
+
+
+float pwm_get() {return pwm.power;}
+
+
+void pwm_deinit(deinit_cb_t cb) {
+  pwm.initialized = false;
+
+  _set_enable(false);
+
+  // Float PWM output pin
+  DIRCLR_PIN(PWM_PIN);
+
+  // Disable clock
+  TIMER_PWM.CTRLA = 0;
+
+  cb();
+}
+
+
+power_update_t pwm_get_update(float power) {
+  power_update_t update = {
+    0 <= power ? POWER_FORWARD : POWER_REVERSE,
+    power,
+    _compute_period(_compute_duty(power))
+  };
+
+  return update;
+}
+
+
+// Called from hi-priority stepper interrupt, must be very fast
+void pwm_update(const power_update_t &update) {
+  if (!pwm.initialized || update.state == POWER_IGNORE) return;
+  _update_clock(update.period);
+  if (update.period) _set_dir(update.state == POWER_FORWARD);
+  pwm.power = update.power;
+}
+
+
+// Var callbacks
+float get_pwm_min_duty() {return pwm.min_duty * 100;}
+
+
+void set_pwm_min_duty(float value) {
+  pwm.min_duty = value * 0.01;
+  _update_pwm();
+}
+
+
+float get_pwm_max_duty() {return pwm.max_duty * 100;}
+
+
+void set_pwm_max_duty(float value) {
+  pwm.max_duty = value * 0.01;
+  _update_pwm();
+}
+
+
+float get_pwm_duty() {return _compute_duty(pwm.power);}
+float get_pwm_freq() {return pwm.freq;}
+
+
+void set_pwm_freq(float value) {
+  if (value < 8) value = 8;
+  if (320000 < value) value = 320000;
+  pwm.freq = value;
+  _update_freq();
+}
+
+
+bool get_pwm_invert() {return PINCTRL_PIN(PWM_PIN) & PORT_INVEN_bm;}
+
+
+void set_pwm_invert(bool invert) {
+  if (invert) PINCTRL_PIN(PWM_PIN) |= PORT_INVEN_bm;
+  else PINCTRL_PIN(PWM_PIN) &= ~PORT_INVEN_bm;
+}

src/avr/src/pwm.h

@@ -0,0 +1,37 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "spindle.h"
+
+
+void pwm_init();
+float pwm_get();
+void pwm_deinit(deinit_cb_t cb);
+power_update_t pwm_get_update(float power);
+void pwm_update(const power_update_t &update);

src/avr/src/report.c

@@ -0,0 +1,54 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "report.h"
+#include "config.h"
+#include "usart.h"
+#include "rtc.h"
+#include "vars.h"
+
+
+static bool _full = false;
+static uint32_t _last = 0;
+
+
+void report_request_full() {_full = true;}
+
+
+void report_callback() {
+  // Wait until output buffer is empty
+  if (!usart_tx_empty()) return;
+
+  // Limit frequency
+  uint32_t now = rtc_get_time();
+  if (now - _last < REPORT_RATE) return;
+  _last = now;
+
+  // Report vars
+  vars_report(_full);
+  _full = false;
+}

src/avr/src/report.h

@@ -0,0 +1,34 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include "status.h"
+
+void report_request_full();
+void report_callback();

src/avr/src/ringbuf.def

@@ -0,0 +1,225 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+/* This file defines an X-Macro ring buffer.  It can be used like this:
+ *
+ *   #define RING_BUF_NAME tx_buf
+ *   #define RING_BUF_SIZE 256
+ *   #include "ringbuf.def"
+ *
+ * This will define the following functions:
+ *
+ *   void <name>_init();
+ *   int <name>_empty();
+ *   int <name>_full();
+ *   <type> <name>_peek();
+ *   void <name>_pop();
+ *   void <name>_push(<type> data);
+ *
+ * Where <name> is defined by RING_BUF_NAME and <type> by RING_BUF_TYPE.
+ * RING_BUF_SIZE defines the length of the ring buffer and must be a power of 2.
+ *
+ * The data type and index type both default to uint8_t but can be changed by
+ * defining RING_BUF_TYPE and RING_BUF_INDEX_TYPE respectively.
+ *
+ * By default these functions are declared static inline but this can be changed
+ * by defining RING_BUF_FUNC.
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include <util/atomic.h>
+
+
+#ifndef RING_BUF_NAME
+#error Must define RING_BUF_NAME
+#endif
+
+#ifndef RING_BUF_SIZE
+#error Must define RING_BUF_SIZE
+#endif
+
+#ifndef RING_BUF_TYPE
+#define RING_BUF_TYPE uint8_t
+#endif
+
+#ifndef RING_BUF_INDEX_TYPE
+#define RING_BUF_INDEX_TYPE volatile uint8_t
+#endif
+
+#ifndef RING_BUF_FUNC
+#define RING_BUF_FUNC static inline
+#endif
+
+#define RING_BUF_MASK (RING_BUF_SIZE - 1)
+#if (RING_BUF_SIZE & RING_BUF_MASK)
+#error RING_BUF_SIZE is not a power of 2
+#endif
+
+#ifndef CONCAT
+#define _CONCAT(prefix, name) prefix##name
+#define CONCAT(prefix, name) _CONCAT(prefix, name)
+#endif
+
+#define RING_BUF_STRUCT CONCAT(RING_BUF_NAME, _ring_buf_t)
+#define RING_BUF RING_BUF_NAME
+
+typedef struct {
+  RING_BUF_TYPE buf[RING_BUF_SIZE];
+  RING_BUF_INDEX_TYPE head;
+  RING_BUF_INDEX_TYPE tail;
+} RING_BUF_STRUCT;
+
+static RING_BUF_STRUCT RING_BUF;
+
+
+RING_BUF_FUNC void CONCAT(RING_BUF_NAME, _init)() {
+  RING_BUF.head = RING_BUF.tail = 0;
+}
+
+
+#define RING_BUF_INC(x) (((x) + 1) & RING_BUF_MASK)
+#define RING_BUF_DEC(x) ((x) ? x - 1 : (RING_BUF_SIZE - 1))
+
+
+#ifdef RING_BUF_ATOMIC_COPY
+
+#undef RING_BUF_ATOMIC_COPY
+#define RING_BUF_ATOMIC_COPY(TO, FROM)          \
+  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) TO = FROM
+
+
+#define RING_BUF_ATOMIC_READ_INDEX(INDEX)                               \
+  RING_BUF_FUNC RING_BUF_INDEX_TYPE CONCAT(RING_BUF_NAME, _read_##INDEX)() { \
+    RING_BUF_INDEX_TYPE index;                                          \
+    RING_BUF_ATOMIC_COPY(index, RING_BUF.INDEX);                        \
+    return index;                                                       \
+  }
+
+
+#define RING_BUF_ATOMIC_WRITE_INDEX(INDEX)                              \
+  RING_BUF_FUNC void CONCAT(RING_BUF_NAME, _write_##INDEX)              \
+    (RING_BUF_INDEX_TYPE value) {                                       \
+    RING_BUF_ATOMIC_COPY(RING_BUF.INDEX, value);                        \
+  }
+
+
+RING_BUF_ATOMIC_READ_INDEX(head);
+RING_BUF_ATOMIC_READ_INDEX(tail);
+RING_BUF_ATOMIC_WRITE_INDEX(head);
+RING_BUF_ATOMIC_WRITE_INDEX(tail);
+
+#define RING_BUF_READ_INDEX(INDEX) CONCAT(RING_BUF_NAME, _read_##INDEX)()
+#define RING_BUF_WRITE_INDEX(INDEX, VALUE) \
+  CONCAT(RING_BUF_NAME, _write_##INDEX)(VALUE)
+
+
+#else // RING_BUF_ATOMIC_COPY
+#define RING_BUF_READ_INDEX(INDEX) RING_BUF.INDEX
+#define RING_BUF_WRITE_INDEX(INDEX, VALUE) RING_BUF.INDEX = VALUE
+#endif // RING_BUF_ATOMIC_COPY
+
+
+RING_BUF_FUNC bool CONCAT(RING_BUF_NAME, _empty)() {
+  return RING_BUF_READ_INDEX(head) == RING_BUF_READ_INDEX(tail);
+}
+
+
+RING_BUF_FUNC bool CONCAT(RING_BUF_NAME, _full)() {
+  return RING_BUF_READ_INDEX(head) == RING_BUF_INC(RING_BUF_READ_INDEX(tail));
+}
+
+
+RING_BUF_FUNC RING_BUF_INDEX_TYPE CONCAT(RING_BUF_NAME, _fill)() {
+  return
+    (RING_BUF_READ_INDEX(tail) - RING_BUF_READ_INDEX(head)) & RING_BUF_MASK;
+}
+
+
+RING_BUF_FUNC RING_BUF_INDEX_TYPE CONCAT(RING_BUF_NAME, _space)() {
+  return (RING_BUF_SIZE - 1) - CONCAT(RING_BUF_NAME, _fill)();
+}
+
+
+RING_BUF_FUNC RING_BUF_TYPE CONCAT(RING_BUF_NAME, _peek)() {
+  return RING_BUF.buf[RING_BUF_READ_INDEX(head)];
+}
+
+
+RING_BUF_FUNC RING_BUF_TYPE *CONCAT(RING_BUF_NAME, _front)() {
+  return &RING_BUF.buf[RING_BUF_READ_INDEX(head)];
+}
+
+
+RING_BUF_FUNC RING_BUF_TYPE *CONCAT(RING_BUF_NAME, _back)() {
+  return &RING_BUF.buf[RING_BUF_DEC(RING_BUF_READ_INDEX(tail))];
+}
+
+
+RING_BUF_FUNC RING_BUF_TYPE CONCAT(RING_BUF_NAME, _get)(int offset) {
+  return RING_BUF.buf[(RING_BUF_READ_INDEX(head) + offset) & RING_BUF_MASK];
+}
+
+
+RING_BUF_FUNC void CONCAT(RING_BUF_NAME, _pop)() {
+  RING_BUF_WRITE_INDEX(head, RING_BUF_INC(RING_BUF_READ_INDEX(head)));
+}
+
+
+RING_BUF_FUNC RING_BUF_TYPE CONCAT(RING_BUF_NAME, _next)() {
+  RING_BUF_TYPE x = CONCAT(RING_BUF_NAME, _peek)();
+  CONCAT(RING_BUF_NAME, _pop)();
+  return x;
+}
+
+
+RING_BUF_FUNC void CONCAT(RING_BUF_NAME, _push)(RING_BUF_TYPE data) {
+  RING_BUF.buf[RING_BUF_READ_INDEX(tail)] = data;
+  RING_BUF_WRITE_INDEX(tail, RING_BUF_INC(RING_BUF_READ_INDEX(tail)));
+}
+
+
+#undef RING_BUF
+#undef RING_BUF_STRUCT
+#undef RING_BUF_INC
+#undef RING_BUF_MASK
+
+#undef RING_BUF_NAME
+#undef RING_BUF_SIZE
+#undef RING_BUF_TYPE
+#undef RING_BUF_INDEX_TYPE
+#undef RING_BUF_FUNC
+
+#undef RING_BUF_READ_INDEX
+#undef RING_BUF_WRITE_INDEX
+
+#ifdef RING_BUF_ATOMIC_COPY
+#undef RING_BUF_ATOMIC_COPY
+#undef RING_BUF_ATOMIC_READ_INDEX
+#undef RING_BUF_ATOMIC_WRITE_INDEX
+#endif // RING_BUF_ATOMIC_COPY

src/avr/src/rtc.c

@@ -0,0 +1,78 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "rtc.h"
+
+#include "switch.h"
+#include "analog.h"
+#include "motor.h"
+#include "lcd.h"
+#include "vfd_spindle.h"
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <avr/wdt.h>
+
+#include <string.h>
+
+
+static uint32_t ticks;
+
+
+ISR(RTC_OVF_vect) {
+  ticks++;
+
+  lcd_rtc_callback();
+  switch_rtc_callback();
+  analog_rtc_callback();
+  vfd_spindle_rtc_callback();
+  if (!(ticks & 255)) motor_rtc_callback();
+  wdt_reset();
+}
+
+
+/// Initialize and start the clock
+/// This routine follows the code in app note AVR1314.
+void rtc_init() {
+  ticks = 0;
+
+  OSC.CTRL |= OSC_RC32KEN_bm;                         // enable internal 32kHz.
+  while (!(OSC.STATUS & OSC_RC32KRDY_bm));            // 32kHz osc stabilize
+  while (RTC.STATUS & RTC_SYNCBUSY_bm);               // wait RTC not busy
+
+  CLK.RTCCTRL = CLK_RTCSRC_RCOSC32_gc | CLK_RTCEN_bm; // 32kHz clock as RTC src
+  while (RTC.STATUS & RTC_SYNCBUSY_bm);               // wait RTC not busy
+
+  // the following must be in this order or it doesn't work
+  RTC.PER = 33;                        // overflow period ~1ms
+  RTC.INTCTRL = RTC_OVFINTLVL_LO_gc;   // overflow LO interrupt
+  RTC.CTRL = RTC_PRESCALER_DIV1_gc;    // no prescale
+}
+
+
+uint32_t rtc_get_time() {return ticks;}
+bool rtc_expired(uint32_t t) {return 0 <= (int32_t)(ticks - t);}

src/avr/src/rtc.h

@@ -0,0 +1,37 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include <stdint.h>
+#include <stdbool.h>
+
+void rtc_init();
+uint32_t rtc_get_time();
+int32_t rtc_diff(uint32_t t);
+bool rtc_expired(uint32_t t);

src/avr/src/seek.c

@@ -0,0 +1,139 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "seek.h"
+
+#include "command.h"
+#include "switch.h"
+#include "estop.h"
+#include "util.h"
+#include "state.h"
+
+#include <stdint.h>
+
+#include "motor.h"
+
+enum {
+  SEEK_ACTIVE = 1 << 0,
+  SEEK_ERROR  = 1 << 1,
+  SEEK_FOUND  = 1 << 2,
+};
+
+
+typedef struct {
+  bool active;
+  switch_id_t sw;
+  uint8_t flags;
+} seek_t;
+
+
+static seek_t seek = {false, SW_INVALID, 0};
+
+
+switch_id_t seek_get_switch() {return seek.active ? seek.sw : SW_INVALID;}
+
+bool seek_active() {
+  return seek.active;
+}
+
+bool seek_switch_found() {
+  if (!seek.active) return false;
+
+  bool inactive = !(seek.flags & SEEK_ACTIVE);
+
+  if (switch_is_active(seek.sw) ^ inactive) {
+    seek.flags |= SEEK_FOUND;
+    return true;
+  }
+
+  return false;
+}
+
+
+void seek_end() {
+  switch_id_t seekMode;
+
+  seekMode = seek_get_switch();
+
+  if (!seek.active) return;
+
+  if (!(SEEK_FOUND & seek.flags) && (SEEK_ERROR & seek.flags))
+    estop_trigger(STAT_SEEK_NOT_FOUND);
+
+  if((seekMode == SW_STALL_0) || (seekMode == SW_STALL_1) || (seekMode ==SW_STALL_2) || (seekMode == SW_STALL_3))
+    disable_stall_microstep();
+  
+  seek.active = false;
+}
+
+
+void seek_cancel() {
+  switch_id_t seekMode;
+
+  seekMode = seek_get_switch();
+
+
+  if((seekMode == SW_STALL_0) || (seekMode == SW_STALL_1) || (seekMode ==SW_STALL_2) || (seekMode == SW_STALL_3))
+    disable_stall_microstep();
+
+  seek.active = false;
+}
+
+
+// Command callbacks
+stat_t command_seek(char *cmd) {
+  switch_id_t sw = (switch_id_t)decode_hex_nibble(cmd[1]);
+  if (sw <= 0) return STAT_INVALID_ARGUMENTS; // Don't allow seek to ESTOP
+  if (!switch_is_enabled(sw)) return STAT_SEEK_NOT_ENABLED;
+
+  uint8_t flags = decode_hex_nibble(cmd[2]);
+  if (flags & 0xfc) return STAT_INVALID_ARGUMENTS;
+
+  //////////////
+  if(sw == SW_STALL_0) enable_stall_microstep(0);
+  if(sw == SW_STALL_1) enable_stall_microstep(1);
+  if(sw == SW_STALL_2) enable_stall_microstep(2);
+  if(sw == SW_STALL_3) enable_stall_microstep(3);
+
+  seek_t seek = {true, sw, flags};
+  command_push(*cmd, &seek);
+
+  return STAT_OK;
+}
+
+
+unsigned command_seek_size() {return sizeof(seek_t);}
+void command_seek_exec(void *data) {
+    //switch_id_t seekMode;
+
+    //seekMode = seek_get_switch();
+
+    //if((seekMode == SW_STALL_0) || (seekMode == SW_STALL_1) || (seekMode ==SW_STALL_2) || (seekMode == SW_STALL_3))
+    //    enable_stall_microstep();
+    
+    seek = *(seek_t *)data;
+}

src/avr/src/seek.h

@@ -0,0 +1,39 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "switch.h"
+
+#include <stdbool.h>
+
+
+switch_id_t seek_get_switch();
+bool seek_switch_found();
+void seek_end();
+void seek_cancel();
+bool seek_active();

src/avr/src/spindle.c

@@ -0,0 +1,334 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "spindle.h"
+#include "pwm.h"
+#include "huanyang.h"
+#include "vfd_spindle.h"
+#include "stepper.h"
+#include "config.h"
+#include "command.h"
+#include "exec.h"
+#include "util.h"
+
+#include <math.h>
+
+
+typedef struct {
+  float dist;
+  float speed;
+} sync_speed_t;
+
+
+static struct {
+  spindle_type_t type;
+  float override;
+  sync_speed_t sync_speed;
+  float speed;
+  bool reversed;
+  float min_rpm;
+  float max_rpm;
+  float inv_max_rpm;
+
+  bool dynamic_power;
+  float inv_feed;
+
+  spindle_type_t next_type;
+
+} spindle = {
+  .type = SPINDLE_TYPE_DISABLED,
+  .override = 1,
+  .sync_speed = {-1, 0}
+};
+
+
+static float _get_power() {
+  switch (spindle.type) {
+  case SPINDLE_TYPE_DISABLED: return 0;
+  case SPINDLE_TYPE_PWM:      return pwm_get();
+  case SPINDLE_TYPE_HUANYANG: return huanyang_get();
+  default:                    return vfd_spindle_get();
+  }
+}
+
+
+static float _speed_to_power(float speed) {
+  bool negative = speed < 0;
+  float power = fabs(speed * spindle.override);
+
+  if (power < spindle.min_rpm) power = 0;
+  else if (spindle.max_rpm <= power) power = 1;
+  else power *= spindle.inv_max_rpm;
+
+  return (negative ^ spindle.reversed) ? -power : power;
+}
+
+
+static void _set_speed(float speed) {
+  spindle.speed = speed;
+
+  float power = _speed_to_power(speed);
+
+  switch (spindle.type) {
+  case SPINDLE_TYPE_DISABLED: break;
+
+  case SPINDLE_TYPE_PWM: {
+    // PWM speed updates must be synchronized with stepper movement
+    spindle.sync_speed.dist = 0;
+    spindle.sync_speed.speed = speed;
+    break;
+  }
+
+  case SPINDLE_TYPE_HUANYANG: huanyang_set(power); break;
+  default: vfd_spindle_set(power); break;
+  }
+}
+
+
+static void _deinit_cb() {
+  spindle.type = spindle.next_type;
+  spindle.next_type = SPINDLE_TYPE_DISABLED;
+
+  switch (spindle.type) {
+  case SPINDLE_TYPE_DISABLED:                     break;
+  case SPINDLE_TYPE_PWM:      pwm_init();         break;
+  case SPINDLE_TYPE_HUANYANG: huanyang_init();    break;
+  default:                    vfd_spindle_init(); break;
+  }
+
+  spindle_update_speed();
+}
+
+
+static void _set_type(spindle_type_t type) {
+  if (type == spindle.type) return;
+
+  spindle_type_t old_type = spindle.type;
+  spindle.next_type = type;
+  spindle.type = SPINDLE_TYPE_DISABLED;
+
+  switch (old_type) {
+  case SPINDLE_TYPE_DISABLED: _deinit_cb();                   break;
+  case SPINDLE_TYPE_PWM:      pwm_deinit(_deinit_cb);         break;
+  case SPINDLE_TYPE_HUANYANG: huanyang_deinit(_deinit_cb);    break;
+  default:                    vfd_spindle_deinit(_deinit_cb); break;
+  }
+}
+
+
+spindle_type_t spindle_get_type() {return spindle.type;}
+
+
+static power_update_t _get_power_update() {
+  float power = _speed_to_power(spindle.speed);
+
+  // Handle dynamic power
+  if (spindle.dynamic_power && spindle.inv_feed) {
+    float scale = spindle.inv_feed * exec_get_velocity();
+    if (scale < 1) power *= scale;
+  }
+
+  return pwm_get_update(power);
+}
+
+
+void spindle_load_power_updates(power_update_t updates[], float minD,
+                                float maxD) {
+  float stepD = (maxD - minD) * (1.0 / POWER_MAX_UPDATES);
+  float d = minD + 1e-3; // Starting distance
+
+  for (unsigned i = 0; i < POWER_MAX_UPDATES; i++) {
+    bool changed = false;
+    d += stepD; // Ending distance for this power step
+
+    while (true) {
+      // Load new sync speed if needed and available
+      if (spindle.sync_speed.dist < 0 && command_peek() == COMMAND_sync_speed)
+        spindle.sync_speed = *(sync_speed_t *)(command_next() + 1);
+
+      // Exit if we don't have a speed or it's not ready to be set
+      if (spindle.sync_speed.dist == -1 || d < spindle.sync_speed.dist) break;
+
+      // Load sync speed
+      spindle.sync_speed.dist = -1; // Mark done
+      spindle.speed = spindle.sync_speed.speed;
+      changed = true;
+    }
+
+    if (spindle.type == SPINDLE_TYPE_PWM) updates[i] = _get_power_update();
+    else {
+      updates[i].state = POWER_IGNORE;
+      if (changed) spindle_update_speed();
+    }
+  }
+}
+
+
+// Called from hi-priority stepper interrupt
+void spindle_update(const power_update_t &update) {pwm_update(update);}
+void spindle_update_speed() {_set_speed(spindle.speed);}
+
+
+// Called from lo-priority stepper interrupt
+void spindle_idle() {
+  if (spindle.sync_speed.dist != -1) {
+    spindle.sync_speed.dist = -1; // Mark done
+    spindle.speed = spindle.sync_speed.speed;
+
+    if (spindle.type == SPINDLE_TYPE_PWM) spindle_update(_get_power_update());
+    else spindle_update_speed();
+  }
+}
+
+
+void spindle_stop() {_set_speed(0);} // Only called when steppers have halted
+void spindle_estop() {_set_type(SPINDLE_TYPE_DISABLED);}
+
+
+// Var callbacks
+uint8_t get_tool_type() {return spindle.type;}
+void set_tool_type(uint8_t value) {_set_type((spindle_type_t)value);}
+bool get_tool_reversed() {return spindle.reversed;}
+
+
+void set_tool_reversed(bool reversed) {
+  if (spindle.reversed == reversed) return;
+  spindle.reversed = reversed;
+  spindle_update_speed();
+}
+
+
+float get_speed() {return _get_power() * spindle.max_rpm;}
+float get_max_spin() {return spindle.max_rpm;}
+
+
+void set_max_spin(float value) {
+  if (spindle.max_rpm != value) {
+    spindle.max_rpm = value;
+    spindle.inv_max_rpm = 1 / value;
+    spindle_update_speed();
+  }
+}
+
+
+float get_min_spin() {return spindle.min_rpm;}
+
+
+void set_min_spin(float value) {
+  if (spindle.min_rpm != value) {
+    spindle.min_rpm = value;
+    spindle_update_speed();
+  }
+}
+
+
+uint16_t get_spindle_status() {
+  switch (spindle.type) {
+  case SPINDLE_TYPE_DISABLED: return 0;
+  case SPINDLE_TYPE_PWM:      return 0;
+  case SPINDLE_TYPE_HUANYANG: return huanyang_get_status();
+  default:                    return vfd_get_status();
+  }
+}
+
+
+uint16_t get_speed_override() {return spindle.override * 1000;}
+
+
+void set_speed_override(uint16_t value) {
+  value *= 0.001;
+
+  if (spindle.override != value) {
+    spindle.override = value;
+    spindle_update_speed();
+  }
+}
+
+
+bool get_dynamic_power() {return spindle.dynamic_power;}
+
+
+void set_dynamic_power(bool enable) {
+  if (spindle.dynamic_power != enable) {
+    spindle.dynamic_power = enable;
+    spindle_update_speed();
+  }
+}
+
+
+float get_inverse_feed() {return spindle.inv_feed;}
+
+
+void set_inverse_feed(float iF) {
+  if (spindle.inv_feed != iF) {
+    spindle.inv_feed = iF;
+    spindle_update_speed();
+  }
+}
+
+
+// Command callbacks
+stat_t command_sync_speed(char *cmd) {
+  sync_speed_t s;
+
+  cmd++; // Skip command code
+
+  // Get distance and speed
+  if (!decode_float(&cmd, &s.dist) || s.dist < 0) return STAT_BAD_FLOAT;
+  if (!decode_float(&cmd, &s.speed))              return STAT_BAD_FLOAT;
+
+  // Queue
+  command_push(COMMAND_sync_speed, &s);
+
+  return STAT_OK;
+}
+
+
+unsigned command_sync_speed_size() {return sizeof(sync_speed_t);}
+
+
+void command_sync_speed_exec(void *data) {
+  _set_speed(((sync_speed_t *)data)->speed);
+}
+
+
+stat_t command_speed(char *cmd) {
+  cmd++; // Skip command code
+
+  // Get speed
+  float speed;
+  if (!decode_float(&cmd, &speed)) return STAT_BAD_FLOAT;
+
+  // Queue
+  command_push(COMMAND_speed, &speed);
+
+  return STAT_OK;
+}
+
+
+unsigned command_speed_size() {return sizeof(float);}
+void command_speed_exec(void *data) {_set_speed(*(float *)data);}

src/avr/src/spindle.h

@@ -0,0 +1,74 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+
+typedef enum {
+  POWER_IGNORE,
+  POWER_FORWARD,
+  POWER_REVERSE
+} power_state_t;
+
+
+typedef struct {
+  power_state_t state;
+  float power;
+  uint16_t period; // Used by PWM
+} power_update_t;
+
+
+typedef enum {
+  SPINDLE_TYPE_DISABLED,
+  SPINDLE_TYPE_PWM,
+  SPINDLE_TYPE_HUANYANG,
+  SPINDLE_TYPE_CUSTOM,
+  SPINDLE_TYPE_AC_TECH,
+  SPINDLE_TYPE_NOWFOREVER,
+  SPINDLE_TYPE_DELTA_VFD015M21A,
+  SPINDLE_TYPE_YL600,
+  SPINDLE_TYPE_FR_D700,
+  SPINDLE_TYPE_SUNFAR_E300,
+  SPINDLE_TYPE_OMRON_MX2,
+  SPINDLE_TYPE_V70,
+} spindle_type_t;
+
+
+typedef void (*deinit_cb_t)();
+
+
+spindle_type_t spindle_get_type();
+void spindle_stop();
+void spindle_estop();
+void spindle_load_power_updates(power_update_t updates[], float minD,
+                                float maxD);
+void spindle_update(const power_update_t &update);
+void spindle_update_speed();
+void spindle_idle();

src/avr/src/state.c

@@ -0,0 +1,259 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "state.h"
+
+#include "exec.h"
+#include "command.h"
+#include "stepper.h"
+#include "spindle.h"
+#include "outputs.h"
+#include "jog.h"
+#include "estop.h"
+#include "seek.h"
+
+#include <stdio.h>
+
+
+static struct {
+  bool flushing;
+  bool resuming;
+  bool stop_requested;
+  bool pause_requested;
+  bool unpause_requested;
+
+  state_t state;
+  uint16_t state_count;
+  hold_reason_t hold_reason;
+} s = {
+  .flushing = true, // Start out flushing
+};
+
+
+PGM_P state_get_pgmstr(state_t state) {
+  switch (state) {
+  case STATE_READY:     return PSTR("READY");
+  case STATE_ESTOPPED:  return PSTR("ESTOPPED");
+  case STATE_RUNNING:   return PSTR("RUNNING");
+  case STATE_JOGGING:   return PSTR("JOGGING");
+  case STATE_STOPPING:  return PSTR("STOPPING");
+  case STATE_HOLDING:   return PSTR("HOLDING");
+  }
+
+  return PSTR("INVALID");
+}
+
+
+PGM_P state_get_hold_reason_pgmstr(hold_reason_t reason) {
+  switch (reason) {
+  case HOLD_REASON_USER_PAUSE:     return PSTR("User pause");
+  case HOLD_REASON_USER_STOP:      return PSTR("User stop");
+  case HOLD_REASON_PROGRAM_PAUSE:  return PSTR("Program pause");
+  case HOLD_REASON_OPTIONAL_PAUSE: return PSTR("Optional pause");
+  case HOLD_REASON_SWITCH_FOUND:   return PSTR("Switch found");
+  }
+
+  return PSTR("INVALID");
+}
+
+
+state_t state_get() {return s.state;}
+
+
+static void _set_state(state_t state) {
+  if (s.state == state) return; // No change
+  if (s.state == STATE_ESTOPPED) return; // Can't leave EStop state
+  s.state = state;
+  s.state_count++;
+}
+
+
+static void _set_hold_reason(hold_reason_t reason) {s.hold_reason = reason;}
+bool state_is_flushing() {return s.flushing && !s.resuming;}
+bool state_is_resuming() {return s.resuming;}
+
+
+static bool _is_idle() {
+  return (state_get() == STATE_READY || state_get() == STATE_HOLDING) &&
+    !st_is_busy();
+}
+
+
+void state_seek_hold() {
+  if (state_get() == STATE_RUNNING) {
+    _set_hold_reason(HOLD_REASON_SWITCH_FOUND);
+    _set_state(STATE_STOPPING);
+  }
+}
+
+
+static void _stop() {
+  _set_hold_reason(HOLD_REASON_USER_STOP);
+
+  switch (state_get()) {
+  case STATE_RUNNING:
+    _set_state(STATE_STOPPING);
+    break;
+
+  case STATE_JOGGING:
+  case STATE_READY:
+  case STATE_HOLDING:
+    jog_stop();
+    spindle_stop();
+    outputs_stop();
+    seek_cancel();
+    break;
+
+  case STATE_STOPPING:
+  case STATE_ESTOPPED:
+    break; // Ignore
+  }
+}
+
+
+void state_holding() {
+  _set_state(STATE_HOLDING);
+  if (s.hold_reason == HOLD_REASON_USER_STOP) _stop();
+}
+
+
+void state_running() {
+  if (state_get() == STATE_READY) _set_state(STATE_RUNNING);
+}
+
+
+void state_jogging() {
+  if (state_get() == STATE_READY || state_get() == STATE_HOLDING)
+    _set_state(STATE_JOGGING);
+}
+
+
+void state_idle() {
+  if (state_get() == STATE_RUNNING || state_get() == STATE_JOGGING)
+    _set_state(STATE_READY);
+}
+
+
+void state_estop() {_set_state(STATE_ESTOPPED);}
+
+
+void state_callback() {
+  if (estop_triggered()) return;
+
+  // Pause
+  if (s.pause_requested) {
+    if (state_get() == STATE_RUNNING) {
+      if (s.pause_requested) _set_hold_reason(HOLD_REASON_USER_PAUSE);
+      _set_state(STATE_STOPPING);
+    }
+
+    s.pause_requested = false;
+  }
+
+  // Stop
+  if (s.stop_requested) {
+    _stop();
+    s.stop_requested = false;
+  }
+
+  // Only flush queue when idle (READY or HOLDING)
+  if (s.flushing && _is_idle()) {
+    command_flush_queue();
+
+    // Resume
+    if (s.resuming) s.flushing = s.resuming = false;
+  }
+
+  // Unpause
+  if (s.unpause_requested && !s.flushing && state_get() != STATE_STOPPING) {
+    s.unpause_requested = false;
+
+    if (state_get() == STATE_HOLDING) {
+      // Check if any moves are buffered
+      if (command_get_count()) _set_state(STATE_RUNNING);
+      else _set_state(STATE_READY);
+    }
+  }
+}
+
+
+// Var callbacks
+PGM_P get_state() {return state_get_pgmstr(state_get());}
+uint16_t get_state_count() {return s.state_count;}
+PGM_P get_hold_reason() {return state_get_hold_reason_pgmstr(s.hold_reason);}
+
+
+// Command callbacks
+stat_t command_pause(char *cmd) {
+  pause_t type = (pause_t)(cmd[1] - '0');
+
+  if (type == PAUSE_USER) s.pause_requested = true;
+  else command_push(cmd[0], &type);
+
+  return STAT_OK;
+}
+
+
+unsigned command_pause_size() {return sizeof(pause_t);}
+
+
+void command_pause_exec(void *data) {
+  switch (*(pause_t *)data) {
+  case PAUSE_PROGRAM_OPTIONAL:
+    _set_hold_reason(HOLD_REASON_OPTIONAL_PAUSE);
+    break;
+
+  case PAUSE_PROGRAM: _set_hold_reason(HOLD_REASON_PROGRAM_PAUSE); break;
+  default: return;
+  }
+
+  state_holding();
+}
+
+
+stat_t command_stop(char *cmd) {
+  s.stop_requested = true;
+  return STAT_OK;
+}
+
+
+stat_t command_unpause(char *cmd) {
+  s.unpause_requested = true;
+  return STAT_OK;
+}
+
+
+stat_t command_resume(char *cmd) {
+  if (s.flushing) s.resuming = true;
+  return STAT_OK;
+}
+
+
+stat_t command_flush(char *cmd) {
+  s.flushing = true;
+  return STAT_OK;
+}

src/avr/src/state.h

@@ -0,0 +1,76 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "pgmspace.h"
+
+#include <stdbool.h>
+
+
+typedef enum {
+  STATE_READY,
+  STATE_ESTOPPED,
+  STATE_RUNNING,
+  STATE_JOGGING,
+  STATE_STOPPING,
+  STATE_HOLDING,
+} state_t;
+
+
+typedef enum {
+  HOLD_REASON_USER_PAUSE,
+  HOLD_REASON_USER_STOP,
+  HOLD_REASON_PROGRAM_PAUSE,
+  HOLD_REASON_OPTIONAL_PAUSE,
+  HOLD_REASON_SWITCH_FOUND,
+} hold_reason_t;
+
+
+typedef enum {
+  PAUSE_USER,
+  PAUSE_PROGRAM,
+  PAUSE_PROGRAM_OPTIONAL,
+} pause_t;
+
+
+PGM_P state_get_pgmstr(state_t state);
+PGM_P state_get_hold_reason_pgmstr(hold_reason_t reason);
+
+state_t state_get();
+
+bool state_is_flushing();
+bool state_is_resuming();
+
+void state_seek_hold();
+void state_holding();
+void state_running();
+void state_jogging();
+void state_idle();
+void state_estop();
+
+void state_callback();

src/avr/src/status.c

@@ -0,0 +1,96 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "status.h"
+#include "estop.h"
+#include "usart.h"
+
+#include <stdio.h>
+#include <stdarg.h>
+
+
+#define STAT_MSG(NAME, TEXT) static const char stat_##NAME[] PROGMEM = TEXT;
+#include "messages.def"
+#undef STAT_MSG
+
+
+static const char *const stat_msg[] PROGMEM = {
+#define STAT_MSG(NAME, TEXT) stat_##NAME,
+#include "messages.def"
+#undef STAT_MSG
+};
+
+
+const char *status_to_pgmstr(stat_t code) {
+  return (const char *)pgm_read_ptr(&stat_msg[code]);
+}
+
+
+const char *status_level_pgmstr(status_level_t level) {
+  switch (level) {
+  case STAT_LEVEL_INFO: return PSTR("info");
+  case STAT_LEVEL_DEBUG: return PSTR("debug");
+  case STAT_LEVEL_WARNING: return PSTR("warning");
+  default: return PSTR("error");
+  }
+}
+
+
+stat_t status_message_P(const char *location, status_level_t level,
+                        stat_t code, const char *msg, ...) {
+  va_list args;
+
+  // Type
+  printf_P(PSTR("\n{\"level\":\"%" PRPSTR "\",\"msg\":\""),
+           status_level_pgmstr(level));
+
+  // Message
+  printf_P(PSTR("%" PRPSTR), status_to_pgmstr(code));
+
+  if (msg && pgm_read_byte(msg)) {
+    putchar(':');
+    putchar(' ');
+
+    // TODO escape invalid chars
+    va_start(args, msg);
+    vfprintf_P(stdout, msg, args);
+    va_end(args);
+  }
+
+  putchar('"');
+
+  // Code
+  if (code) printf_P(PSTR(",\"code\":%d"), code);
+
+  // Location
+  if (location) printf_P(PSTR(",\"where\":\"%" PRPSTR "\""), location);
+
+  putchar('}');
+  putchar('\n');
+
+  return code;
+}

src/avr/src/status.h

@@ -0,0 +1,84 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "pgmspace.h"
+
+
+typedef enum {
+#define STAT_MSG(NAME, TEXT) STAT_##NAME,
+#include "messages.def"
+#undef STAT_MSG
+
+  STAT_MAX,
+  STAT_DO_NOT_EXCEED = 255 // Do not exceed 255
+} stat_t;
+
+
+typedef enum {
+  STAT_LEVEL_INFO,
+  STAT_LEVEL_DEBUG,
+  STAT_LEVEL_WARNING,
+  STAT_LEVEL_ERROR,
+} status_level_t;
+
+
+extern stat_t status_code;
+
+const char *status_to_pgmstr(stat_t code);
+const char *status_level_pgmstr(status_level_t level);
+stat_t status_message_P(const char *location, status_level_t level,
+                        stat_t code, const char *msg, ...);
+
+#define TO_STRING(x) _TO_STRING(x)
+#define _TO_STRING(x) #x
+
+#define STATUS_LOCATION PSTR(__FILE__ ":" TO_STRING(__LINE__))
+
+#define STATUS_MESSAGE(LEVEL, CODE, MSG, ...)                           \
+  status_message_P(STATUS_LOCATION, LEVEL, CODE, PSTR(MSG), ##__VA_ARGS__)
+
+#define STATUS_INFO(MSG, ...)                                   \
+  STATUS_MESSAGE(STAT_LEVEL_INFO, STAT_OK, MSG, ##__VA_ARGS__)
+
+#define STATUS_DEBUG(MSG, ...)                                  \
+  STATUS_MESSAGE(STAT_LEVEL_DEBUG, STAT_OK, MSG, ##__VA_ARGS__)
+
+#define STATUS_WARNING(CODE, MSG, ...)                          \
+  STATUS_MESSAGE(STAT_LEVEL_WARNING, CODE, MSG, ##__VA_ARGS__)
+
+#define STATUS_ERROR(CODE, MSG, ...)                            \
+  STATUS_MESSAGE(STAT_LEVEL_ERROR, CODE, MSG, ##__VA_ARGS__)
+
+
+#ifdef DEBUG
+#define DEBUG_CALL(FMT, ...) \
+  printf_P(PSTR("%s(" FMT ")\n"), __FUNCTION__, ##__VA_ARGS__)
+#else // DEBUG
+#define DEBUG_CALL(...)
+#endif // DEBUG

src/avr/src/stepper.c

@@ -0,0 +1,247 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "stepper.h"
+
+#include "config.h"
+#include "motor.h"
+#include "hardware.h"
+#include "estop.h"
+#include "util.h"
+#include "cpp_magic.h"
+#include "exec.h"
+#include "drv8711.h"
+
+#include <util/atomic.h>
+
+#include <string.h>
+#include <stdio.h>
+
+
+typedef struct {
+  // Runtime
+  bool busy;
+  bool requesting;
+  float dwell;
+  uint8_t power_buf;
+  uint8_t power_index;
+
+  // Move prep
+  bool move_ready;  // Prepped move ready for loader
+  bool move_queued; // Prepped move queued
+  float prep_dwell;
+  int8_t power_next;
+
+  power_update_t powers[2][POWER_MAX_UPDATES];
+
+  uint32_t underrun;
+} stepper_t;
+
+
+static stepper_t st = {0};
+
+
+void stepper_init() {
+  // Setup step timer
+  TIMER_STEP.CTRLB    = TC_WGMODE_NORMAL_gc; // Count to TOP & rollover
+  TIMER_STEP.INTCTRLA = TC_OVFINTLVL_HI_gc;  // Interrupt level
+  TIMER_STEP.PER      = STEP_TIMER_POLL;     // Timer rate
+  TIMER_STEP.CTRLA    = TC_CLKSEL_DIV8_gc;   // Start step timer
+}
+
+
+static void _end_move() {
+  for (int motor = 0; motor < MOTORS; motor++)
+    motor_end_move(motor);
+}
+
+
+static void _load_move() {
+  for (int motor = 0; motor < MOTORS; motor++)
+    motor_load_move(motor);
+}
+
+
+void st_shutdown() {
+  TIMER_STEP.CTRLA = 0;         // Stop stepper clock
+  _end_move();                  // Stop motor clocks
+  ADCB_CH0_INTCTRL = 0;         // Disable next move interrupt
+}
+
+
+/// Return true if motors or dwell are running
+bool st_is_busy() {return st.busy;}
+
+
+/// Interrupt handler for calling move exec function.
+/// ADC channel 0 triggered by load ISR as a "software" interrupt.
+ISR(STEP_LOW_LEVEL_ISR) {
+  while (true) {
+    stat_t status = exec_next();
+
+    switch (status) {
+    case STAT_NOP:                          // No move executed, idle
+      if (!st.busy) {
+        if (MIN_VELOCITY < exec_get_velocity()) st.underrun++;
+        exec_set_velocity(0); // Velocity is zero if there are no moves
+
+        spindle_idle();
+      }
+      break;
+
+    case STAT_AGAIN: continue;              // No command executed, try again
+
+    case STAT_OK:                           // Move executed
+      if (!st.move_queued)
+        estop_trigger(STAT_EXPECTED_MOVE);  // No move was queued
+      st.move_queued = false;
+      st.move_ready = true;
+      break;
+
+    default: estop_trigger(status); break;
+    }
+
+    break;
+  }
+
+  ADCB_CH0_INTCTRL = 0;
+  st.requesting = false;
+}
+
+
+static void _request_exec_move() {
+  if (st.requesting) return;
+  st.requesting = true;
+
+  // Use ADC as "software" interrupt to trigger next move exec as low interrupt.
+  ADCB_CH0_INTCTRL = ADC_CH_INTLVL_LO_gc;
+  ADCB_CTRLA = ADC_ENABLE_bm | ADC_CH0START_bm;
+}
+
+
+static void _update_power() {
+  if (st.power_index < POWER_MAX_UPDATES)
+    spindle_update(st.powers[st.power_buf][st.power_index++]);
+}
+
+
+/// Step timer interrupt routine.
+/// Dwell or dequeue and load next move.
+ISR(STEP_TIMER_ISR) {
+  static uint8_t tick = 0;
+
+  // Update spindle power on every tick
+  _update_power();
+
+  // Dwell
+  if (0 < st.dwell) {
+    st.dwell -= 0.001; // 1ms
+    return;
+  }
+  st.dwell = 0;
+
+  if (tick++ & 3) return; // Proceed every 4 ticks
+
+  // If the next move is not ready try to load it
+  if (!st.move_ready) {
+    _request_exec_move();
+    _end_move();
+    tick = 0; // Try again in 1ms
+    st.busy = false;
+    return;
+  }
+
+  if (st.prep_dwell) {
+    // End last move, if any
+    _end_move();
+
+    // Start dwell
+    st.dwell = st.prep_dwell;
+    st.prep_dwell = 0;
+
+  } else {
+    // Start move
+    _load_move();
+
+    // Request next move when not in a dwell.  Requesting the next move may
+    // power up motors which should not be powered up during a dwell.
+    _request_exec_move();
+  }
+
+  // Handle power updates
+  if (st.power_next != -1) {
+    st.power_index = 0;
+    st.power_buf = st.power_next;
+    st.power_next = -1;
+    _update_power();
+  }
+
+  st.busy = true;        // Executing move so mark busy
+  st.move_ready = false; // We are done with this move, flip the flag back
+}
+
+
+void st_prep_power(const power_update_t powers[]) {
+  ESTOP_ASSERT(!st.move_ready, STAT_STEPPER_NOT_READY);
+  st.power_next = !st.power_buf;
+  memcpy(st.powers[st.power_next], powers,
+         sizeof(power_update_t) * POWER_MAX_UPDATES);
+}
+
+
+void st_prep_line(const float target[]) {
+  // Trap conditions that would prevent queuing the line
+  ESTOP_ASSERT(!st.move_ready, STAT_STEPPER_NOT_READY);
+
+  // Prepare motor moves
+  for (int motor = 0; motor < MOTORS; motor++)
+    motor_prep_move(motor, target[motor_get_axis(motor)]);
+
+  st.move_queued = true; // signal prep buffer ready (do this last)
+}
+
+
+/// Add a dwell to the move buffer
+void st_prep_dwell(float seconds) {
+  ESTOP_ASSERT(!st.move_ready, STAT_STEPPER_NOT_READY);
+  if (seconds <= 1e-4) seconds = 1e-4; // Min dwell
+  st.power_next = !st.power_buf;
+  spindle_load_power_updates(st.powers[st.power_next], 0, 0);
+  st.prep_dwell = seconds;
+  st.move_queued = true; // signal prep buffer ready
+}
+
+
+// Var callbacks
+uint32_t get_underrun() {return st.underrun;}
+
+
+float get_dwell_time() {
+  float dwell;
+  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) dwell = st.dwell;
+  return dwell;
+}

src/avr/src/stepper.h

@@ -0,0 +1,42 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "spindle.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+
+void stepper_init();
+void st_shutdown();
+bool st_is_busy();
+void st_set_power_scale(float scale);
+void st_prep_power(const power_update_t powers[]);
+void st_prep_line(const float target[]);
+void st_prep_dwell(float seconds);

src/avr/src/switch.c

@@ -0,0 +1,209 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "switch.h"
+#include "config.h"
+
+#include <stdbool.h>
+#include <stdio.h>
+
+
+static struct {
+  uint16_t debounce;
+  uint16_t lockout;
+} sw = {
+  .debounce = SWITCH_DEBOUNCE,
+  .lockout = SWITCH_LOCKOUT,
+};
+
+typedef struct {
+  uint8_t pin;
+  switch_type_t type;
+
+  switch_callback_t cb;
+  bool state;
+  uint16_t debounce;
+  uint16_t lockout;
+  bool initialized;
+} switch_t;
+
+
+// Order must match indices in var functions below
+static switch_t switches[] = {
+  {.pin = ESTOP_PIN,       .type = SW_DISABLED},
+  {.pin = PROBE_PIN,       .type = SW_DISABLED},
+  {.pin = MIN_0_PIN,       .type = SW_DISABLED},
+  {.pin = MAX_0_PIN,       .type = SW_DISABLED},
+  {.pin = MIN_1_PIN,       .type = SW_DISABLED},
+  {.pin = MAX_1_PIN,       .type = SW_DISABLED},
+  {.pin = MIN_2_PIN,       .type = SW_DISABLED},
+  {.pin = MAX_2_PIN,       .type = SW_DISABLED},
+  {.pin = MIN_3_PIN,       .type = SW_DISABLED},
+  {.pin = MAX_3_PIN,       .type = SW_DISABLED},
+  {.pin = STALL_0_PIN,     .type = SW_DISABLED},
+  {.pin = STALL_1_PIN,     .type = SW_DISABLED},
+  {.pin = STALL_2_PIN,     .type = SW_DISABLED},
+  {.pin = STALL_3_PIN,     .type = SW_DISABLED},
+  {.pin = MOTOR_FAULT_PIN, .type = SW_DISABLED},
+};
+
+
+static const int num_switches = sizeof(switches) / sizeof (switch_t);
+
+
+void switch_init() {
+  for (int i = 0; i < num_switches; i++) {
+    switch_t *s = &switches[i];
+    PINCTRL_PIN(s->pin) = PORT_OPC_PULLUP_gc; // Pull up
+    DIRCLR_PIN(s->pin); // Input
+  }
+}
+
+
+/// Called from RTC on each tick
+void switch_rtc_callback() {
+  for (int i = 0; i < num_switches; i++) {
+    switch_t *s = &switches[i];
+
+    if (s->type == SW_DISABLED) continue;
+    if (s->lockout && --s->lockout) continue;
+
+    // Debounce switch
+    bool state = IN_PIN(s->pin);
+    if (state == s->state && s->initialized) s->debounce = 0;
+    else if (++s->debounce == sw.debounce) {
+      s->state = state;
+      s->debounce = 0;
+      s->initialized = true;
+      s->lockout = sw.lockout;
+      if (s->cb) s->cb((switch_id_t)i, switch_is_active((switch_id_t)i));
+    }
+  }
+}
+
+
+bool switch_is_active(switch_id_t sw) {
+  if (sw < 0 || num_switches <= sw || !switches[sw].initialized) return false;
+
+  // NOTE, switch inputs are active lo
+  switch (switches[sw].type) {
+  case SW_DISABLED:        break; // A disabled switch cannot be active
+  case SW_NORMALLY_OPEN:   return !switches[sw].state;
+  case SW_NORMALLY_CLOSED: return switches[sw].state;
+  }
+
+  return false;
+}
+
+
+bool switch_is_enabled(switch_id_t sw) {
+  return switch_get_type(sw) != SW_DISABLED;
+}
+
+
+switch_type_t switch_get_type(switch_id_t sw) {
+  return (sw < 0 || num_switches <= sw) ? SW_DISABLED : switches[sw].type;
+}
+
+
+void switch_set_type(switch_id_t sw, switch_type_t type) {
+  if (sw < 0 || num_switches <= sw) return;
+  switch_t *s = &switches[sw];
+
+  if (s->type != type) {
+    bool wasActive = switch_is_active(sw);
+    s->type = type;
+    bool isActive = switch_is_active(sw);
+    if (wasActive != isActive && s->cb) s->cb(sw, isActive);
+  }
+}
+
+
+void switch_set_callback(switch_id_t sw, switch_callback_t cb) {
+  switches[sw].cb = cb;
+}
+
+
+// Var callbacks
+uint8_t get_min_sw_mode(int index) {return switch_get_type(MIN_SWITCH(index));}
+
+
+void set_min_sw_mode(int index, uint8_t value) {
+  switch_set_type(MIN_SWITCH(index), (switch_type_t)value);
+}
+
+
+uint8_t get_max_sw_mode(int index) {return switch_get_type(MAX_SWITCH(index));}
+
+
+void set_max_sw_mode(int index, uint8_t value) {
+  switch_set_type(MAX_SWITCH(index), (switch_type_t)value);
+}
+
+
+uint8_t get_estop_mode() {return switch_get_type(SW_ESTOP);}
+
+
+void set_estop_mode(uint8_t value) {
+  switch_set_type(SW_ESTOP, (switch_type_t)value);
+}
+
+
+uint8_t get_probe_mode() {return switch_get_type(SW_PROBE);}
+
+
+void set_probe_mode(uint8_t value) {
+  switch_set_type(SW_PROBE, (switch_type_t)value);
+}
+
+
+static uint8_t _get_state(int index) {
+  if (!switch_is_enabled((switch_id_t)index)) return 2; // Disabled
+  return switches[index].state;
+}
+
+
+uint8_t get_min_switch(int index) {return _get_state(MIN_SWITCH(index));}
+uint8_t get_max_switch(int index) {return _get_state(MAX_SWITCH(index));}
+uint8_t get_estop_switch() {return _get_state(SW_ESTOP);}
+uint8_t get_probe_switch() {return _get_state(SW_PROBE);}
+
+
+void set_switch_debounce(uint16_t debounce) {
+  sw.debounce = SWITCH_MAX_DEBOUNCE < debounce ? SWITCH_DEBOUNCE : debounce;
+}
+
+
+uint16_t get_switch_debounce() {return sw.debounce;}
+
+
+void set_switch_lockout(uint16_t lockout) {
+  sw.lockout = SWITCH_MAX_LOCKOUT < lockout ? SWITCH_LOCKOUT : lockout;
+}
+
+
+uint16_t get_switch_lockout() {return sw.lockout;}

src/avr/src/switch.h

@@ -0,0 +1,72 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include "config.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+
+
+// macros for finding the index into the switch table give the axis number
+#define MIN_SWITCH(axis) ((switch_id_t)(2 + axis * 2))
+#define MAX_SWITCH(axis) ((switch_id_t)(2 + axis * 2 + 1))
+
+
+typedef enum {
+  SW_DISABLED,
+  SW_NORMALLY_OPEN,
+  SW_NORMALLY_CLOSED,
+} switch_type_t;
+
+
+/// Switch IDs
+typedef enum {
+  SW_INVALID = -1,
+  SW_ESTOP, SW_PROBE,
+  SW_MIN_0, SW_MAX_0,
+  SW_MIN_1, SW_MAX_1,
+  SW_MIN_2, SW_MAX_2,
+  SW_MIN_3, SW_MAX_3,
+  SW_STALL_0, SW_STALL_1,
+  SW_STALL_2, SW_STALL_3,
+  SW_MOTOR_FAULT,
+} switch_id_t;
+
+
+typedef void (*switch_callback_t)(switch_id_t sw, bool active);
+
+
+void switch_init();
+void switch_rtc_callback();
+bool switch_is_active(switch_id_t sw);
+bool switch_is_enabled(switch_id_t sw);
+switch_type_t switch_get_type(switch_id_t sw);
+void switch_set_type(switch_id_t sw, switch_type_t type);
+void switch_set_callback(switch_id_t sw, switch_callback_t cb);

src/avr/src/type.c

@@ -0,0 +1,216 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "type.h"
+#include "base64.h"
+
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+#include <ctype.h>
+#include <stdlib.h>
+#include <inttypes.h>
+
+
+#define TYPEDEF(TYPE, DEF)                                              \
+  static const char TYPE##_name [] PROGMEM = "<" #TYPE ">";             \
+  pstr type_get_##TYPE##_name_pgm() {return TYPE##_name;}
+
+#include "type.def"
+#undef TYPEDEF
+
+
+// String
+bool type_eq_str(str a, str b) {return a == b;}
+void type_print_str(str s) {printf_P(PSTR("\"%s\""), s);}
+str type_parse_str(const char *s, stat_t *) {return s;}
+
+// Program string
+bool type_eq_pstr(pstr a, pstr b) {return a == b;}
+void type_print_pstr(pstr s) {printf_P(PSTR("\"%" PRPSTR "\""), s);}
+const char *type_parse_pstr(const char *value, stat_t *) {return value;}
+
+
+// Float
+bool type_eq_f32(float a, float b) {return a == b || (isnan(a) && isnan(b));}
+
+
+void type_print_f32(float x) {
+  if (isnan(x)) printf_P(PSTR("\"nan\""));
+  else if (isinf(x)) printf_P(PSTR("\"%cinf\""), x < 0 ? '-' : '+');
+
+  else {
+    char buf[20];
+
+    int len = sprintf_P(buf, PSTR("%.3f"), x);
+
+    // Remove trailing zeros
+    for (int i = len; 0 < i; i--) {
+      if (buf[i - 1] == '.') buf[i - 1] = 0;
+      else if (buf[i - 1] == '0') {
+        buf[i - 1] = 0;
+        continue;
+      }
+
+      break;
+    }
+
+    printf(buf);
+  }
+}
+
+
+float type_parse_f32(const char *value, stat_t *status) {
+  while (*value && isspace(*value)) value++;
+
+  if (*value == ':') {
+    value++;
+    if (strnlen(value, 6) != 6) {
+      if (status) *status = STAT_INVALID_VALUE;
+      return NAN;
+    }
+
+    float f;
+    if (b64_decode_float(value, &f)) return f;
+    else {
+      if (status) *status = STAT_BAD_FLOAT;
+      return NAN;
+    }
+  }
+
+  char *endptr = 0;
+  float x = strtod(value, &endptr);
+
+  if (endptr == value) {
+    if (status) *status = STAT_BAD_FLOAT;
+    return NAN;
+  }
+
+  return x;
+}
+
+
+// bool
+bool type_eq_b8(bool a, bool b) {return a == b;}
+void type_print_b8(bool x) {printf_P(x ? PSTR("true") : PSTR("false"));}
+
+
+bool type_parse_b8(const char *value, stat_t *status) {
+  return !strcasecmp(value, "true") || type_parse_f32(value, status);
+}
+
+
+// s8
+bool type_eq_s8(s8 a, s8 b) {return a == b;}
+void type_print_s8(s8 x) {printf_P(PSTR("%" PRIi8), x);}
+
+
+s8 type_parse_s8(const char *value, stat_t *status) {
+  char *endptr = 0;
+  s8 x = strtol(value, &endptr, 0);
+  if (endptr == value && status) *status = STAT_BAD_INT;
+  return x;
+}
+
+
+// u8
+bool type_eq_u8(u8 a, u8 b) {return a == b;}
+void type_print_u8(u8 x) {printf_P(PSTR("%" PRIu8), x);}
+
+
+u8 type_parse_u8(const char *value, stat_t *status) {
+  char *endptr = 0;
+  u8 x = strtoul(value, &endptr, 0);
+  if (endptr == value && status) *status = STAT_BAD_INT;
+  return x;
+}
+
+
+// u16
+bool type_eq_u16(u16 a, u16 b) {return a == b;}
+void type_print_u16(u16 x) {printf_P(PSTR("%" PRIu16), x);}
+
+
+u16 type_parse_u16(const char *value, stat_t *status) {
+  char *endptr = 0;
+  u16 x = strtoul(value, &endptr, 0);
+  if (endptr == value && status) *status = STAT_BAD_INT;
+  return x;
+}
+
+
+// s32
+bool type_eq_s32(s32 a, s32 b) {return a == b;}
+void type_print_s32(s32 x) {printf_P(PSTR("%" PRIi32), x);}
+
+
+s32 type_parse_s32(const char *value, stat_t *status) {
+  char *endptr = 0;
+  s32 x = strtol(value, &endptr, 0);
+  if (endptr == value && status) *status = STAT_BAD_INT;
+  return x;
+}
+
+
+// u32
+bool type_eq_u32(u32 a, u32 b) {return a == b;}
+void type_print_u32(u32 x) {printf_P(PSTR("%" PRIu32), x);}
+
+
+u32 type_parse_u32(const char *value, stat_t *status) {
+  char *endptr = 0;
+  u32 x = strtoul(value, &endptr, 0);
+  if (endptr == value && status) *status = STAT_BAD_INT;
+  return x;
+}
+
+
+type_u type_parse(type_t type, const char *s, stat_t *status) {
+  type_u value;
+
+  if (status) *status = STAT_OK;
+
+  switch (type) {
+#define TYPEDEF(TYPE, ...)                                              \
+    case TYPE_##TYPE: value._##TYPE = type_parse_##TYPE(s, status); break;
+#include "type.def"
+#undef TYPEDEF
+    default: if (status) *status = STAT_INVALID_TYPE;
+  }
+
+  return value;
+}
+
+
+void type_print(type_t type, type_u value) {
+  switch (type) {
+#define TYPEDEF(TYPE, ...)                                      \
+    case TYPE_##TYPE: type_print_##TYPE(value._##TYPE); break;
+#include "type.def"
+#undef TYPEDEF
+  }
+}

src/avr/src/type.def

@@ -0,0 +1,37 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+//      TYPE   DEF
+TYPEDEF(str,   const char *)
+TYPEDEF(pstr,  PGM_P)
+TYPEDEF(f32,   float)
+TYPEDEF(u8,    uint8_t)
+TYPEDEF(s8,    int8_t)
+TYPEDEF(u16,   uint16_t)
+TYPEDEF(s32,   int32_t)
+TYPEDEF(u32,   uint32_t)
+TYPEDEF(b8,    bool)

src/avr/src/type.h

@@ -0,0 +1,67 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "pgmspace.h"
+#include "status.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+
+
+// Define types
+#define TYPEDEF(TYPE, DEF) typedef DEF TYPE;
+#include "type.def"
+#undef TYPEDEF
+
+typedef enum {
+#define TYPEDEF(TYPE, ...) TYPE_##TYPE,
+#include "type.def"
+#undef TYPEDEF
+} type_t;
+
+
+typedef union {
+#define TYPEDEF(TYPE, ...) TYPE _##TYPE;
+#include "type.def"
+#undef TYPEDEF
+} type_u;
+
+
+// Define functions
+#define TYPEDEF(TYPE, DEF)                                  \
+  pstr type_get_##TYPE##_name_pgm();                        \
+  bool type_eq_##TYPE(TYPE a, TYPE b);                      \
+  TYPE type_parse_##TYPE(const char *s, stat_t *status);    \
+  void type_print_##TYPE(TYPE x);
+#include "type.def"
+#undef TYPEDEF
+
+
+type_u type_parse(type_t type, const char *s, stat_t *status);
+void type_print(type_t type, type_u value);

src/avr/src/usart.c

@@ -0,0 +1,285 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "usart.h"
+#include "cpp_magic.h"
+#include "config.h"
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+
+#include <stdio.h>
+#include <stdbool.h>
+#include <string.h>
+
+
+// Ring buffers
+#define RING_BUF_INDEX_TYPE volatile uint16_t
+#define RING_BUF_NAME tx_buf
+#define RING_BUF_SIZE USART_TX_RING_BUF_SIZE
+#define RING_BUF_ATOMIC_COPY 1
+#include "ringbuf.def"
+
+#define RING_BUF_INDEX_TYPE volatile uint16_t
+#define RING_BUF_NAME rx_buf
+#define RING_BUF_SIZE USART_RX_RING_BUF_SIZE
+#define RING_BUF_ATOMIC_COPY 1
+#include "ringbuf.def"
+
+static bool _flush = false;
+
+
+static void _set_dre_interrupt(bool enable) {
+  if (enable) SERIAL_PORT.CTRLA |= USART_DREINTLVL_MED_gc;
+  else SERIAL_PORT.CTRLA &= ~USART_DREINTLVL_MED_gc;
+}
+
+
+static void _set_rxc_interrupt(bool enable) {
+  if (enable) {
+    if (SERIAL_CTS_THRESH <= rx_buf_space())
+      OUTCLR_PIN(SERIAL_CTS_PIN); // CTS Lo (enable)
+
+    SERIAL_PORT.CTRLA |= USART_RXCINTLVL_HI_gc;
+
+  } else SERIAL_PORT.CTRLA &= ~USART_RXCINTLVL_HI_gc;
+}
+
+
+// Data register empty interrupt vector
+ISR(SERIAL_DRE_vect) {
+  if (tx_buf_empty()) _set_dre_interrupt(false); // Disable interrupt
+
+  else {
+    SERIAL_PORT.DATA = tx_buf_peek();
+    tx_buf_pop();
+  }
+}
+
+
+// Data received interrupt vector
+ISR(SERIAL_RXC_vect) {
+  if (rx_buf_full()) _set_rxc_interrupt(false); // Disable interrupt
+  else rx_buf_push(SERIAL_PORT.DATA);
+
+  if (rx_buf_space() < SERIAL_CTS_THRESH)
+    OUTSET_PIN(SERIAL_CTS_PIN); // CTS Hi (disable)
+}
+
+
+#ifdef __AVR__
+static int _usart_putchar(char c, FILE *f) {
+  usart_putc(c);
+  return 0;
+}
+#endif // __AVR__
+
+
+static void _set_baud(USART_t *port, uint16_t bsel, uint8_t bscale) {
+  port->BAUDCTRLB = (uint8_t)((bscale << 4) | (bsel >> 8));
+  port->BAUDCTRLA = bsel;
+  port->CTRLB |= USART_CLK2X_bm;
+}
+
+
+void usart_set_baud(USART_t *port, baud_t baud) {
+  // The BSEL / BSCALE values provided below assume a 32 Mhz clock
+  // With CTRLB CLK2X is set
+  // See http://www.avrcalc.elektronik-projekt.de/xmega/baud_rate_calculator
+
+  switch (baud) {
+  case USART_BAUD_9600:    _set_baud(port, 3325, 0b1101); break;
+  case USART_BAUD_19200:   _set_baud(port, 3317, 0b1100); break;
+  case USART_BAUD_38400:   _set_baud(port, 3301, 0b1011); break;
+  case USART_BAUD_57600:   _set_baud(port, 1095, 0b1100); break;
+  case USART_BAUD_115200:  _set_baud(port, 1079, 0b1011); break;
+  case USART_BAUD_230400:  _set_baud(port, 1047, 0b1010); break;
+  case USART_BAUD_460800:  _set_baud(port, 983,  0b1001); break;
+  case USART_BAUD_921600:  _set_baud(port, 107,  0b1011); break;
+  case USART_BAUD_500000:  _set_baud(port, 1,    0b0010); break;
+  case USART_BAUD_1000000: _set_baud(port, 1,    0b0001); break;
+  }
+}
+
+
+void usart_set_parity(USART_t *port, parity_t parity) {
+  uint8_t reg = port->CTRLC & ~USART_PMODE_gm;
+
+  switch (parity) {
+  case USART_NONE: reg |= USART_PMODE_DISABLED_gc; break;
+  case USART_EVEN: reg |= USART_PMODE_EVEN_gc; break;
+  case USART_ODD:  reg |= USART_PMODE_ODD_gc; break;
+  }
+
+  port->CTRLC = reg;
+}
+
+
+void usart_set_stop(USART_t *port, stop_t stop) {
+  switch (stop) {
+  case USART_1STOP: port->CTRLC &= ~USART_SBMODE_bm; break;
+  case USART_2STOP: port->CTRLC |= USART_SBMODE_bm;  break;
+  }
+}
+
+
+void usart_set_bits(USART_t *port, bits_t bits) {
+  uint8_t reg = port->CTRLC & ~USART_CHSIZE_gm;
+
+  switch (bits) {
+  case USART_5BITS: reg |= USART_CHSIZE_5BIT_gc; break;
+  case USART_6BITS: reg |= USART_CHSIZE_6BIT_gc; break;
+  case USART_7BITS: reg |= USART_CHSIZE_7BIT_gc; break;
+  case USART_8BITS: reg |= USART_CHSIZE_8BIT_gc; break;
+  case USART_9BITS: reg |= USART_CHSIZE_9BIT_gc; break;
+  }
+
+  port->CTRLC = reg;
+}
+
+
+void usart_init_port(USART_t *port, baud_t baud, parity_t parity, bits_t bits,
+                     stop_t stop) {
+  // Set baud rate
+  usart_set_baud(port, baud);
+
+  // Async, no parity, 8 data bits, 1 stop bit
+  port->CTRLC = USART_CMODE_ASYNCHRONOUS_gc;
+  usart_set_parity(port, parity);
+  usart_set_bits(port, bits);
+  usart_set_stop(port, stop);
+
+  // Configure receiver and transmitter
+  port->CTRLB |= USART_RXEN_bm | USART_TXEN_bm;
+}
+
+
+void usart_init() {
+  // Setup ring buffer
+  tx_buf_init();
+  rx_buf_init();
+
+  PR.PRPC &= ~PR_USART0_bm; // Disable power reduction
+
+  // Setup pins
+  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
+
+  // Configure port
+  usart_init_port(&SERIAL_PORT, SERIAL_BAUD, USART_NONE, USART_8BITS,
+                  USART_1STOP);
+
+  PMIC.CTRL |= PMIC_HILVLEN_bm; // Interrupt level on
+
+#ifdef __AVR__
+  // Connect IO
+  static FILE _stdout;
+  memset(&_stdout, 0, sizeof(FILE));
+  _stdout.put = _usart_putchar;
+  _stdout.flags = _FDEV_SETUP_WRITE;
+
+  stdout = &_stdout;
+  stderr = &_stdout;
+#endif // __AVR__
+
+  // Enable Rx
+  _set_rxc_interrupt(true);
+}
+
+
+
+void usart_putc(char c) {
+  while (tx_buf_full() || _flush) continue;
+  tx_buf_push(c);
+  _set_dre_interrupt(true); // Enable interrupt
+}
+
+
+void usart_puts(const char *s) {while (*s) usart_putc(*s++);}
+
+
+int8_t usart_getc() {
+  while (rx_buf_empty()) continue;
+  uint8_t data = rx_buf_next();
+  _set_rxc_interrupt(true); // Enable interrupt
+  return data;
+}
+
+
+/*** Line editing features:
+ *
+ *   ENTER     Submit current command line.
+ *   BS        Backspace, delete last character.
+ *   CTRL-X    Cancel current line entry.
+ */
+char *usart_readline() {
+  static char line[INPUT_BUFFER_LEN];
+  static int i = 0;
+  bool eol = false;
+
+  while (!rx_buf_empty()) {
+    char data = usart_getc();
+
+    switch (data) {
+    case '\r': case '\n': eol = true; break;
+    case '\b': if (i) i--; break; // BS - backspace
+    case 0x18: i = 0; break;      // CAN - Cancel or CTRL-X
+
+    default:
+      line[i++] = data;
+      if (i == INPUT_BUFFER_LEN - 1) eol = true; // Line buffer full
+      break;
+    }
+
+    if (eol) {
+      line[i] = 0;
+      i = 0;
+      return line;
+    }
+  }
+
+  return 0;
+}
+
+
+void usart_flush() {
+  _flush = true;
+
+  while (!tx_buf_empty() || !(SERIAL_PORT.STATUS & USART_DREIF_bm) ||
+         !(SERIAL_PORT.STATUS & USART_TXCIF_bm))
+    continue;
+}
+
+
+void usart_rx_flush() {rx_buf_init();}
+int16_t usart_rx_space() {return rx_buf_space();}
+int16_t usart_rx_fill() {return rx_buf_fill();}
+int16_t usart_tx_space() {return tx_buf_space();}
+int16_t usart_tx_fill() {return tx_buf_fill();}

src/avr/src/usart.h

@@ -0,0 +1,100 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include <avr/io.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+
+
+// NOTE, RING_BUF_INDEX_TYPE must be be large enough to cover the buffer
+#define USART_TX_RING_BUF_SIZE 1024
+#define USART_RX_RING_BUF_SIZE 1024
+
+
+typedef enum {
+  USART_BAUD_9600,
+  USART_BAUD_19200,
+  USART_BAUD_38400,
+  USART_BAUD_57600,
+  USART_BAUD_115200,
+  USART_BAUD_230400,
+  USART_BAUD_460800,
+  USART_BAUD_921600,
+  USART_BAUD_500000,
+  USART_BAUD_1000000
+} baud_t;
+
+
+typedef enum {
+  USART_NONE,
+  USART_EVEN,
+  USART_ODD,
+} parity_t;
+
+
+typedef enum {
+  USART_1STOP,
+  USART_2STOP,
+} stop_t;
+
+
+typedef enum {
+  USART_5BITS,
+  USART_6BITS,
+  USART_7BITS,
+  USART_8BITS,
+  USART_9BITS,
+} bits_t;
+
+
+void usart_set_baud(USART_t *port, baud_t baud);
+void usart_set_parity(USART_t *port, parity_t parity);
+void usart_set_stop(USART_t *port, stop_t stop);
+void usart_set_bits(USART_t *port, bits_t bits);
+void usart_init_port(USART_t *port, baud_t baud, parity_t parity, bits_t bits,
+                     stop_t stop);
+
+void usart_init();
+void usart_putc(char c);
+void usart_puts(const char *s);
+int8_t usart_getc();
+char *usart_readline();
+void usart_flush();
+
+void usart_rx_flush();
+int16_t usart_rx_fill();
+int16_t 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 bool usart_tx_empty() {return !usart_tx_fill();}
+inline bool usart_tx_full() {return !usart_tx_space();}

src/avr/src/util.c

@@ -0,0 +1,95 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "util.h"
+
+#include "base64.h"
+
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+
+
+/// Fast inverse square root originally from Quake III Arena code.  Original
+/// comments left intact.
+/// See: https://en.wikipedia.org/wiki/Fast_inverse_square_root
+float invsqrt(float x) {
+  // evil floating point bit level hacking
+  union {
+    float f;
+    int32_t i;
+  } u;
+
+  const float xhalf = x * 0.5f;
+  u.f = x;
+  u.i = 0x5f3759df - (u.i >> 1);          // what the fuck?
+  u.f = u.f * (1.5f - xhalf * u.f * u.f); // 1st iteration
+  u.f = u.f * (1.5f - xhalf * u.f * u.f); // 2nd iteration, can be removed
+
+  return u.f;
+}
+
+
+int8_t decode_hex_nibble(char c) {
+  if ('0' <= c && c <= '9') return c - '0';
+  if ('a' <= c && c <= 'f') return c - 'a' + 10;
+  if ('A' <= c && c <= 'F') return c - 'A' + 10;
+  return -1;
+}
+
+
+bool decode_float(char **s, float *f) {
+  bool ok = b64_decode_float(*s, f) && isfinite(*f);
+  *s += 6;
+  return ok;
+}
+
+
+stat_t decode_axes(char **cmd, float axes[AXES]) {
+  while (**cmd) {
+    const char *names = "xyzabc";
+    const char *match = strchr(names, **cmd);
+    if (!match) break;
+    char *s = *cmd + 1;
+    if (!decode_float(&s, &axes[match - names])) return STAT_BAD_FLOAT;
+    *cmd = s;
+  }
+
+  return STAT_OK;
+}
+
+
+// Assumes the caller provide format buffer length is @param len * 2 + 1.
+void format_hex_buf(char *buf, const uint8_t *data, unsigned len) {
+  uint8_t i;
+
+  for (i = 0; i < len; i++)
+    sprintf(buf + i * 2, "%02x", data[i]);
+
+  buf[i * 2] = 0;
+}

src/avr/src/util.h

@@ -0,0 +1,85 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include "config.h"
+#include "status.h"
+
+#include <stdint.h>
+#include <math.h>
+#include <string.h>
+#include <stdbool.h>
+
+
+// Vector utilities
+#define clear_vector(a) memset(a, 0, sizeof(a))
+#define copy_vector(d, s) memcpy(d, s, sizeof(d))
+
+// Math utilities
+inline static float min(float a, float b) {return a < b ? a : b;}
+inline static float max(float a, float b) {return a < b ? b : a;}
+inline static float min3(float a, float b, float c) {return min(min(a, b), c);}
+inline static float max3(float a, float b, float c) {return max(max(a, b), c);}
+inline static float min4(float a, float b, float c, float d)
+{return min(min(a, b), min(c, d));}
+inline static float max4(float a, float b, float c, float d)
+{return max(max(a, b), max(c, d));}
+
+float invsqrt(float number);
+
+#ifndef __AVR__
+inline static float square(float x) {return x * x;}
+#endif
+
+// Floating-point utilities
+#define EPSILON 0.00001 // allowable rounding error for floats
+inline static bool fp_EQ(float a, float b) {return fabs(a - b) < EPSILON;}
+inline static bool fp_NE(float a, float b) {return fabs(a - b) > EPSILON;}
+inline static bool fp_ZERO(float a) {return fabs(a) < EPSILON;}
+inline static bool fp_FALSE(float a) {return fp_ZERO(a);}
+inline static bool fp_TRUE(float a) {return !fp_ZERO(a);}
+
+int8_t decode_hex_nibble(char c);
+bool decode_float(char **s, float *f);
+stat_t decode_axes(char **cmd, float axes[AXES]);
+void format_hex_buf(char *buf, const uint8_t *data, unsigned len);
+
+// Constants
+#define MM_PER_INCH 25.4
+#define INCHES_PER_MM (1 / 25.4)
+#define MICROSECONDS_PER_MINUTE 60000000.0
+
+// 24bit integers
+#ifdef __AVR__
+typedef __int24 int24_t;
+typedef __uint24 uint24_t;
+#else
+typedef int32_t int24_t;
+typedef uint32_t uint24_t;
+#endif

src/avr/src/vars.c

@@ -0,0 +1,411 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "vars.h"
+
+#include "type.h"
+#include "status.h"
+#include "hardware.h"
+#include "config.h"
+#include "axis.h"
+#include "cpp_magic.h"
+#include "report.h"
+#include "command.h"
+
+#include <string.h>
+#include <stdio.h>
+
+// Format strings
+static const char code_fmt[] PROGMEM = "\"%s\":";
+static const char indexed_code_fmt[] PROGMEM = "\"%c%s\":";
+
+
+// Ensure no var code is used more than once
+enum {
+#define VAR(NAME, CODE, ...) var_code_##CODE,
+#include "vars.def"
+#undef VAR
+  var_code_count
+};
+
+
+// Var forward declarations
+#define VAR(NAME, CODE, TYPE, INDEX, SET, ...)          \
+  TYPE get_##NAME(IF(INDEX)(int index));                \
+  IF(SET)                                               \
+  (void set_##NAME(IF(INDEX)(int index,) TYPE value);)
+
+#include "vars.def"
+#undef VAR
+
+
+// Set callback union
+typedef union {
+  void *ptr;
+#define TYPEDEF(TYPE, ...) void (*set_##TYPE)(TYPE);
+#include "type.def"
+#undef TYPEDEF
+
+#define TYPEDEF(TYPE, ...) void (*set_##TYPE##_index)(int i, TYPE);
+#include "type.def"
+#undef TYPEDEF
+} set_cb_u;
+
+
+// Get callback union
+typedef union {
+  void *ptr;
+#define TYPEDEF(TYPE, ...) TYPE (*get_##TYPE)();
+#include "type.def"
+#undef TYPEDEF
+
+#define TYPEDEF(TYPE, ...) TYPE (*get_##TYPE##_index)(int i);
+#include "type.def"
+#undef TYPEDEF
+} get_cb_u;
+
+
+typedef struct {
+  type_t type;
+  char name[5];
+  int8_t index;
+  get_cb_u get;
+  set_cb_u set;
+} var_info_t;
+
+
+// Var names
+#define VAR(NAME, CODE, TYPE, INDEX, SET, REPORT)           \
+  static const char NAME##_name[] PROGMEM = #NAME;
+
+#include "vars.def"
+#undef VAR
+
+
+// Last value
+#define VAR(NAME, CODE, TYPE, INDEX, ...)       \
+  static TYPE NAME##_state IF(INDEX)([INDEX]);
+
+#include "vars.def"
+#undef VAR
+
+
+// Report
+static uint8_t _report_var[(var_code_count >> 3) + 1] = {0,};
+
+
+static bool _get_report_var(int index) {
+  return _report_var[index >> 3] & (1 << (index & 7));
+}
+
+
+static void _set_report_var(int index, bool enable) {
+  if (enable) _report_var[index >> 3] |= 1 << (index & 7);
+  else _report_var[index >> 3] &= ~(1 << (index & 7));
+}
+
+
+static int _find_code(const char *code) {
+#define VAR(NAME, CODE, TYPE, INDEX, ...)                               \
+  if (!strcmp(code, #CODE)) return var_code_##CODE;                     \
+
+#include "vars.def"
+#undef VAR
+  return -1;
+}
+
+
+void vars_init() {
+  // Initialize var state
+#define VAR(NAME, CODE, TYPE, INDEX, ...)                       \
+  IF(INDEX)(for (int i = 0; i < INDEX; i++))                    \
+    (NAME##_state)IF(INDEX)([i]) = get_##NAME(IF(INDEX)(i));
+
+#include "vars.def"
+#undef VAR
+
+// Report
+#define VAR(NAME, CODE, TYPE, INDEX, SET, REPORT, ...)  \
+  _set_report_var(var_code_##CODE, REPORT);
+
+#include "vars.def"
+#undef VAR
+}
+
+
+void vars_report(bool full) {
+  bool reported = false;
+
+#define VAR(NAME, CODE, TYPE, INDEX, ...)                               \
+  if (_get_report_var(var_code_##CODE)) {                               \
+    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 (full || (!type_eq_##TYPE(value, last))) {                     \
+        (NAME##_state)IF(INDEX)([i]) = value;                           \
+                                                                        \
+        if (!reported) {                                                \
+          reported = true;                                              \
+          putchar('{');                                                 \
+        } else putchar(',');                                            \
+                                                                        \
+        printf_P                                                        \
+          (IF_ELSE(INDEX)(indexed_code_fmt, code_fmt),                  \
+           IF(INDEX)(INDEX##_LABEL[i],) #CODE);                         \
+                                                                        \
+        type_print_##TYPE(value);                                       \
+      }                                                                 \
+    }                                                                   \
+  }
+
+#include "vars.def"
+#undef VAR
+
+  if (reported) printf("}\n");
+}
+
+void vars_report_all(bool enable) {
+#define VAR(NAME, CODE, TYPE, INDEX, SET, REPORT, ...)                  \
+  _set_report_var(var_code_##CODE, enable);
+
+#include "vars.def"
+#undef VAR
+}
+
+
+void vars_report_var(const char *code, bool enable) {
+  int index = _find_code(code);
+  if (index != -1) _set_report_var(index, enable);
+}
+
+
+static char *_resolve_name(const char *_name) {
+  unsigned len = strlen(_name);
+
+  if (!len || 4 < len) return 0;
+
+  static char name[5];
+  strncpy(name, _name, 4);
+  name[4] = 0;
+
+  // Handle axis to motor mapping
+  if (2 < len && name[1] == '.') {
+    int axis = axis_get_id(name[0]);
+    if (axis < 0) return 0;
+    int motor = axis_get_motor(axis);
+    if (motor < 0) return 0;
+
+    name[0] = MOTORS_LABEL[motor];
+    for (int i = 1; _name[i]; i++)
+      name[i] = _name[i + 1];
+  }
+
+  return name;
+}
+
+
+static int _index(char c, const char *s) {
+  const char *index = strchr(s, c);
+  return index ? index - s : -1;
+}
+
+
+static bool _find_var(const char *_name, var_info_t *info) {
+  char *name = _resolve_name(_name);
+  if (!name) return false;
+
+  int i = -1;
+  memset(info, 0, sizeof(var_info_t));
+  strcpy(info->name, name);
+
+#define VAR(NAME, CODE, TYPE, INDEX, SET, ...)                          \
+  if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)                    \
+      IF(INDEX)(&& (i = _index(name[0], INDEX##_LABEL)) != -1)          \
+      ) {                                                               \
+                                                                        \
+    info->type = TYPE_##TYPE;                                           \
+    info->index = i;                                                    \
+    info->get.IF_ELSE(INDEX)(get_##TYPE##_index, get_##TYPE) =          \
+      get_##NAME;                                                       \
+                                                                        \
+    IF(SET)(info->set.IF_ELSE(INDEX)                                    \
+            (set_##TYPE##_index, set_##TYPE) = set_##NAME;)             \
+                                                                        \
+      return true;                                                      \
+  }
+
+#include "vars.def"
+#undef VAR
+
+  return false;
+}
+
+
+static type_u _get(type_t type, int8_t index, get_cb_u cb) {
+  type_u value;
+
+  switch (type) {
+#define TYPEDEF(TYPE, ...)                                              \
+    case TYPE_##TYPE:                                                   \
+      if (index == -1) value._##TYPE = cb.get_##TYPE();                 \
+      value._##TYPE = cb.get_##TYPE##_index(index);                     \
+      break;
+#include "type.def"
+#undef TYPEDEF
+  }
+
+  return value;
+}
+
+
+static void _set(type_t type, int8_t index, set_cb_u cb, type_u value) {
+  switch (type) {
+#define TYPEDEF(TYPE, ...)                                              \
+    case TYPE_##TYPE:                                                   \
+      if (index == -1) cb.set_##TYPE(value._##TYPE);                    \
+      else cb.set_##TYPE##_index(index, value._##TYPE);                 \
+      break;
+#include "type.def"
+#undef TYPEDEF
+  }
+}
+
+
+stat_t vars_print(const char *name) {
+  var_info_t info;
+  if (!_find_var(name, &info)) return STAT_UNRECOGNIZED_NAME;
+
+  printf_P(PSTR("{\"%s\":"), info.name);
+  type_print(info.type, _get(info.type, info.index, info.get));
+  putchar('}');
+  putchar('\n');
+
+  return STAT_OK;
+}
+
+
+stat_t vars_set(const char *name, const char *value) {
+  var_info_t info;
+  if (!_find_var(name, &info)) return STAT_UNRECOGNIZED_NAME;
+  if (!info.set.ptr) return STAT_READ_ONLY;
+
+  stat_t status;
+  type_u x = type_parse(info.type, value, &status);
+  if (status == STAT_OK) _set(info.type, info.index, info.set, x);
+
+  return status;
+}
+
+
+void vars_print_json() {
+  bool first = true;
+  static const char fmt[] PROGMEM =
+    "\"%s\":{\"name\":\"%" PRPSTR "\",\"type\":\"%" PRPSTR "\"";
+  static const char index_fmt[] PROGMEM = ",\"index\":\"%s\"";
+
+#define VAR(NAME, CODE, TYPE, INDEX, ...)                               \
+  if (first) first = false; else putchar(',');                          \
+  printf_P(fmt, #CODE, NAME##_name, type_get_##TYPE##_name_pgm());      \
+  IF(INDEX)(printf_P(index_fmt, INDEX##_LABEL));                        \
+  putchar('}');
+#include "vars.def"
+#undef VAR
+}
+
+
+// Command callbacks
+stat_t command_var(char *cmd) {
+  cmd++; // Skip command code
+
+  if (*cmd == '$' && !cmd[1]) {
+    report_request_full();
+    return STAT_OK;
+  }
+
+  // Get or set variable
+  char *value = strchr(cmd, '=');
+  if (value) {
+    *value++ = 0;
+    stat_t status = vars_set(cmd, value);
+    value[-1] = '='; // For error reporting
+    return status;
+  }
+
+  return vars_print(cmd);
+}
+
+
+typedef struct {
+  type_t type;
+  int8_t index;
+  set_cb_u set;
+  type_u value;
+} var_cmd_t;
+
+
+stat_t command_sync_var(char *cmd) {
+  // Get value
+  char *value = strchr(cmd + 1, '=');
+  if (!value) return STAT_INVALID_COMMAND;
+  *value++ = 0;
+
+  var_info_t info;
+  if (!_find_var(cmd + 1, &info)) return STAT_UNRECOGNIZED_NAME;
+  if (!info.set.ptr) return STAT_READ_ONLY;
+
+  stat_t status;
+  var_cmd_t buffer;
+
+  buffer.type = info.type;
+  buffer.index = info.index;
+  buffer.set = info.set;
+  buffer.value = type_parse(info.type, value, &status);
+
+  if (status == STAT_OK) command_push(*cmd, &buffer);
+
+  return status;
+}
+
+
+unsigned command_sync_var_size() {return sizeof(var_cmd_t);}
+
+
+void command_sync_var_exec(void *data) {
+  var_cmd_t *cmd = (var_cmd_t *)data;
+  _set(cmd->type, cmd->index, cmd->set, cmd->value);
+}
+
+
+stat_t command_report(char *cmd) {
+  bool enable = cmd[1] != '0';
+
+  if (cmd[2]) vars_report_var(cmd + 2, enable);
+  else vars_report_all(enable);
+
+  return STAT_OK;
+}

src/avr/src/vars.def

@@ -0,0 +1,150 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#define   AXES_LABEL "xyzabc"
+#define MOTORS_LABEL "0123"
+#define   OUTS_LABEL "ed12ft"
+#define ANALOG_LABEL "12"
+#define VFDREG_LABEL "0123456789abcdefghijklmnopqrstuv"
+
+// VAR(name, code, type, index, settable, report)
+
+// Motor
+VAR(motor_axis,      an, u8,    MOTORS, 1, 1) // Maps motor to axis
+
+VAR(motor_enabled,   me, b8,    MOTORS, 1, 1) // Motor enabled
+VAR(drive_current,   dc, f32,   MOTORS, 1, 1) // Max motor drive current
+VAR(idle_current,    ic, f32,   MOTORS, 1, 1) // Motor idle current
+
+VAR(reverse,         rv, b8,    MOTORS, 1, 1) // Reverse motor polarity
+VAR(microstep,       mi, u16,   MOTORS, 1, 1) // Microsteps per full step
+VAR(velocity_max,    vm, f32,   MOTORS, 1, 1) // Maxium vel in mm/min
+VAR(accel_max,       am, f32,   MOTORS, 1, 1) // Maxium accel in mm/min^2
+VAR(jerk_max,        jm, f32,   MOTORS, 1, 1) // Maxium jerk in mm/min^3
+VAR(step_angle,      sa, f32,   MOTORS, 1, 1) // In degrees per full step
+VAR(travel,          tr, f32,   MOTORS, 1, 1) // Travel in mm/rev
+
+VAR(min_soft_limit,  tn, f32,   MOTORS, 1, 1) // Min soft limit
+VAR(max_soft_limit,  tm, f32,   MOTORS, 1, 1) // Max soft limit
+VAR(homed,            h, b8,    MOTORS, 1, 1) // Motor homed status
+
+VAR(active_current,  ac, f32,   MOTORS, 0, 0) // Motor current now
+VAR(driver_flags,    df, u16,   MOTORS, 1, 1) // Motor driver flags
+VAR(driver_stalled,  sl, b8,    MOTORS, 0, 0) // Motor driver status
+VAR(stall_volts,     tv, f32,   MOTORS, 1, 1) // Motor BEMF threshold voltage
+VAR(encoder,         en, s32,   MOTORS, 0, 0) // Motor encoder
+VAR(error,           ee, s32,   MOTORS, 0, 0) // Motor position error
+
+VAR(stall_microstep, lm, u16,   MOTORS, 1, 1) // Motor stall microsteps
+VAR(stall_sample_time,sp,u16,   MOTORS, 1, 1) // Motor stall sample time
+VAR(stall_current,   tc, f32,   MOTORS, 1, 1) // Motor stall current
+
+VAR(motor_fault,     fa, b8,    0,      0, 1) // Motor fault status
+
+// Switches
+VAR(min_sw_mode,     ls, u8,    MOTORS, 1, 1) // Minimum switch mode
+VAR(max_sw_mode,     xs, u8,    MOTORS, 1, 1) // Maximum switch mode
+VAR(estop_mode,      et, u8,    0,      1, 1) // Estop switch mode
+VAR(probe_mode,      pt, u8,    0,      1, 1) // Probe switch mode
+VAR(min_switch,      lw, u8,    MOTORS, 0, 1) // Minimum switch state
+VAR(max_switch,      xw, u8,    MOTORS, 0, 1) // Maximum switch state
+VAR(estop_switch,    ew, u8,    0,      0, 1) // Estop switch state
+VAR(probe_switch,    pw, u8,    0,      0, 1) // Probe switch state
+VAR(switch_debounce, sd, u16,   0,      1, 1) // Switch debounce time in ms
+VAR(switch_lockout,  sc, u16,   0,      1, 1) // Switch lockout time in ms
+
+// Axis
+VAR(axis_position,    p, f32,   AXES,   0, 1) // Axis position
+
+// Outputs
+VAR(output_active,   oa, b8,    OUTS,   1, 1) // Output pin active
+VAR(output_state,    os, u8,    OUTS,   0, 1) // Output pin state
+VAR(output_mode,     om, u8,    OUTS,   1, 1) // Output pin mode
+
+// Analog
+VAR(analog_input,    ai, f32,   ANALOG, 0, 0) // Analog input pins
+
+// Spindle
+VAR(tool_type,       st, u8,    0,      1, 1) // See spindle.c
+VAR(speed,            s, f32,   0,      0, 1) // Current spindle speed
+VAR(tool_reversed,   sr, b8,    0,      1, 1) // Reverse tool
+VAR(max_spin,        sx, f32,   0,      1, 1) // Maximum spindle speed
+VAR(min_spin,        sm, f32,   0,      1, 1) // Minimum spindle speed
+VAR(spindle_status,  ss, u16,   0,      0, 1) // Spindle status code
+
+// PWM spindle
+VAR(pwm_invert,      pi, b8,    0,      1, 1) // Inverted spindle PWM
+VAR(pwm_min_duty,    nd, f32,   0,      1, 1) // Minimum PWM duty cycle
+VAR(pwm_max_duty,    md, f32,   0,      1, 1) // Maximum PWM duty cycle
+VAR(pwm_duty,        pd, f32,   0,      0, 0) // Current PWM duty cycle
+VAR(pwm_freq,        sf, f32,   0,      1, 0) // Spindle PWM frequency in Hz
+
+// Modbus spindle
+VAR(mb_debug,        hb, b8,    0,      1, 1) // Modbus debugging
+VAR(mb_id,           hi, u8,    0,      1, 1) // Modbus ID
+VAR(mb_baud,         mb, u8,    0,      1, 1) // Modbus BAUD rate
+VAR(mb_parity,       ma, u8,    0,      1, 1) // Modbus parity
+VAR(mb_status,       mx, u8,    0,      0, 1) // Modbus status
+VAR(mb_crc_errs,     cr, u16,   0,      0, 1) // Modbus CRC error counter
+
+// VFD spindle
+VAR(vfd_max_freq,    vf, u16,   0,      1, 1) // VFD maximum frequency
+VAR(vfd_multi_write, mw, b8,    0,      1, 1) // Use Modbus multi write mode
+VAR(vfd_reg_type,    vt, u8,    VFDREG, 1, 1) // VFD register type
+VAR(vfd_reg_addr,    va, u16,   VFDREG, 1, 1) // VFD register address
+VAR(vfd_reg_val,     vv, u16,   VFDREG, 1, 1) // VFD register value
+VAR(vfd_reg_fails,   vr, u8,    VFDREG, 1, 1) // VFD register fail count
+
+// Huanyang spindle
+VAR(hy_freq,         hz, f32,   0,      0, 0) // Huanyang actual freq
+VAR(hy_current,      hc, f32,   0,      0, 0) // Huanyang actual current
+VAR(hy_temp,         ht, u16,   0,      0, 0) // Huanyang temperature
+VAR(hy_max_freq,     hx, f32,   0,      0, 1) // Huanyang max freq
+VAR(hy_min_freq,     hm, f32,   0,      0, 1) // Huanyang min freq
+VAR(hy_rated_rpm,    hq, u16,   0,      0, 1) // Huanyang rated RPM
+
+// Machine state
+VAR(id,              id, u16,   0,      1, 1) // Last executed command ID
+VAR(feed_override,   fo, u16,   0,      1, 1) // Feed rate override
+VAR(speed_override,  so, u16,   0,      1, 1) // Spindle speed override
+
+// System
+VAR(velocity,         v, f32,   0,      0, 1) // Current velocity
+VAR(acceleration,    ax, f32,   0,      0, 0) // Current acceleration
+VAR(jerk,             j, f32,   0,      0, 0) // Current jerk
+VAR(peak_vel,        pv, f32,   0,      1, 1) // Peak velocity, set to clear
+VAR(peak_accel,      pa, f32,   0,      1, 1) // Peak accel, set to clear
+VAR(dynamic_power,   dp, b8,    0,      1, 1) // Dynamic power
+VAR(inverse_feed,    if, f32,   0,      1, 1) // Inverse feed rate
+VAR(hw_id,          hid, str,   0,      0, 1) // Hardware ID
+VAR(estop,           es, b8,    0,      1, 1) // Emergency stop
+VAR(estop_reason,    er, pstr,  0,      0, 1) // Emergency stop reason
+VAR(state,           xx, pstr,  0,      0, 1) // Machine state
+VAR(state_count,     xc, u16,   0,      0, 1) // Machine state change count
+VAR(hold_reason,     pr, pstr,  0,      0, 1) // Machine pause reason
+VAR(underrun,        un, u32,   0,      0, 1) // Stepper buffer underrun count
+VAR(dwell_time,      dt, f32,   0,      0, 1) // Dwell timer

src/avr/src/vars.h

@@ -0,0 +1,45 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "status.h"
+
+#include <stdbool.h>
+
+
+float var_decode_float(const char *value);
+bool var_parse_bool(const char *value);
+
+void vars_init();
+
+void vars_report(bool full);
+void vars_report_all(bool enable);
+void vars_report_var(const char *code, bool enable);
+stat_t vars_print(const char *name);
+stat_t vars_set(const char *name, const char *value);
+void vars_print_json();

src/avr/src/vars.json.in

@@ -0,0 +1,14 @@
+#include "cpp_magic.h"
+{
+#define VAR(NAME, CODE, TYPE, INDEX, SET, REPORT) \
+#CODE: {                                        \
+    "name": #NAME,                              \
+    "type": #TYPE,                              \
+    "index": IF_ELSE(INDEX)(true, false),       \
+    "setable": IF_ELSE(SET)(true, false),       \
+    "report": IF_ELSE(REPORT)(true, false)      \
+  },
+#include "vars.def"
+#undef VAR
+  "_": {}
+}

src/avr/src/vfd_spindle.c

@@ -0,0 +1,478 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "vfd_spindle.h"
+#include "modbus.h"
+#include "rtc.h"
+#include "config.h"
+#include "estop.h"
+#include "pgmspace.h"
+
+#include <util/atomic.h>
+
+#include <string.h>
+#include <math.h>
+#include <stdint.h>
+
+
+typedef enum {
+  REG_DISABLED,
+
+  REG_CONNECT_WRITE,
+
+  REG_MAX_FREQ_READ,
+  REG_MAX_FREQ_FIXED,
+
+  REG_FREQ_SET,
+  REG_FREQ_SIGN_SET,
+
+  REG_STOP_WRITE,
+  REG_FWD_WRITE,
+  REG_REV_WRITE,
+
+  REG_FREQ_READ,
+  REG_FREQ_SIGN_READ,
+  REG_FREQ_ACTECH_READ,
+
+  REG_STATUS_READ,
+
+  REG_DISCONNECT_WRITE,
+} vfd_reg_type_t;
+
+
+typedef struct {
+  vfd_reg_type_t type;
+  uint16_t addr;
+  uint16_t value;
+  uint8_t fails;
+} vfd_reg_t;
+
+
+#define P(H, L) ((H) << 8 | (L))
+
+
+// NOTE, Modbus reg = AC Tech reg + 1
+const vfd_reg_t ac_tech_regs[] PROGMEM = {
+  {REG_CONNECT_WRITE,    48,   19}, // Password unlock
+  {REG_CONNECT_WRITE,     1,  512}, // Manual mode
+  {REG_MAX_FREQ_READ,    62,    0}, // Max frequency
+  {REG_FREQ_SET,         40,    0}, // Frequency
+  {REG_STOP_WRITE,        1,    4}, // Stop drive
+  {REG_FWD_WRITE,         1,  128}, // Forward
+  {REG_FWD_WRITE,         1,    8}, // Start drive
+  {REG_REV_WRITE,         1,   64}, // Reverse
+  {REG_REV_WRITE,         1,    8}, // Start drive
+  {REG_FREQ_ACTECH_READ, 24,    0}, // Actual freq
+  {REG_DISCONNECT_WRITE,  1,    2}, // Lock controls and parameters
+  {REG_DISABLED},
+};
+
+
+const vfd_reg_t nowforever_regs[] PROGMEM = {
+  {REG_MAX_FREQ_READ,     7,    0}, // Max frequency
+  {REG_FREQ_SET,       2305,    0}, // Frequency
+  {REG_STOP_WRITE,     2304,    0}, // Stop drive
+  {REG_FWD_WRITE,      2304,    1}, // Forward
+  {REG_REV_WRITE,      2304,    3}, // Reverse
+  {REG_FREQ_READ,      1282,    0}, // Output freq
+  {REG_STATUS_READ,     768,    0}, // Status
+  {REG_DISABLED},
+};
+
+
+const vfd_reg_t delta_vfd015m21a_regs[] PROGMEM = {
+  {REG_CONNECT_WRITE, 0x2002,  2}, // Reset fault
+  {REG_MAX_FREQ_READ,      3,  0}, // Max frequency
+  {REG_FREQ_SET,      0x2001,  0}, // Frequency
+  {REG_STOP_WRITE,    0x2000,  1}, // Stop drive
+  {REG_FWD_WRITE,     0x2000, 18}, // Forward
+  {REG_REV_WRITE,     0x2000, 34}, // Reverse
+  {REG_FREQ_READ,     0x2103,  0}, // Output freq
+  {REG_STATUS_READ,   0x2100,  0}, // Status
+  {REG_DISABLED},
+};
+
+
+const vfd_reg_t yl600_regs[] PROGMEM = {
+  {REG_CONNECT_WRITE, 0x2000, 128}, // Reset all errors
+  {REG_MAX_FREQ_READ, 0x0004,   0}, // Max frequency
+  {REG_FREQ_SET,      0x2001,   0}, // Frequency
+  {REG_STOP_WRITE,    0x2000,   1}, // Stop drive
+  {REG_FWD_WRITE,     0x2000,  18}, // Forward
+  {REG_REV_WRITE,     0x2000,  34}, // Reverse
+  {REG_FREQ_READ,     0x200b,   0}, // Output freq
+  {REG_STATUS_READ,   0x2008,   0}, // Status
+  {REG_DISABLED},
+};
+
+
+const vfd_reg_t fr_d700_regs[] PROGMEM = {
+  {REG_MAX_FREQ_READ,  1000,    0}, // Max frequency
+  {REG_FREQ_SET,         13,    0}, // Frequency
+  {REG_STOP_WRITE,        8,    1}, // Stop drive
+  {REG_FWD_WRITE,         8,    2}, // Forward
+  {REG_REV_WRITE,         8,    4}, // Reverse
+  {REG_FREQ_READ,       200,    0}, // Output freq
+  {REG_DISABLED},
+};
+
+
+const vfd_reg_t sunfar_e300_regs[] PROGMEM = {
+  {REG_CONNECT_WRITE, 0x1001,  32}, // Reset all errors
+  {REG_MAX_FREQ_READ, 0xf004,   0}, // Max frequency F0.4
+  {REG_FREQ_SET,      0x1002,   0}, // Frequency
+  {REG_STOP_WRITE,    0x1001,   3}, // Stop drive
+  {REG_FWD_WRITE,     0x1001,   1}, // Forward
+  {REG_REV_WRITE,     0x1001,   2}, // Reverse
+  {REG_FREQ_READ,     0xd000,   0}, // Output freq d.0
+  {REG_STATUS_READ,   0x2000,   0}, // Status
+  {REG_DISABLED},
+};
+
+
+const vfd_reg_t omron_mx2_regs[] PROGMEM = {
+  {REG_CONNECT_WRITE,  0x1200,     3}, // A001 Frequency reference modbus
+  {REG_CONNECT_WRITE,  0x1201,     3}, // A002 Run command modbus
+  {REG_MAX_FREQ_FIXED,      0, 40000}, // TODO Want to use A004 max frequency
+  {REG_FREQ_SET,       0x0001,     0}, // F001 Frequency
+  {REG_STOP_WRITE,     0x1f00,     0}, // Stop drive
+  {REG_FWD_WRITE,      0x1f00,     2}, // Forward
+  {REG_REV_WRITE,      0x1f00,     6}, // Reverse
+  {REG_FREQ_READ,      0x1001,     0}, // D001 Output freq
+  {REG_STATUS_READ,    0x0004,     0}, // Status A
+  {REG_DISABLED},
+};
+
+
+const vfd_reg_t v70_regs[] PROGMEM = {
+  {REG_MAX_FREQ_READ, 0x0005, 0}, // Maximum operating frequency
+  {REG_FREQ_SET,      0x0201, 0}, // Set frequency in 0.1Hz
+  {REG_STOP_WRITE,    0x0200, 0}, // Stop
+  {REG_FWD_WRITE,     0x0200, 1}, // Run forward
+  {REG_REV_WRITE,     0x0200, 5}, // Run reverse
+  {REG_FREQ_READ,     0x0220, 0}, // Read operating frequency
+  {REG_STATUS_READ,   0x0210, 0}, // Read status
+  {REG_DISABLED},
+};
+
+
+static vfd_reg_t regs[VFDREG];
+static vfd_reg_t custom_regs[VFDREG];
+
+static struct {
+  vfd_reg_type_t state;
+  int8_t reg;
+  uint8_t read_count;
+  bool changed;
+  bool shutdown;
+
+  float power;
+  uint16_t max_freq;
+  bool user_multi_write;
+  float actual_power;
+  uint16_t status;
+
+  uint32_t wait;
+  deinit_cb_t deinit_cb;
+} vfd;
+
+
+static void _disconnected() {
+  modbus_deinit();
+  if (vfd.deinit_cb) vfd.deinit_cb();
+  vfd.deinit_cb = 0;
+}
+
+
+static bool _next_state() {
+  switch (vfd.state) {
+  case REG_MAX_FREQ_FIXED:
+    if (!vfd.power) vfd.state = REG_STOP_WRITE;
+    else vfd.state = REG_FREQ_SET;
+    break;
+
+  case REG_FREQ_SIGN_SET:
+    if (vfd.power < 0) vfd.state = REG_REV_WRITE;
+    else if (0 < vfd.power) vfd.state = REG_FWD_WRITE;
+    else vfd.state = REG_STOP_WRITE;
+    break;
+
+  case REG_STOP_WRITE: case REG_FWD_WRITE: case REG_REV_WRITE:
+    vfd.state = REG_FREQ_READ;
+    break;
+
+  case REG_STATUS_READ:
+    if (vfd.shutdown || estop_triggered()) vfd.state = REG_DISCONNECT_WRITE;
+
+    else if (vfd.changed) {
+      // Update frequency and state
+      vfd.changed = false;
+      vfd.state = REG_MAX_FREQ_READ;
+
+    } else {
+      // Continue querying after delay
+      vfd.state = REG_FREQ_READ;
+      vfd.wait = rtc_get_time() + VFD_QUERY_DELAY;
+      return false;
+    }
+    break;
+
+  case REG_DISCONNECT_WRITE:
+    _disconnected();
+    return false;
+
+  default:
+    vfd.state = (vfd_reg_type_t)(vfd.state + 1);
+  }
+
+  return true;
+}
+
+
+static bool _exec_command();
+
+
+static void _next_reg() {
+  while (true) {
+    vfd.reg++;
+
+    if (vfd.reg == VFDREG) {
+      vfd.reg = -1;
+      vfd.read_count = 0;
+      if (!_next_state()) break;
+
+    } else if (regs[vfd.reg].type == vfd.state && _exec_command()) break;
+  }
+}
+
+
+static void _connect() {
+  vfd.state = REG_CONNECT_WRITE;
+  vfd.reg = -1;
+  _next_reg();
+}
+
+
+static void _modbus_cb(bool ok, uint16_t addr, uint16_t value) {
+  // Handle error
+  if (!ok) {
+    if (regs[vfd.reg].fails < 255) regs[vfd.reg].fails++;
+    if (vfd.shutdown || estop_triggered()) _disconnected();
+    else _connect();
+    return;
+  }
+
+  // Handle read result
+  vfd.read_count++;
+
+  switch (regs[vfd.reg].type) {
+  case REG_MAX_FREQ_READ: vfd.max_freq = value; break;
+  case REG_FREQ_READ: vfd.actual_power = value / (float)vfd.max_freq; break;
+
+  case REG_FREQ_SIGN_READ:
+    vfd.actual_power = (int16_t)value / (float)vfd.max_freq;
+    break;
+
+  case REG_FREQ_ACTECH_READ:
+    if (vfd.read_count == 2) vfd.actual_power = value / (float)vfd.max_freq;
+    if (vfd.read_count < 6) return;
+    break;
+
+  case REG_STATUS_READ: vfd.status = value; break;
+
+  default: break;
+  }
+
+  // Next
+  _next_reg();
+}
+
+
+static bool _use_multi_write() {
+  switch (spindle_get_type()) {
+  case SPINDLE_TYPE_CUSTOM:     return vfd.user_multi_write;
+  case SPINDLE_TYPE_NOWFOREVER: return true;
+  default:                      return false;
+  }
+}
+
+
+static bool _exec_command() {
+  if (vfd.wait) return true;
+
+  vfd_reg_t reg = regs[vfd.reg];
+  uint16_t words = 1;
+  bool read = false;
+  bool write = false;
+
+  switch (reg.type) {
+  case REG_DISABLED: break;
+
+  case REG_MAX_FREQ_FIXED: vfd.max_freq = reg.value; break;
+
+  case REG_FREQ_SET:
+    write = true;
+    reg.value = fabs(vfd.power) * vfd.max_freq;
+    break;
+
+  case REG_FREQ_SIGN_SET:
+    write = true;
+    reg.value = vfd.power * vfd.max_freq;
+    break;
+
+  case REG_CONNECT_WRITE:
+  case REG_STOP_WRITE:
+  case REG_FWD_WRITE:
+  case REG_REV_WRITE:
+  case REG_DISCONNECT_WRITE:
+    write = true;
+    break;
+
+  case REG_FREQ_ACTECH_READ:
+    words = 6;
+
+  case REG_FREQ_READ:
+  case REG_FREQ_SIGN_READ:
+  case REG_MAX_FREQ_READ:
+  case REG_STATUS_READ:
+    read = true;
+    break;
+  }
+
+  if (read) modbus_read(reg.addr, words, _modbus_cb);
+  else if (write) (_use_multi_write() ? modbus_multi_write : modbus_write)
+                    (reg.addr, reg.value, _modbus_cb);
+  else return false;
+
+  return true;
+}
+
+
+static void _load(const vfd_reg_t *_regs) {
+  memset(&regs, 0, sizeof(regs));
+
+  for (int i = 0; i < VFDREG; i++) {
+    regs[i].type = (vfd_reg_type_t)pgm_read_byte(&_regs[i].type);
+    if (!regs[i].type) break;
+    regs[i].addr = pgm_read_word(&_regs[i].addr);
+    regs[i].value = pgm_read_word(&_regs[i].value);
+  }
+}
+
+
+void vfd_spindle_init() {
+  memset(&vfd, 0, sizeof(vfd));
+  for (int i = 0; i < VFDREG; i++) regs[i].fails = 0;
+  modbus_init();
+
+  switch (spindle_get_type()) {
+  case SPINDLE_TYPE_CUSTOM:  memcpy(regs, custom_regs, sizeof(regs)); break;
+  case SPINDLE_TYPE_AC_TECH:          _load(ac_tech_regs);            break;
+  case SPINDLE_TYPE_NOWFOREVER:       _load(nowforever_regs);         break;
+  case SPINDLE_TYPE_DELTA_VFD015M21A: _load(delta_vfd015m21a_regs);   break;
+  case SPINDLE_TYPE_YL600:            _load(yl600_regs);              break;
+  case SPINDLE_TYPE_FR_D700:          _load(fr_d700_regs);            break;
+  case SPINDLE_TYPE_SUNFAR_E300:      _load(sunfar_e300_regs);        break;
+  case SPINDLE_TYPE_OMRON_MX2:        _load(omron_mx2_regs);          break;
+  case SPINDLE_TYPE_V70:              _load(v70_regs);                break;
+  default: break;
+  }
+
+  _connect();
+}
+
+
+void vfd_spindle_deinit(deinit_cb_t cb) {
+  vfd.shutdown = true;
+  vfd.deinit_cb = cb;
+}
+
+
+void vfd_spindle_set(float power) {
+  if (vfd.power != power)
+    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+      vfd.power = power;
+      vfd.changed = true;
+    }
+}
+
+
+float vfd_spindle_get() {return vfd.actual_power;}
+uint16_t vfd_get_status() {return vfd.status;}
+
+
+void vfd_spindle_rtc_callback() {
+  if (!vfd.wait || !rtc_expired(vfd.wait)) return;
+  vfd.wait = 0;
+  _next_reg();
+}
+
+
+// Variable callbacks
+uint16_t get_vfd_max_freq() {return vfd.max_freq;}
+void set_vfd_max_freq(uint16_t max_freq) {vfd.max_freq = max_freq;}
+bool get_vfd_multi_write() {return vfd.user_multi_write;}
+void set_vfd_multi_write(bool value) {vfd.user_multi_write = value;}
+uint8_t get_vfd_reg_type(int reg) {return regs[reg].type;}
+
+
+void set_vfd_reg_type(int reg, uint8_t type) {
+  custom_regs[reg].type = (vfd_reg_type_t)type;
+  if (spindle_get_type() == SPINDLE_TYPE_CUSTOM)
+    regs[reg].type = custom_regs[reg].type;
+  vfd.changed = true;
+}
+
+
+uint16_t get_vfd_reg_addr(int reg) {return regs[reg].addr;}
+
+
+void set_vfd_reg_addr(int reg, uint16_t addr) {
+  custom_regs[reg].addr = addr;
+  if (spindle_get_type() == SPINDLE_TYPE_CUSTOM)
+    regs[reg].addr = custom_regs[reg].addr;
+  vfd.changed = true;
+}
+
+
+uint16_t get_vfd_reg_val(int reg) {return regs[reg].value;}
+
+
+void set_vfd_reg_val(int reg, uint16_t value) {
+  custom_regs[reg].value = value;
+  if (spindle_get_type() == SPINDLE_TYPE_CUSTOM)
+    regs[reg].value = custom_regs[reg].value;
+  vfd.changed = true;
+}
+
+
+uint8_t get_vfd_reg_fails(int reg) {return regs[reg].fails;}
+
+
+void set_vfd_reg_fails(int reg, uint8_t value) {
+  regs[reg].fails = value;
+}

src/avr/src/vfd_spindle.h

@@ -0,0 +1,38 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "spindle.h"
+
+
+void vfd_spindle_init();
+void vfd_spindle_deinit(deinit_cb_t cb);
+void vfd_spindle_set(float power);
+float vfd_spindle_get();
+uint16_t vfd_get_status();
+void vfd_spindle_rtc_callback();

src/avr/step-test/Makefile

@@ -0,0 +1,20 @@
+# Makefile for Bulidbotics step-test
+PROJECT  = step-test
+MCU      = atxmega192a3u
+CLOCK    = 32000000
+
+# SRC
+SRC = usart.c lcd.c pins.c hardware.c
+SRC := $(wildcard *.c) $(patsubst %,../src/%,$(SRC))
+
+include ../Makefile.common
+
+CFLAGS += -I../src -I.
+
+# Build
+all: $(PROJECT).hex size
+
+$(PROJECT).elf: $(SRC)
+	$(CC) $(SRC) $(CFLAGS) $(LDFLAGS) $(LIBS) -o $@
+
+.PHONY: all

src/avr/step-test/plot_velocity.py

@@ -0,0 +1,187 @@
+#!/usr/bin/env python3
+
+################################################################################
+#                                                                              #
+#                 This file is part of the Buildbotics firmware.               #
+#                                                                              #
+#                   Copyright (c) 2015 - 2018, Buildbotics LLC                 #
+#                              All rights reserved.                            #
+#                                                                              #
+#      This file ("the software") is free software: you can redistribute it    #
+#      and/or modify it under the terms of the GNU General Public License,     #
+#       version 2 as published by the Free Software Foundation. You should     #
+#       have received a copy of the GNU General Public License, version 2      #
+#      along with the software. If not, see <http://www.gnu.org/licenses/>.    #
+#                                                                              #
+#      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                #
+#                        <http://www.gnu.org/licenses/>.                       #
+#                                                                              #
+#                 For information regarding this software email:               #
+#                   "Joseph Coffland" <joseph@buildbotics.com>                 #
+#                                                                              #
+################################################################################
+
+import sys, serial, argparse
+import numpy as np
+import math
+import json
+from time import sleep
+from collections import deque
+from datetime import datetime
+
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+
+
+MM_PER_STEP = 5 * 1.8 / 360 / 32
+SAMPLES_PER_MIN = 6000
+
+
+class Plot:
+  def __init__(self, port, baud, max_len):
+    # Open serial port
+    self.sp = serial.Serial(port, baud)
+
+    # Create data series
+    self.series = []
+    for i in range(5):
+      self.series.append(deque([0.0] * max_len))
+
+    # Init vars
+    self.max_len = max_len
+    self.incoming = ''
+    self.last = [None] * 4
+
+    # Open velocity log
+    ts = datetime.now().strftime('%Y-%m-%d-%H:%M:%S')
+    self.log = open('velocity-%s.log' % ts, 'w')
+
+
+  # Add new series data
+  def add(self, i, value):
+    self.series[i].pop()
+    self.series[i].appendleft(value)
+
+
+  def update_text(self, text, vel, data):
+      text[4].set_text('V {0:8,.2f}'.format(vel))
+
+      for i in range(4):
+        text[i].set_text('{} {:11,}'.format('XYZA'[i], int(data[i])))
+
+
+  def update(self, frame, axes, text):
+    # Read new data
+    try:
+      data = self.sp.read(self.sp.in_waiting)
+      self.incoming += data.decode('utf-8')
+
+    except Exception as e:
+      print(e)
+      return
+
+    while True:
+      # Parse lines
+      i = self.incoming.find('\n')
+      if i == -1: break
+      line = self.incoming[0:i]
+      self.incoming = self.incoming[i + 1:]
+
+      # Handle reset
+      if line.find('RESET') != -1:
+        self.update_text(text, 0, [0] * 4)
+        self.log.write(line + '\n')
+        self.last = [None] * 4
+        continue
+
+      # Parse data
+      try:
+        data = [float(value) for value in line.split(',')]
+      except ValueError: continue
+
+      if len(data) != 4: continue
+
+      # Compute axis velocities
+      v = []     # Axis velocity
+      totalV = 0 # Tool velocity
+
+      for i in range(4):
+        if self.last[i] is not None:
+          delta = self.last[i] - data[i]
+          v.append(delta * MM_PER_STEP * SAMPLES_PER_MIN) # mm/min
+          totalV += math.pow(v[i], 2)
+
+        self.last[i] = data[i]
+
+      # Compute tool velocity
+      totalV = math.sqrt(totalV)
+
+      # Update position and velocity text
+      self.update_text(text, totalV, data)
+
+      # Don't update plots when not moving
+      if totalV == 0: continue
+
+      # Add new data
+      for i in range(4): self.add(i, v[i])
+      self.add(4, totalV)
+
+      # Update plots
+      for i in range(5):
+        axes[i].set_data(range(self.max_len), self.series[i])
+
+      self.log.write(line + '\n')
+
+
+  def close(self):
+    self.sp.flush()
+    self.sp.close()
+
+
+if __name__ == '__main__':
+  # Parse command line arguments
+  description = "Plot velocity data in real-time"
+  parser = argparse.ArgumentParser(description = description)
+  parser.add_argument('-p', '--port', default = '/dev/ttyUSB0')
+  parser.add_argument('-b', '--baud', default = 115200, type = int)
+  parser.add_argument('-m', '--max-width', default = 2000, type = int)
+  args = parser.parse_args()
+
+  # Create plot
+  plot = Plot(args.port, args.baud, args.max_width)
+  fig = plt.figure()
+  ax = plt.axes(xlim = (0, args.max_width), ylim = (-10000, 10000))
+  axes = []
+  axes_text = []
+
+  # Setup position and velocity text fields
+  font = dict(fontsize = 14, family = 'monospace')
+
+  axes_text.append(plt.text(0,    11700, '', **font))
+  axes_text.append(plt.text(800,  11700, '', **font))
+  axes_text.append(plt.text(0,    10500, '', **font))
+  axes_text.append(plt.text(800,  10500, '', **font))
+  axes_text.append(plt.text(1500, 11700, '', **font))
+
+  # Create axes
+  for i in range(5):
+    axes.append(ax.plot([], [])[0])
+    # Set text color to match axis color
+    axes_text[i].set_color(axes[i].get_color())
+
+  # Initial text views
+  plot.update_text(axes_text, 0, [0] * 4)
+
+  # Set up animation
+  anim = animation.FuncAnimation(fig, plot.update, fargs = [axes, axes_text],
+                                 interval = 100)
+
+  # Run
+  plt.show()
+  plot.close()

src/avr/step-test/step-test.c

@@ -0,0 +1,230 @@
+/******************************************************************************\
+
+                 This file is part of the Buildbotics firmware.
+
+                   Copyright (c) 2015 - 2018, Buildbotics LLC
+                              All rights reserved.
+
+      This file ("the software") is free software: you can redistribute it
+      and/or modify it under the terms of the GNU General Public License,
+       version 2 as published by the Free Software Foundation. You should
+       have received a copy of the GNU General Public License, version 2
+      along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+      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
+                        <http://www.gnu.org/licenses/>.
+
+                 For information regarding this software email:
+                   "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "config.h"
+#include "hardware.h"
+#include "usart.h"
+#include "lcd.h"
+
+#include <avr/interrupt.h>
+
+#include <stdint.h>
+#include <stdio.h>
+
+
+#define RESET_PIN SPI_MOSI_PIN
+
+
+void rtc_init() {}
+
+
+static struct {
+  uint8_t step_pin;
+  uint8_t dir_pin;
+  TC0_t *timer;
+  volatile int16_t high;
+  volatile bool reading;
+
+} channel[4] = {
+  {STEP_X_PIN, DIR_X_PIN, &TCC0, 0},
+  {STEP_Y_PIN, DIR_Y_PIN, &TCD0, 0},
+  {STEP_Z_PIN, DIR_Z_PIN, &TCE0, 0},
+  {STEP_A_PIN, DIR_A_PIN, &TCF0, 0},
+};
+
+
+static int reset = 0;
+
+
+void channel_reset(int i) {channel[i].timer->CNT = channel[i].high = 0;}
+
+
+#define EVSYS_CHMUX(CH) (&EVSYS_CH0MUX)[CH]
+#define EVSYS_CHCTRL(CH) (&EVSYS_CH0CTRL)[CH]
+
+
+void channel_overflow(int i) {
+  if (IN_PIN(channel[i].dir_pin)) channel[i].high--;
+  else channel[i].high++;
+  channel[i].reading = false;
+}
+
+
+ISR(TCC0_OVF_vect) {channel_overflow(0);}
+ISR(TCD0_OVF_vect) {channel_overflow(1);}
+ISR(TCE0_OVF_vect) {channel_overflow(2);}
+ISR(TCF0_OVF_vect) {channel_overflow(3);}
+
+
+void channel_update_dir(int i) {
+  if (IN_PIN(channel[i].dir_pin)) channel[i].timer->CTRLFSET = TC0_DIR_bm;
+  else channel[i].timer->CTRLFCLR = TC0_DIR_bm;
+}
+
+
+ISR(PORTE_INT0_vect) {for (int i = 0; i < 4; i++) channel_update_dir(i);}
+
+
+int32_t __attribute__ ((noinline)) _channel_read(int i) {
+  return (int32_t)channel[i].high << 16 | channel[i].timer->CNT;
+}
+
+
+int32_t channel_read(int i) {
+  while (true) {
+    channel[i].reading = true;
+
+    int32_t x = _channel_read(i);
+    int32_t y = _channel_read(i);
+
+    if (x != y || !channel[i].reading) continue;
+
+    channel[i].reading = false;
+    return x;
+  }
+}
+
+
+void channel_update_enable(int i) {
+  if (IN_PIN(MOTOR_ENABLE_PIN))
+    channel[i].timer->CTRLA = TC_CLKSEL_EVCH0_gc + i;
+  else channel[i].timer->CTRLA = 0;
+}
+
+
+ISR(PORTF_INT0_vect) {
+  for (int i = 0; i < 4; i++) channel_update_enable(i);
+  if (!IN_PIN(MOTOR_ENABLE_PIN)) reset = 2;
+}
+
+
+ISR(PORTC_INT0_vect) {reset = 32;}
+
+
+ISR(TCC1_OVF_vect) {
+  if (reset) reset--;
+
+  // Report measured steps
+  static int32_t counts[4] = {0, 0, 0, 0};
+  bool moving = false;
+  bool zero = true;
+  for (int i = 0; i < 4; i++) {
+    int32_t count = channel_read(i);
+    if (count != counts[i]) moving = true;
+    if (count) zero = false;
+    counts[i] = count;
+  }
+
+  if (reset && !zero) {
+    for (int i = 0; i < 4; i++) channel_reset(i);
+    printf("RESET\n");
+    return;
+  }
+
+  if (moving)
+    printf("%ld,%ld,%ld,%ld\n", counts[0], counts[1], counts[2], counts[3]);
+}
+
+
+static void _splash(uint8_t addr) {
+  lcd_init(addr);
+  lcd_goto(addr, 5, 1);
+  lcd_pgmstr(addr, PSTR("Step Test"));
+}
+
+
+void channel_init(int i) {
+  uint8_t step_pin = channel[i].step_pin;
+  uint8_t dir_pin = channel[i].dir_pin;
+
+  // Configure I/O
+  DIRCLR_PIN(step_pin);
+  DIRCLR_PIN(dir_pin);
+  PINCTRL_PIN(step_pin) = PORT_SRLEN_bm | PORT_ISC_RISING_gc;
+  PINCTRL_PIN(dir_pin)  = PORT_SRLEN_bm | PORT_ISC_BOTHEDGES_gc;
+
+  // Dir change interrupt
+  PIN_PORT(dir_pin)->INTCTRL  |= PORT_INT0LVL_HI_gc;
+  PIN_PORT(dir_pin)->INT0MASK |= PIN_BM(dir_pin);
+
+  // Events
+  EVSYS_CHMUX(i) = PIN_EVSYS_CHMUX(step_pin);
+  EVSYS_CHCTRL(i) = EVSYS_DIGFILT_8SAMPLES_gc;
+
+  // Clock
+  channel_update_enable(i);
+  channel[i].timer->INTCTRLA = TC_OVFINTLVL_HI_gc;
+
+  // Set initial clock direction
+  channel_update_dir(i);
+}
+
+
+static void init() {
+  cli();
+
+  hw_init();
+  usart_init();
+
+  // Motor channels
+  for (int i = 0; i < 4; i++) channel_init(i);
+
+  // Motor enable
+  DIRCLR_PIN(MOTOR_ENABLE_PIN);
+  PINCTRL_PIN(MOTOR_ENABLE_PIN) =
+    PORT_SRLEN_bm | PORT_ISC_BOTHEDGES_gc | PORT_OPC_PULLUP_gc;
+  PIN_PORT(MOTOR_ENABLE_PIN)->INTCTRL  |= PORT_INT0LVL_HI_gc;
+  PIN_PORT(MOTOR_ENABLE_PIN)->INT0MASK |= PIN_BM(MOTOR_ENABLE_PIN);
+
+  // Configure report clock
+  TCC1.INTCTRLA = TC_OVFINTLVL_LO_gc;
+  TCC1.PER = F_CPU / 256 * 0.01; // 10ms
+  TCC1.CTRLA = TC_CLKSEL_DIV256_gc;
+
+  // Reset switch
+  DIRCLR_PIN(RESET_PIN);
+  PINCTRL_PIN(RESET_PIN) =
+    PORT_SRLEN_bm | PORT_ISC_RISING_gc | PORT_OPC_PULLUP_gc;
+  PIN_PORT(RESET_PIN)->INTCTRL  |= PORT_INT0LVL_LO_gc;
+  PIN_PORT(RESET_PIN)->INT0MASK |= PIN_BM(RESET_PIN);
+
+  printf("RESET\n");
+
+  sei();
+}
+
+
+int main() {
+  init();
+
+  _splash(0x27);
+  _splash(0x3f);
+
+  while (true) continue;
+
+  return 0;
+}