diff --git a/.gitmodules b/.gitmodules
index cfd187deaa33bef6903824d987cd63ba74ab17bc..0bb89b4c4cdf3b1c089eab64733f5bbe9fbf80f1 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -28,3 +28,6 @@
[submodule "firmware/axl-stepper/src/osape_arduino"]
path = firmware/axl-stepper/src/osape_arduino
url = https://github.com/jakeread/osape_arduino.git
+[submodule "firmware/axl-stepper/src/utils_samd51"]
+ path = firmware/axl-stepper/src/utils_samd51
+ url = https://github.com/jakeread/utils_samd51.git
diff --git a/firmware/axl-stepper/src/axl b/firmware/axl-stepper/src/axl
index 79ca7181653d7e8c567cf026e7cc882a5c410feb..c874b7afa9102ae3e5c150e444b01703c7143303 160000
--- a/firmware/axl-stepper/src/axl
+++ b/firmware/axl-stepper/src/axl
@@ -1 +1 @@
-Subproject commit 79ca7181653d7e8c567cf026e7cc882a5c410feb
+Subproject commit c874b7afa9102ae3e5c150e444b01703c7143303
diff --git a/firmware/axl-stepper/src/drivers/dacs.cpp b/firmware/axl-stepper/src/drivers/dacs.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b9dbf277cb341d4d1c36240456da9fd240eaa5c1
--- /dev/null
+++ b/firmware/axl-stepper/src/drivers/dacs.cpp
@@ -0,0 +1,122 @@
+/*
+osap/drivers/dacs.cpp
+
+dacs on the d51
+
+Jake Read at the Center for Bits and Atoms
+(c) Massachusetts Institute of Technology 2019
+
+This work may be reproduced, modified, distributed, performed, and
+displayed for any purpose, but must acknowledge the squidworks and ponyo
+projects. Copyright is retained and must be preserved. The work is provided as
+is; no warranty is provided, and users accept all liability.
+*/
+
+#include "dacs.h"
+//#include "ucbus_drop.h"
+
+DACs* DACs::instance = 0;
+
+DACs* DACs::getInstance(void){
+ if(instance == 0){
+ instance = new DACs();
+ }
+ return instance;
+}
+
+DACs* dacs = DACs::getInstance();
+
+DACs::DACs() {}
+
+void DACs::init(){
+ /*
+ // the below code was an attempt to scrape from
+ // scrape https://github.com/adafruit/ArduinoCore-samd/blob/master/cores/arduino/startup.c (clock)
+ // scrape https://github.com/adafruit/ArduinoCore-samd/blob/master/cores/arduino/wiring.c (peripheral clock)
+ // scrape https://github.com/adafruit/ArduinoCore-samd/blob/master/cores/arduino/wiring_analog.c
+ // to setup the DAC 'from scratch' - of course it occurred to me later that this
+ // setup already happens in arduino's boot. so I omitted this and just used
+ // the messy per-analogWrite-call config below, and wrote small write-to-dac functions
+ // to operate under the assumption that this init happens once.
+
+ // ...
+ // put the pins on the peripheral,
+ // DAC0 is PA02, Peripheral B
+ // DAC1 is PA05, Peripheral B
+ //PORT->Group[0].DIRSET.reg = (uint32_t)(1 << 2);
+ //PORT->Group[0].DIRCLR.reg = (uint32_t)(1 << 2);
+ PORT->Group[0].PINCFG[2].bit.PMUXEN = 1;
+ PORT->Group[0].PMUX[2 >> 1].reg |= PORT_PMUX_PMUXE(1);
+ //PORT->Group[0].DIRSET.reg = (uint32_t)(1 << 5);
+ //PORT->Group[0].DIRCLR.reg = (uint32_t)(1 << 5);
+ PORT->Group[0].PINCFG[5].bit.PMUXEN = 1;
+ PORT->Group[0].PMUX[5 >> 1].reg |= PORT_PMUX_PMUXO(1);
+
+ // unmask the DAC peripheral
+ MCLK->APBDMASK.bit.DAC_ = 1;
+
+ // DAC needs a clock,
+ GCLK->GENCTRL[GENERIC_CLOCK_GENERATOR_12M].reg = GCLK_GENCTRL_SRC_DFLL |
+ GCLK_GENCTRL_IDC |
+ GCLK_GENCTRL_DIV(4) |
+ GCLK_GENCTRL_GENEN;
+ while(GCLK->SYNCBUSY.reg & GENERIC_CLOCK_GENERATOR_12M_SYNC);
+ // feed that clock to the DAC,
+ GCLK->PCHCTRL[DAC_GCLK_ID].reg = GCLK_PCHCTRL_CHEN | GCLK_PCHCTRL_GEN(GENERIC_CLOCK_GENERATOR_12M_SYNC);
+ while(GCLK->PCHCTRL[DAC_GCLK_ID].bit.CHEN == 0);
+
+ // software reset the DAC
+ while(DAC->SYNCBUSY.bit.SWRST == 1);
+ DAC->CTRLA.bit.SWRST = 1;
+ while(DAC->SYNCBUSY.bit.SWRST == 1);
+ // and finally the DAC itself,
+ while(DAC->SYNCBUSY.bit.ENABLE || DAC->SYNCBUSY.bit.SWRST);
+ DAC->CTRLA.bit.ENABLE = 0;
+ // enable both channels
+ while(DAC->SYNCBUSY.bit.ENABLE || DAC->SYNCBUSY.bit.SWRST);
+ DAC->DACCTRL[0].reg = DAC_DACCTRL_ENABLE | DAC_DACCTRL_REFRESH(2);
+ while(DAC->SYNCBUSY.bit.ENABLE || DAC->SYNCBUSY.bit.SWRST);
+ DAC->DACCTRL[1].reg = DAC_DACCTRL_ENABLE | DAC_DACCTRL_REFRESH(2);
+ // voltage out, and select vref
+ DAC->CTRLB.reg = DAC_CTRLB_REFSEL_VDDANA;
+ // re-enable dac
+ while(DAC->SYNCBUSY.bit.ENABLE || DAC->SYNCBUSY.bit.SWRST);
+ DAC->CTRLA.bit.ENABLE = 1;
+ // await up,
+ while(!DAC->STATUS.bit.READY0);
+ while(!DAC->STATUS.bit.READY1);
+ */
+ while(DAC->SYNCBUSY.bit.ENABLE || DAC->SYNCBUSY.bit.SWRST);
+ DAC->CTRLA.bit.ENABLE = 0;
+ while(DAC->SYNCBUSY.bit.ENABLE || DAC->SYNCBUSY.bit.SWRST);
+ DAC->DACCTRL[0].bit.ENABLE = 1;
+ DAC->DACCTRL[1].bit.ENABLE = 1;
+ while(DAC->SYNCBUSY.bit.ENABLE || DAC->SYNCBUSY.bit.SWRST);
+ DAC->CTRLA.bit.ENABLE = 1;
+ while(!DAC->STATUS.bit.READY0);
+ while(!DAC->STATUS.bit.READY1);
+}
+
+// 0 - 4095
+void DACs::writeDac0(uint16_t val){
+ //analogWrite(A0, val);
+ while(DAC->SYNCBUSY.bit.DATA0);
+ DAC->DATA[0].reg = val;//DAC_DATA_DATA(val);
+ currentVal0 = val;
+}
+
+void DACs::writeDac1(uint16_t val){
+ //analogWrite(A1, val);
+ while(DAC->SYNCBUSY.bit.DATA1);
+ DAC->DATA[1].reg = val;//DAC_DATA_DATA(val);
+ currentVal1 = val;
+}
+
+void DACs::refresh(void){
+ writeDac0(currentVal0);
+ writeDac1(currentVal1);
+ uint32_t now = micros();
+ if(now > lastRefresh + 1000){
+ lastRefresh = now;
+ }
+}
diff --git a/firmware/axl-stepper/src/drivers/dacs.h b/firmware/axl-stepper/src/drivers/dacs.h
new file mode 100644
index 0000000000000000000000000000000000000000..9aba2d9a8a422560702ea4cc0dcecd697c0dae09
--- /dev/null
+++ b/firmware/axl-stepper/src/drivers/dacs.h
@@ -0,0 +1,54 @@
+/*
+osap/drivers/dacs.h
+
+dacs on the d51
+
+Jake Read at the Center for Bits and Atoms
+(c) Massachusetts Institute of Technology 2019
+
+This work may be reproduced, modified, distributed, performed, and
+displayed for any purpose, but must acknowledge the squidworks and ponyo
+projects. Copyright is retained and must be preserved. The work is provided as
+is; no warranty is provided, and users accept all liability.
+*/
+
+#ifndef DACS_H_
+#define DACS_H_
+
+#include <arduino.h>
+
+#include "indicators.h"
+
+// scrape https://github.com/adafruit/ArduinoCore-samd/blob/master/cores/arduino/wiring_analog.c
+// scrape https://github.com/adafruit/ArduinoCore-samd/blob/master/cores/arduino/startup.c (clock)
+// scrape https://github.com/adafruit/ArduinoCore-samd/blob/master/cores/arduino/wiring.c (peripheral clock)
+// DAC0 is on PA02
+// DAC1 is on PA05
+
+// NOTE: the DAC must be refreshed manually to maintain voltage.
+// there does appear to be a refresh register in DACCTRL band,
+// but it does *not* seem to work...
+
+#define GENERIC_CLOCK_GENERATOR_12M (4u)
+#define GENERIC_CLOCK_GENERATOR_12M_SYNC GCLK_SYNCBUSY_GENCTRL4
+
+class DACs {
+ private:
+ // is driver, is singleton,
+ static DACs* instance;
+ volatile uint16_t currentVal0 = 0;
+ volatile uint16_t currentVal1 = 0;
+ volatile uint32_t lastRefresh = 0;
+
+ public:
+ DACs();
+ static DACs* getInstance(void);
+ void init(void);
+ void writeDac0(uint16_t val);
+ void writeDac1(uint16_t val);
+ void refresh(void);
+};
+
+extern DACs* dacs;
+
+#endif
\ No newline at end of file
diff --git a/firmware/axl-stepper/src/drivers/peripheral_nums.h b/firmware/axl-stepper/src/drivers/peripheral_nums.h
new file mode 100644
index 0000000000000000000000000000000000000000..eed9f188afacfb0da271d43603f833f61ec61191
--- /dev/null
+++ b/firmware/axl-stepper/src/drivers/peripheral_nums.h
@@ -0,0 +1,18 @@
+#ifndef PERIPHERAL_NUMS_H_
+#define PERIPHERAL_NUMS_H_
+
+#define PERIPHERAL_A 0
+#define PERIPHERAL_B 1
+#define PERIPHERAL_C 2
+#define PERIPHERAL_D 3
+#define PERIPHERAL_E 4
+#define PERIPHERAL_F 5
+#define PERIPHERAL_G 6
+#define PERIPHERAL_H 7
+#define PERIPHERAL_I 8
+#define PERIPHERAL_K 9
+#define PERIPHERAL_L 10
+#define PERIPHERAL_M 11
+#define PERIPHERAL_N 12
+
+#endif
\ No newline at end of file
diff --git a/firmware/axl-stepper/src/drivers/step_a4950.cpp b/firmware/axl-stepper/src/drivers/step_a4950.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..440e5a8678dd941ce5280e5a8104661e94c09d7b
--- /dev/null
+++ b/firmware/axl-stepper/src/drivers/step_a4950.cpp
@@ -0,0 +1,214 @@
+/*
+osap/drivers/step_a4950.cpp
+
+stepper code for two A4950s
+
+Jake Read at the Center for Bits and Atoms
+(c) Massachusetts Institute of Technology 2019
+
+This work may be reproduced, modified, distributed, performed, and
+displayed for any purpose, but must acknowledge the squidworks and ponyo
+projects. Copyright is retained and must be preserved. The work is provided as
+is; no warranty is provided, and users accept all liability.
+*/
+
+#include "step_a4950.h"
+//#include "ucbus_drop.h"
+
+// sine, 0-8190, 4095 center / 'zero', 256 steps
+uint16_t LUT_8190[256] = {
+ 4095,4195,4296,4396,4496,4596,4696,4795,4894,4992,
+ 5090,5187,5284,5380,5475,5569,5662,5754,5846,5936,
+ 6025,6113,6200,6286,6370,6453,6534,6614,6693,6770,
+ 6845,6919,6991,7061,7129,7196,7260,7323,7384,7443,
+ 7500,7555,7607,7658,7706,7753,7797,7839,7878,7916,
+ 7951,7983,8014,8042,8067,8091,8111,8130,8146,8159,
+ 8170,8179,8185,8189,8190,8189,8185,8179,8170,8159,
+ 8146,8130,8111,8091,8067,8042,8014,7983,7951,7916,
+ 7878,7839,7797,7753,7706,7658,7607,7555,7500,7443,
+ 7384,7323,7260,7196,7129,7061,6991,6919,6845,6770,
+ 6693,6614,6534,6453,6370,6286,6200,6113,6025,5936,
+ 5846,5754,5662,5569,5475,5380,5284,5187,5090,4992,
+ 4894,4795,4696,4596,4496,4396,4296,4195,4095,3995,
+ 3894,3794,3694,3594,3494,3395,3296,3198,3100,3003,
+ 2906,2810,2715,2621,2528,2436,2344,2254,2165,2077,
+ 1990,1904,1820,1737,1656,1576,1497,1420,1345,1271,
+ 1199,1129,1061,994,930,867,806,747,690,635,
+ 583,532,484,437,393,351,312,274,239,207,
+ 176,148,123,99,79,60,44,31,20,11,
+ 5,1,0,1,5,11,20,31,44,60,
+ 79,99,123,148,176,207,239,274,312,351,
+ 393,437,484,532,583,635,690,747,806,867,
+ 930,994,1061,1129,1199,1271,1345,1420,1497,1576,
+ 1656,1737,1820,1904,1990,2077,2165,2254,2344,2436,
+ 2528,2621,2715,2810,2906,3003,3100,3198,3296,3395,
+ 3494,3594,3694,3794,3894,3995
+};
+
+// sine, 0-1022 (511 center / 'zero'), 256 steps
+uint16_t LUT_1022[256] = {
+ 511,524,536,549,561,574,586,598,611,623,635,647,659,671,683,695,
+ 707,718,729,741,752,763,774,784,795,805,815,825,835,845,854,863,
+ 872,881,890,898,906,914,921,929,936,943,949,956,962,967,973,978,
+ 983,988,992,996,1000,1003,1007,1010,1012,1014,1016,1018,1020,1021,1021,1022,
+ 1022,1022,1021,1021,1020,1018,1016,1014,1012,1010,1007,1003,1000,996,992,988,
+ 983,978,973,967,962,956,949,943,936,929,921,914,906,898,890,881,
+ 872,863,854,845,835,825,815,805,795,784,774,763,752,741,729,718,
+ 707,695,683,671,659,647,635,623,611,598,586,574,561,549,536,524,
+ 511,498,486,473,461,448,436,424,411,399,387,375,363,351,339,327,
+ 315,304,293,281,270,259,248,238,227,217,207,197,187,177,168,159,
+ 150,141,132,124,116,108,101,93,86,79,73,66,60,55,49,44,
+ 39,34,30,26,22,19,15,12,10,8,6,4,2,1,1,0,
+ 0,0,1,1,2,4,6,8,10,12,15,19,22,26,30,34,
+ 39,44,49,55,60,66,73,79,86,93,101,108,116,124,132,141,
+ 150,159,168,177,187,197,207,217,227,238,248,259,270,281,293,304,
+ 315,327,339,351,363,375,387,399,411,424,436,448,461,473,486,498,
+};
+
+uint16_t dacLUT[256];
+
+STEP_A4950* STEP_A4950::instance = 0;
+
+STEP_A4950* STEP_A4950::getInstance(void){
+ if(instance == 0){
+ instance = new STEP_A4950();
+ }
+ return instance;
+}
+
+STEP_A4950* stepper_hw = STEP_A4950::getInstance();
+
+STEP_A4950::STEP_A4950() {}
+
+void STEP_A4950::init(boolean invert, float cscale){
+ // all of 'em, outputs
+ AIN1_PORT.DIRSET.reg = AIN1_BM;
+ AIN2_PORT.DIRSET.reg = AIN2_BM;
+ BIN1_PORT.DIRSET.reg = BIN1_BM;
+ BIN2_PORT.DIRSET.reg = BIN2_BM;
+ // floating cscale
+ if(cscale < 0){
+ _cscale = 0;
+ } else if (cscale > 1){
+ _cscale = 1;
+ } else {
+ _cscale = cscale;
+ }
+ // write a rectified LUT for writing to DACs
+ for(uint16_t i = 0; i < 256; i ++){
+ if(LUT_8190[i] > 4095){
+ dacLUT[i] = LUT_8190[i] - 4095;
+ } else if (LUT_8190[i] < 4095){
+ dacLUT[i] = abs(4095 - LUT_8190[i]);
+ } else {
+ dacLUT[i] = 0;
+ }
+ }
+ // invert direction / not
+ _dir_invert = invert;
+ // start the DAAAC
+ dacs->init();
+ // start condition,
+ step();
+}
+
+// sequence like
+// S: 1 2 3 4 5 6 7 8
+// A: ^ ^ ^ x v v v x
+// B: ^ x v v v x ^ ^
+void STEP_A4950::step(void){
+ // increment: wrapping comes for free with uint8_t, bless
+ if(_dir){
+ if(_dir_invert){
+ _aStep -= _microstep_count;
+ _bStep -= _microstep_count;
+ } else {
+ _aStep += _microstep_count;
+ _bStep += _microstep_count;
+ }
+ } else {
+ if(_dir_invert){
+ _aStep += _microstep_count;
+ _bStep += _microstep_count;
+ } else {
+ _aStep -= _microstep_count;
+ _bStep -= _microstep_count;
+ }
+ }
+ // a phase,
+ if(LUT_8190[_aStep] > 4095){
+ A_UP;
+ } else if (LUT_8190[_aStep] < 4095){
+ A_DOWN;
+ } else {
+ A_OFF;
+ }
+ // a DAC
+ // so that we can easily rewrite currents on the fly. will extend to servoing, yeah
+ dacs->writeDac0(dacLUT[_aStep] * _cscale);
+ // b phase,
+ if(LUT_8190[_bStep] > 4095){
+ B_UP;
+ } else if (LUT_8190[_bStep] < 4095){
+ B_DOWN;
+ } else {
+ B_OFF;
+ }
+ // b DAC
+ dacs->writeDac1(dacLUT[_bStep] * _cscale);
+}
+
+void STEP_A4950::dir(boolean val){
+ _dir = val;
+}
+
+boolean STEP_A4950::getDir(void){
+ return _dir;
+}
+
+void STEP_A4950::setMicrostep(uint8_t microstep){
+ switch(microstep){
+ case 64:
+ _microstep_count = MICROSTEP_64_COUNT;
+ break;
+ case 32:
+ _microstep_count = MICROSTEP_32_COUNT;
+ break;
+ case 16:
+ _microstep_count = MICROSTEP_16_COUNT;
+ break;
+ case 8:
+ _microstep_count = MICROSTEP_8_COUNT;
+ break;
+ case 4:
+ _microstep_count = MICROSTEP_4_COUNT;
+ break;
+ case 1:
+ _microstep_count = MICROSTEP_1_COUNT;
+ break;
+ default:
+ _microstep_count = MICROSTEP_1_COUNT;
+ break;
+ }
+}
+
+void STEP_A4950::setCurrent(float cscale){
+ if(cscale > 1){
+ _cscale = 1;
+ } else if(cscale < 0){
+ _cscale = 0;
+ } else {
+ _cscale = cscale;
+ }
+ // do DAC re-writes
+ dacs->writeDac0(dacLUT[_aStep] * _cscale);
+ dacs->writeDac1(dacLUT[_bStep] * _cscale);
+}
+
+void STEP_A4950::setInversion(boolean inv){
+ _dir_invert = inv;
+}
+
+void STEP_A4950::dacRefresh(void){
+ dacs->refresh();
+}
diff --git a/firmware/axl-stepper/src/drivers/step_a4950.h b/firmware/axl-stepper/src/drivers/step_a4950.h
new file mode 100644
index 0000000000000000000000000000000000000000..fb19f2f12c2d651dbf52826d16f6f3fb311b4b9c
--- /dev/null
+++ b/firmware/axl-stepper/src/drivers/step_a4950.h
@@ -0,0 +1,114 @@
+/*
+osap/drivers/step_a4950.h
+
+stepper code for two A4950s
+
+Jake Read at the Center for Bits and Atoms
+(c) Massachusetts Institute of Technology 2019
+
+This work may be reproduced, modified, distributed, performed, and
+displayed for any purpose, but must acknowledge the squidworks and ponyo
+projects. Copyright is retained and must be preserved. The work is provided as
+is; no warranty is provided, and users accept all liability.
+*/
+
+#ifndef STEP_A4950_H_
+#define STEP_A4950_H_
+
+#include <Arduino.h>
+
+#include "dacs.h"
+#include "indicators.h"
+
+// C_SCALE
+// 1: DACs go 0->512 (of 4096, peak current is 1.6A at 4096): 0.2A
+// 2: DACs go 0->1024,
+// ...
+// 8: DACs go full width
+//#define C_SCALE 8 // on init
+// MICROSTEP_COUNT
+// 1: do 1 tick of 256 table, for full resolution, this is 64 'microsteps'
+// 2: 32 microsteps
+// 4: 16 microsteps
+// 8: 8 microsteps
+// 16: 4 microsteps
+// 32: 2 microsteps (half steps)
+// 64: full steps
+#define MICROSTEP_COUNT 1
+
+#define MICROSTEP_64_COUNT 1
+#define MICROSTEP_32_COUNT 2
+#define MICROSTEP_16_COUNT 4
+#define MICROSTEP_8_COUNT 8
+#define MICROSTEP_4_COUNT 16
+#define MICROSTEP_2_COUNT 32
+#define MICROSTEP_1_COUNT 64
+
+// AIN1 PB06
+// AIN2 PA04
+// BIN1 PA07
+// BIN2 PA06
+#define AIN1_PIN 6
+#define AIN1_PORT PORT->Group[1]
+#define AIN1_BM (uint32_t)(1 << AIN1_PIN)
+#define AIN2_PIN 4
+#define AIN2_PORT PORT->Group[0]
+#define AIN2_BM (uint32_t)(1 << AIN2_PIN)
+#define BIN1_PIN 7
+#define BIN1_PORT PORT->Group[0]
+#define BIN1_BM (uint32_t)(1 << BIN1_PIN)
+#define BIN2_PIN 6
+#define BIN2_PORT PORT->Group[0]
+#define BIN2_BM (uint32_t)(1 << BIN2_PIN)
+
+// handles
+#define AIN1_HI AIN1_PORT.OUTSET.reg = AIN1_BM
+#define AIN1_LO AIN1_PORT.OUTCLR.reg = AIN1_BM
+#define AIN2_HI AIN2_PORT.OUTSET.reg = AIN2_BM
+#define AIN2_LO AIN2_PORT.OUTCLR.reg = AIN2_BM
+#define BIN1_HI BIN1_PORT.OUTSET.reg = BIN1_BM
+#define BIN1_LO BIN1_PORT.OUTCLR.reg = BIN1_BM
+#define BIN2_HI BIN2_PORT.OUTSET.reg = BIN2_BM
+#define BIN2_LO BIN2_PORT.OUTCLR.reg = BIN2_BM
+
+// set a phase up or down direction
+// transition low first, avoid brake condition for however many ns
+#define A_UP AIN2_LO; AIN1_HI
+#define A_OFF AIN2_LO; AIN1_LO
+#define A_DOWN AIN1_LO; AIN2_HI
+#define B_UP BIN2_LO; BIN1_HI
+#define B_OFF BIN2_LO; BIN1_LO
+#define B_DOWN BIN1_LO; BIN2_HI
+
+class STEP_A4950 {
+ private:
+ // is driver, is singleton,
+ static STEP_A4950* instance;
+ volatile uint8_t _aStep = 0; // 0 of 256 micros,
+ volatile uint8_t _bStep = 63; // of the same table, startup 90' out of phase
+ volatile boolean _dir = false;
+ boolean _dir_invert = false;
+ uint8_t _microstep_count = 1;
+ // try single scalar
+ float _cscale = 0.1;
+
+ public:
+ STEP_A4950();
+ static STEP_A4950* getInstance(void);
+ // do like
+ void init(boolean invert, float cscale);
+ void step(void);
+ void dir(boolean val);
+ boolean getDir(void);
+ // microstep setting
+ void setMicrostep(uint8_t microstep);
+ // current settings
+ void setCurrent(float cscale);
+ void setInversion(boolean inv);
+ // for the dacs
+ void dacRefresh(void);
+};
+
+extern STEP_A4950* stepper_hw;
+
+#endif
\ No newline at end of file
diff --git a/firmware/axl-stepper/src/indicators.h b/firmware/axl-stepper/src/indicators.h
new file mode 100644
index 0000000000000000000000000000000000000000..5d7452112ea3debaa5eb0b8128a73a66e150f953
--- /dev/null
+++ b/firmware/axl-stepper/src/indicators.h
@@ -0,0 +1,59 @@
+// indicators for the macrofab-d
+#define CLKLIGHT_PIN 27
+#define CLKLIGHT_PORT PORT->Group[0]
+#define ERRLIGHT_PIN 8
+#define ERRLIGHT_PORT PORT->Group[1]
+
+// PB05, is DIP1
+#define DEBUG1PIN_PIN 5
+#define DEBUG1PIN_PORT PORT->Group[1]
+// PA23, is limit
+//#define DEBUG1PIN_PIN 23
+//#define DEBUG1PIN_PORT PORT->Group[0]
+// PB04, is DIP2
+#define DEBUG2PIN_PIN 4
+#define DEBUG2PIN_PORT PORT->Group[1]
+// NOT setup
+#define DEBUG3PIN_PIN 13
+#define DEBUG3PIN_PORT PORT->Group[1]
+#define DEBUG4PIN_PIN 14
+#define DEBUG4PIN_PORT PORT->Group[1]
+
+// PA27
+#define CLKLIGHT_BM (uint32_t)(1 << CLKLIGHT_PIN)
+#define CLKLIGHT_ON CLKLIGHT_PORT.OUTCLR.reg = CLKLIGHT_BM
+#define CLKLIGHT_OFF CLKLIGHT_PORT.OUTSET.reg = CLKLIGHT_BM
+#define CLKLIGHT_TOGGLE CLKLIGHT_PORT.OUTTGL.reg = CLKLIGHT_BM
+#define CLKLIGHT_SETUP CLKLIGHT_PORT.DIRSET.reg = CLKLIGHT_BM; CLKLIGHT_OFF
+
+// PB08
+#define ERRLIGHT_BM (uint32_t)(1 << ERRLIGHT_PIN)
+#define ERRLIGHT_ON ERRLIGHT_PORT.OUTCLR.reg = ERRLIGHT_BM
+#define ERRLIGHT_OFF ERRLIGHT_PORT.OUTSET.reg = ERRLIGHT_BM
+#define ERRLIGHT_TOGGLE ERRLIGHT_PORT.OUTTGL.reg = ERRLIGHT_BM
+#define ERRLIGHT_SETUP ERRLIGHT_PORT.DIRSET.reg = ERRLIGHT_BM; ERRLIGHT_OFF
+
+// the limit: turn off as input if using as output
+#define DEBUG1PIN_BM (uint32_t)(1 << DEBUG1PIN_PIN)
+#define DEBUG1PIN_ON DEBUG1PIN_PORT.OUTSET.reg = DEBUG1PIN_BM
+#define DEBUG1PIN_OFF DEBUG1PIN_PORT.OUTCLR.reg = DEBUG1PIN_BM
+#define DEBUG1PIN_TOGGLE DEBUG1PIN_PORT.OUTTGL.reg = DEBUG1PIN_BM
+#define DEBUG1PIN_SETUP DEBUG1PIN_PORT.DIRSET.reg = DEBUG1PIN_BM; DEBUG1PIN_OFF
+
+#define DEBUG2PIN_BM (uint32_t)(1 << DEBUG2PIN_PIN)
+#define DEBUG2PIN_ON DEBUG2PIN_PORT.OUTSET.reg = DEBUG2PIN_BM
+#define DEBUG2PIN_OFF DEBUG2PIN_PORT.OUTCLR.reg = DEBUG2PIN_BM
+#define DEBUG2PIN_TOGGLE DEBUG2PIN_PORT.OUTTGL.reg = DEBUG2PIN_BM
+#define DEBUG2PIN_SETUP DEBUG2PIN_PORT.DIRSET.reg = DEBUG2PIN_BM; DEBUG2PIN_OFF
+
+#define DEBUG3PIN_BM (uint32_t)(1 << DEBUG3PIN_PIN)
+#define DEBUG3PIN_ON DEBUG3PIN_PORT.OUTSET.reg = DEBUG3PIN_BM
+#define DEBUG3PIN_OFF DEBUG3PIN_PORT.OUTCLR.reg = DEBUG3PIN_BM
+#define DEBUG3PIN_TOGGLE DEBUG3PIN_PORT.OUTTGL.reg = DEBUG3PIN_BM
+#define DEBUG3PIN_SETUP DEBUG3PIN_PORT.DIRSET.reg = DEBUG3PIN_BM; DEBUG3PIN_OFF
+
+#define DEBUG4PIN_BM (uint32_t)(1 << DEBUG4PIN_PIN)
+#define DEBUG4PIN_ON DEBUG4PIN_PORT.OUTSET.reg = DEBUG4PIN_BM
+#define DEBUG4PIN_OFF DEBUG4PIN_PORT.OUTCLR.reg = DEBUG4PIN_BM
+#define DEBUG4PIN_TOGGLE DEBUG4PIN_PORT.OUTTGL.reg = DEBUG4PIN_BM
+#define DEBUG4PIN_SETUP DEBUG4PIN_PORT.DIRSET.reg = DEBUG4PIN_BM; DEBUG4PIN_OFF
\ No newline at end of file
diff --git a/firmware/axl-stepper/src/main.cpp b/firmware/axl-stepper/src/main.cpp
index 58b344c5757478d990bb23dd41243135880fb2f5..460f8bac738574457eb4ba0a7b4fb85dfb61d80f 100644
--- a/firmware/axl-stepper/src/main.cpp
+++ b/firmware/axl-stepper/src/main.cpp
@@ -1,9 +1,135 @@
#include <Arduino.h>
+#include "indicators.h"
+
+#include "drivers/step_a4950.h"
+#include "axl/axl.h"
+#include "axl/axl_config.h"
+#include "utils_samd51/clock_utils.h"
+
+#include "osape/core/osap.h"
+#include "osape/core/ts.h"
+#include "osape/vertices/endpoint.h"
+#include "osape_arduino/vp_arduinoSerial.h"
+
+OSAP osap("hmc-motor");
+
+VPort_ArduinoSerial vpUSBSerial(&osap, "arduinoUSBSerial", &Serial);
+
+// -------------------------------------------------------- 2: States
+
+EP_ONDATA_RESPONSES onStateData(uint8_t* data, uint16_t len){
+ // we have accel, rate, posn data,
+ dofs targ;
+ uint16_t rptr = 0;
+ uint8_t mode = data[rptr];
+ if(mode == AXL_MODE_ACCEL){
+ // read from 1st block of ndof
+ /*
+ rptr = 1;
+ for(uint8_t a = 0; a < AXL_NUM_DOF; a ++){
+ targ.axis[a] = ts_readFloat32(data, &rptr);
+ }
+ axl_setAccelTarget(targ);
+ */
+ } else if (mode == AXL_MODE_VELOCITY){
+ //axl_setVelocityTarget(rate.f);
+ rptr = 1 + AXL_NUM_DOF * 4 * 1;
+ for(uint8_t a = 0; a < AXL_NUM_DOF; a ++){
+ targ.axis[a] = ts_readFloat32(data, &rptr);
+ }
+ axl_setVelocityTarget(targ);
+ } else if (mode == AXL_MODE_POSITION){
+ rptr = 1 + AXL_NUM_DOF * 4 * 2;
+ for(uint8_t a = 0; a < AXL_NUM_DOF; a ++){
+ targ.axis[a] = ts_readFloat32(data, &rptr);
+ }
+ axl_setPositionTarget(targ);
+ } else {
+ // no / bad mode set, noop
+ OSAP::error("bad set-states request to axl, no / bad mode info", MEDIUM);
+ }
+ // chunk_float32 acc = { .bytes = { data[0], data[1], data[2], data[3] } };
+ // chunk_float32 rate = { .bytes = { data[4], data[5], data[6], data[7] } };
+ // chunk_float32 posn = { .bytes = { data[8], data[9], data[10], data[11] } };
+ // we 'reject' this i.e. we don't write these data into the endpoint,
+ // since we routinely update it w/ actual states (not requests)
+ return EP_ONDATA_REJECT;
+}
+
+Endpoint statesEP(&osap, "states", onStateData);
+
+void updateStatesEP(void){
+ uint8_t numBytes = AXL_NUM_DOF * 4 * 3 + 1;
+ uint8_t stash[numBytes]; uint16_t wptr = 0;
+ stash[wptr ++] = axl_getMode();
+ dofs temp = axl_getPositions();
+ for(uint8_t a = 0; a < AXL_NUM_DOF; a ++){
+ ts_writeFloat32(temp.axis[a], stash, &wptr);
+ }
+ temp = axl_getVelocities();
+ for(uint8_t a = 0; a < AXL_NUM_DOF; a ++){
+ ts_writeFloat32(temp.axis[a], stash, &wptr);
+ }
+ temp = axl_getAccelerations();
+ for(uint8_t a = 0; a < AXL_NUM_DOF; a ++){
+ ts_writeFloat32(temp.axis[a], stash, &wptr);
+ }
+ statesEP.write(stash, numBytes);
+}
+
void setup() {
- // put your setup code here, to run once:
+ CLKLIGHT_SETUP;
+ ERRLIGHT_SETUP;
+ DEBUG1PIN_SETUP;
+ DEBUG2PIN_SETUP;
+ // port begin
+ vpUSBSerial.begin();
+ // setup stepper machine
+ stepper_hw->init(false, 0.35F);
+ stepper_hw->setMicrostep(4);
+ // setup controller
+ axl_setup();
+ // ticker begin:
+ d51ClockUtils->start_ticker_a(AXL_TICKER_INTERVAL_US);
}
+uint32_t lastBlink = 0;
+uint32_t blinkInterval = 50; // ms
+
void loop() {
- // put your main code here, to run repeatedly:
+ osap.loop();
+ stepper_hw->dacRefresh();
+ if(lastBlink + blinkInterval < millis()){
+ lastBlink = millis();
+ CLKLIGHT_TOGGLE;
+ updateStatesEP();
+ }
+}
+
+uint8_t axisPick = 0;
+float spu = 100.0F;
+
+volatile float stepRatchet = 0.0F;
+void axl_onPositionDelta(uint8_t axis, float delta){
+ if(axis != axisPick) return;
+ stepRatchet += delta * spu;
+ if(stepRatchet >= 1.0F){
+ stepper_hw->dir(true);
+ stepper_hw->step();
+ stepRatchet -= 1.0F;
+ } else if (stepRatchet <= -1.0F){
+ stepper_hw->dir(false);
+ stepper_hw->step();
+ stepRatchet += 1.0F;
+ }
+}
+
+void TC0_Handler(void){
+ DEBUG1PIN_ON;
+ TC0->COUNT32.INTFLAG.bit.MC0 = 1;
+ TC0->COUNT32.INTFLAG.bit.MC1 = 1;
+ // run the loop,
+ axl_integrator();
+ DEBUG1PIN_OFF;
}
\ No newline at end of file
diff --git a/firmware/axl-stepper/src/osap_config.h b/firmware/axl-stepper/src/osap_config.h
new file mode 100644
index 0000000000000000000000000000000000000000..a0efe77035e1e3e5e37a72cd5e7d9bdc6d129dc8
--- /dev/null
+++ b/firmware/axl-stepper/src/osap_config.h
@@ -0,0 +1,31 @@
+/*
+osap_config.h
+
+config options for an osap-embedded build
+
+Jake Read at the Center for Bits and Atoms
+(c) Massachusetts Institute of Technology 2022
+
+This work may be reproduced, modified, distributed, performed, and
+displayed for any purpose, but must acknowledge the osap project.
+Copyright is retained and must be preserved. The work is provided as is;
+no warranty is provided, and users accept all liability.
+*/
+
+#ifndef OSAP_CONFIG_H_
+#define OSAP_CONFIG_H_
+
+// size of vertex stacks, lenght, then count,
+#define VT_SLOTSIZE 256
+#define VT_STACKSIZE 3 // must be >= 2 for ringbuffer operation
+#define VT_MAXCHILDREN 16
+#define VT_MAXITEMSPERTURN 8
+
+// max # of endpoints that could be spawned here,
+#define MAX_CONTEXT_ENDPOINTS 64
+
+// count of routes each endpoint can have,
+#define ENDPOINT_MAX_ROUTES 4
+#define ENDPOINT_ROUTE_MAX_LEN 64
+
+#endif
\ No newline at end of file
diff --git a/firmware/axl-stepper/src/osape b/firmware/axl-stepper/src/osape
index d413d255cdccd7b4ae9f3467ab2c2a9a84b9abb8..4f5c37e60669b7357b29cba0b842014cdda23161 160000
--- a/firmware/axl-stepper/src/osape
+++ b/firmware/axl-stepper/src/osape
@@ -1 +1 @@
-Subproject commit d413d255cdccd7b4ae9f3467ab2c2a9a84b9abb8
+Subproject commit 4f5c37e60669b7357b29cba0b842014cdda23161
diff --git a/firmware/axl-stepper/src/ucbus_config.h b/firmware/axl-stepper/src/ucbus_config.h
new file mode 100644
index 0000000000000000000000000000000000000000..2b84fba6edf04a4d5ad1fc9fd5a11bfee67d08f6
--- /dev/null
+++ b/firmware/axl-stepper/src/ucbus_config.h
@@ -0,0 +1,29 @@
+/*
+ucbus_confi.h
+
+config options for an ucbus instance
+
+Jake Read at the Center for Bits and Atoms
+(c) Massachusetts Institute of Technology 2022
+
+This work may be reproduced, modified, distributed, performed, and
+displayed for any purpose, but must acknowledge the osap project.
+Copyright is retained and must be preserved. The work is provided as is;
+no warranty is provided, and users accept all liability.
+*/
+
+#ifndef UCBUS_CONFIG_H_
+#define UCBUS_CONFIG_H_
+
+#define UCBUS_MAX_DROPS 32
+//#define UCBUS_IS_DROP
+#define UCBUS_IS_HEAD
+
+#define UCBUS_BAUD 2
+
+#define UCBUS_IS_D51
+// #define UCBUS_IS_D21
+
+#define UCBUS_ON_OSAP
+
+#endif
\ No newline at end of file
diff --git a/firmware/axl-stepper/src/utils_samd51 b/firmware/axl-stepper/src/utils_samd51
new file mode 160000
index 0000000000000000000000000000000000000000..d77c3a9993a908c52ef53efd69d407be3c288e7e
--- /dev/null
+++ b/firmware/axl-stepper/src/utils_samd51
@@ -0,0 +1 @@
+Subproject commit d77c3a9993a908c52ef53efd69d407be3c288e7e