From eed7f8f748add5f5a373b2160ca4cfadaa558d5d Mon Sep 17 00:00:00 2001 From: Jake Read <jake.read@cba.mit.edu> Date: Mon, 9 Aug 2021 12:38:17 -0400 Subject: [PATCH] bring firmware to this repo, motor FW at haystack state, but new bus delineation scheme is currently half-devd --- .gitmodules | 3 + 2020-06_ucbus-stepper-melted/eagle.epf | 9 +- firmware/cl-step-controller/src/main.cpp | 1 - firmware/stepper-drop/.gitignore | 5 + firmware/stepper-drop/.vscode/extensions.json | 7 + firmware/stepper-drop/include/README | 39 ++ firmware/stepper-drop/lib/README | 46 +++ firmware/stepper-drop/platformio.ini | 14 + firmware/stepper-drop/src/config.h | 12 + firmware/stepper-drop/src/drivers/dacs.cpp | 122 ++++++ firmware/stepper-drop/src/drivers/dacs.h | 55 +++ .../src/drivers/peripheral_nums.h | 18 + .../stepper-drop/src/drivers/step_a4950.cpp | 214 +++++++++++ .../stepper-drop/src/drivers/step_a4950.h | 113 ++++++ firmware/stepper-drop/src/indicators.h | 53 +++ firmware/stepper-drop/src/main.cpp | 361 ++++++++++++++++++ firmware/stepper-drop/src/osape-d51 | 1 + firmware/stepper-drop/src/syserror.cpp | 93 +++++ firmware/stepper-drop/src/syserror.h | 10 + firmware/stepper-drop/test/README | 11 + 20 files changed, 1181 insertions(+), 6 deletions(-) create mode 100644 firmware/stepper-drop/.gitignore create mode 100644 firmware/stepper-drop/.vscode/extensions.json create mode 100644 firmware/stepper-drop/include/README create mode 100644 firmware/stepper-drop/lib/README create mode 100644 firmware/stepper-drop/platformio.ini create mode 100644 firmware/stepper-drop/src/config.h create mode 100644 firmware/stepper-drop/src/drivers/dacs.cpp create mode 100644 firmware/stepper-drop/src/drivers/dacs.h create mode 100644 firmware/stepper-drop/src/drivers/peripheral_nums.h create mode 100644 firmware/stepper-drop/src/drivers/step_a4950.cpp create mode 100644 firmware/stepper-drop/src/drivers/step_a4950.h create mode 100644 firmware/stepper-drop/src/indicators.h create mode 100644 firmware/stepper-drop/src/main.cpp create mode 160000 firmware/stepper-drop/src/osape-d51 create mode 100644 firmware/stepper-drop/src/syserror.cpp create mode 100644 firmware/stepper-drop/src/syserror.h create mode 100644 firmware/stepper-drop/test/README diff --git a/.gitmodules b/.gitmodules index 66af72e..2e8fa46 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,3 +7,6 @@ [submodule "firmware/osape-smoothieroll-drop-stepper/src/osape-d51"] path = firmware/osape-smoothieroll-drop-stepper/src/osape-d51 url = ssh://git@gitlab.cba.mit.edu:846/jakeread/osape-d51.git +[submodule "firmware/stepper-drop/src/osape-d51"] + path = firmware/stepper-drop/src/osape-d51 + url = ssh://git@gitlab.cba.mit.edu:846/jakeread/osape-d51.git diff --git a/2020-06_ucbus-stepper-melted/eagle.epf b/2020-06_ucbus-stepper-melted/eagle.epf index fa1582d..ddffdca 100644 --- a/2020-06_ucbus-stepper-melted/eagle.epf +++ b/2020-06_ucbus-stepper-melted/eagle.epf @@ -266,7 +266,6 @@ UsedLibraryUrn="urn:adsk.eagle:library:197" UsedLibraryUrn="urn:adsk.eagle:library:198" UsedLibraryUrn="urn:adsk.eagle:library:199" UsedLibrary="C:/Dropbox/CBA/circuits/eagle/parts/810-22-003-40-005191.lbr" -UsedLibrary="C:/Dropbox/CBA/circuits/eagle/parts/MAX98306ETD_.lbr" UsedLibrary="C:/Dropbox/CBA/circuits/eagle/parts/comm.lbr" UsedLibrary="C:/Dropbox/CBA/circuits/eagle/parts/connector.lbr" UsedLibrary="C:/Dropbox/CBA/circuits/eagle/parts/dfet.lbr" @@ -287,7 +286,7 @@ UsedLibrary="C:/Dropbox/CBA/circuits/eagle/parts/usbraw.lbr" Type="Schematic Editor" Number=1 File="2020-06_ucbus-stepper-melted.sch" -View="142.238 57.5843 328.514 158.882" +View="142.399 22.4818 328.676 123.78" WireWidths=" 0.0762 0.1016 0.127 0.15 0.2 0.2032 0.254 0.3048 0.4064 0.508 0.6096 0.8128 1.016 1.27 2.54 0.1524" PadDiameters=" 0.254 0.3048 0.4064 0.6096 0.8128 1.016 1.27 1.4224 1.6764 1.778 1.9304 2.1844 2.54 3.81 6.4516 0" PadDrills=" 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.65 0.7 0.75 0.8 0.85 0.9 1 0.6" @@ -326,14 +325,14 @@ ArcDirection=0 AddLevel=2 PadsSameType=0 Layer=91 -Views=" 1: 142.238 57.5843 328.514 158.882" +Views=" 1: 142.399 22.4818 328.676 123.78" Sheet="1" [Win_2] Type="Board Editor" Number=2 File="2020-06_ucbus-stepper-melted.brd" -View="6.486 13.9492 31.1792 40.1548" +View="8.9747 13.925 33.6679 40.1306" WireWidths=" 0.8 0.9 0.1 0.05 0.5 0 0.3 0.2032 0.1524" PadDiameters=" 0.254 0.3048 0.4064 0.6096 0.8128 1.016 1.27 1.4224 1.6764 1.778 1.9304 2.1844 2.54 3.81 6.4516 0" PadDrills=" 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.65 0.7 0.75 0.8 0.85 0.9 1 0.6" @@ -378,7 +377,7 @@ Type="Control Panel" Number=0 [Desktop] -Screen="1920 1080" +Screen="3171 2520" Window="Win_1" Window="Win_2" Window="Win_3" diff --git a/firmware/cl-step-controller/src/main.cpp b/firmware/cl-step-controller/src/main.cpp index ae33368..7a66bd2 100644 --- a/firmware/cl-step-controller/src/main.cpp +++ b/firmware/cl-step-controller/src/main.cpp @@ -13,7 +13,6 @@ VPort_USBSerial* vPortSerial = new VPort_USBSerial(); #include "drivers/step_cl.h" #include "osape/utils/clocks_d51.h" - // -------------------------------------------------------- 0: MAG DIAGNOSTICS boolean onMagDiagnosticsPut(uint8_t* data, uint16_t len); diff --git a/firmware/stepper-drop/.gitignore b/firmware/stepper-drop/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/firmware/stepper-drop/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/firmware/stepper-drop/.vscode/extensions.json b/firmware/stepper-drop/.vscode/extensions.json new file mode 100644 index 0000000..e80666b --- /dev/null +++ b/firmware/stepper-drop/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ] +} diff --git a/firmware/stepper-drop/include/README b/firmware/stepper-drop/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/firmware/stepper-drop/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/firmware/stepper-drop/lib/README b/firmware/stepper-drop/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/firmware/stepper-drop/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include <Foo.h> +#include <Bar.h> + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/firmware/stepper-drop/platformio.ini b/firmware/stepper-drop/platformio.ini new file mode 100644 index 0000000..50208f0 --- /dev/null +++ b/firmware/stepper-drop/platformio.ini @@ -0,0 +1,14 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:adafruit_feather_m4] +platform = atmelsam +board = adafruit_feather_m4 +framework = arduino diff --git a/firmware/stepper-drop/src/config.h b/firmware/stepper-drop/src/config.h new file mode 100644 index 0000000..cd39823 --- /dev/null +++ b/firmware/stepper-drop/src/config.h @@ -0,0 +1,12 @@ +#ifndef CONFIG_H_ +#define CONFIG_H_ + +//#define UCBUS_IS_HEAD +#define UCBUS_IS_DROP + +// if you're using the 'module board' https://gitlab.cba.mit.edu/jakeread/ucbus-module +// the first (og) revision has an SMT header, and some of the RS485 pins are varied, +// set this flag. otherwise, if you have thru-hole JTAG header, comment it out +// #define IS_OG_MODULE + +#endif \ No newline at end of file diff --git a/firmware/stepper-drop/src/drivers/dacs.cpp b/firmware/stepper-drop/src/drivers/dacs.cpp new file mode 100644 index 0000000..b9dbf27 --- /dev/null +++ b/firmware/stepper-drop/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/stepper-drop/src/drivers/dacs.h b/firmware/stepper-drop/src/drivers/dacs.h new file mode 100644 index 0000000..196984e --- /dev/null +++ b/firmware/stepper-drop/src/drivers/dacs.h @@ -0,0 +1,55 @@ +/* +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" +#include "../syserror.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/stepper-drop/src/drivers/peripheral_nums.h b/firmware/stepper-drop/src/drivers/peripheral_nums.h new file mode 100644 index 0000000..eed9f18 --- /dev/null +++ b/firmware/stepper-drop/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/stepper-drop/src/drivers/step_a4950.cpp b/firmware/stepper-drop/src/drivers/step_a4950.cpp new file mode 100644 index 0000000..440e5a8 --- /dev/null +++ b/firmware/stepper-drop/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/stepper-drop/src/drivers/step_a4950.h b/firmware/stepper-drop/src/drivers/step_a4950.h new file mode 100644 index 0000000..fc435d3 --- /dev/null +++ b/firmware/stepper-drop/src/drivers/step_a4950.h @@ -0,0 +1,113 @@ +/* +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" +#include "../syserror.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: 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_1_COUNT 32 + +// 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/stepper-drop/src/indicators.h b/firmware/stepper-drop/src/indicators.h new file mode 100644 index 0000000..664b8f4 --- /dev/null +++ b/firmware/stepper-drop/src/indicators.h @@ -0,0 +1,53 @@ +// for the new one! with the DIP switch! +#define CLKLIGHT_PIN 27 +#define CLKLIGHT_PORT PORT->Group[0] +#define ERRLIGHT_PIN 8 +#define ERRLIGHT_PORT PORT->Group[1] + +#define DEBUG1PIN_PIN 23 +#define DEBUG1PIN_PORT PORT->Group[0] +#define DEBUG2PIN_PIN 12 +#define DEBUG2PIN_PORT PORT->Group[1] +#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/stepper-drop/src/main.cpp b/firmware/stepper-drop/src/main.cpp new file mode 100644 index 0000000..f25f17a --- /dev/null +++ b/firmware/stepper-drop/src/main.cpp @@ -0,0 +1,361 @@ +#include <Arduino.h> + +#include "indicators.h" +#include "drivers/step_a4950.h" +#include "osape-d51/osape/osap/osap.h" +#include "osape-d51/vertices/vt_usbSerial.h" +#include "osape-d51/vertices/vt_ucBusDrop.h" +#include "osape-d51/vertices/ucBusDrop.h" + +// bare defaults: use vm / bus id to set on startup +uint8_t axis_pick = 0; +float spu = 400.0F; +float old_spu = 400.0F; +volatile boolean spu_was_set = false; +float c_scale = 0.00F; +#define TICKS_PER_PACKET 25.0F +#define TICKS_PER_SECOND 50000.0F + +// -------------------------------------------------------- AXIS PICK EP + +EP_ONDATA_RESPONSES onAxisPickData(uint8_t* data, uint16_t len){ + if(data[0] > 3){ + axis_pick = 0; + } else { + axis_pick = data[0]; + } + return EP_ONDATA_ACCEPT; +} + +vertex_t* axisPickEp = osapBuildEndpoint("axisPick", onAxisPickData, nullptr); + +// -------------------------------------------------------- AXIS INVERSION EP + +EP_ONDATA_RESPONSES onAxisInvertData(uint8_t* data, uint16_t len){ + if(data[0] > 0){ + stepper_hw->setInversion(true); + } else { + stepper_hw->setInversion(false); + } + return EP_ONDATA_ACCEPT; +} + +vertex_t* axisInvertEp = osapBuildEndpoint("axisInvert", onAxisInvertData, nullptr); + +// -------------------------------------------------------- MICROSTEP EP + +EP_ONDATA_RESPONSES onMicrostepData(uint8_t* data, uint16_t len){ + stepper_hw->setMicrostep(data[0]); + return EP_ONDATA_ACCEPT; +} + +vertex_t* microstepEp = osapBuildEndpoint("microstep", onMicrostepData, nullptr); + +// -------------------------------------------------------- SPU EP + +EP_ONDATA_RESPONSES onSPUData(uint8_t* data, uint16_t len){ + chunk_float32 spuc = { .bytes = { data[0], data[1], data[2], data[3] } }; + old_spu = spu; + spu = fabsf(spuc.f); + spu_was_set = true; + return EP_ONDATA_ACCEPT; +} + +vertex_t* spuEp = osapBuildEndpoint("SPU", onSPUData, nullptr); + +// -------------------------------------------------------- CSCALE DATA + +EP_ONDATA_RESPONSES onCScaleData(uint8_t* data, uint16_t len){ + chunk_float32 cscalec = { .bytes = { data[0], data[1], data[2], data[3] } }; + if(cscalec.f > 1.0F){ + cscalec.f = 1.0F; + } else if (cscalec.f < 0.0F){ + cscalec.f = 0.0F; + } + stepper_hw->setCurrent(cscalec.f); + return EP_ONDATA_ACCEPT; +} + +vertex_t* cScaleEp = osapBuildEndpoint("CScale", onCScaleData, nullptr); + +// -------------------------------------------------------- HOME ROUTINE + +// some homeing globals, +#define HOME_NOT 0 +#define HOME_FIRST 1 +#define HOME_BACKOFF 2 + +uint8_t homing = 0; // statemachine +float homeStepCounter = 0.0F; // step-float-counter +float homePos = 0.0F; // position (units) +float homeStepRate = 0.0F; // rate (steps/tick) +float homePosRate = 0.0F; // rate (units/tick) +boolean homeDir = false; // direction +float homeOffset = 0.0F; // after-home offset + +EP_ONDATA_RESPONSES onHomeData(uint8_t* data, uint16_t len){ + chunk_float32 rate = { .bytes = { data[0], data[1], data[2], data[3] } }; + chunk_float32 offset = { .bytes = { data[4], data[5], data[6], data[7] } }; + homing = HOME_FIRST; + homeStepCounter = 0.0F; + if(rate.f > 0){ + homeDir = true; + stepper_hw->dir(true); + } else { + homeDir = false; + stepper_hw->dir(false); + } + homeStepRate = abs(rate.f * spu) / TICKS_PER_SECOND; + homePosRate = abs(rate.f) / TICKS_PER_SECOND; + homeOffset = offset.f; + return EP_ONDATA_ACCEPT; +} + +vertex_t* homeEp = osapBuildEndpoint("Home", onHomeData, nullptr); + +// -------------------------------------------------------- HOME STATE + +boolean beforeHomeStateQuery(void); + +vertex_t* homeStateEp = osapBuildEndpoint("HomeState", nullptr, beforeHomeStateQuery); + +boolean beforeHomeStateQuery(void){ + homeStateEp->ep->data[0] = homing; + homeStateEp->ep->dataLen = 1; + return true; +} + +// -------------------------------------------------------- LIMIT SETUP + +#define LIMIT_PORT PORT->Group[0] +#define LIMIT_PIN 23 +#define LIMIT_BM ((uint32_t)(1 << LIMIT_PIN)) + +void limitSetup(void){ + // not-an-output + LIMIT_PORT.DIRCLR.reg = LIMIT_BM; + // enable input + LIMIT_PORT.PINCFG[LIMIT_PIN].bit.INEN = 1; + // enable pull + LIMIT_PORT.PINCFG[LIMIT_PIN].bit.PULLEN = 1; + // 'pull' references direction from 'out' register, so we set hi to pull up (switch pulls to gnd) + LIMIT_PORT.OUTSET.reg = LIMIT_BM; +} + +boolean limitIsMade(void){ + // return true if switch is hit + return (LIMIT_PORT.IN.reg & LIMIT_BM); +} + +void setup() { + ERRLIGHT_SETUP; + CLKLIGHT_SETUP; + DEBUG1PIN_SETUP; + // limit switch + //limitSetup(); + // osap + osapSetup(); + // ports + vt_usbSerial_setup(); + osapAddVertex(vt_usbSerial); // 0 + vt_ucBusDrop_setup(); + osapAddVertex(vt_ucBusDrop); // 1 + // axis pick + osapAddVertex(axisPickEp); // 2 + // axis invert + osapAddVertex(axisInvertEp); // 3 + // microstep + osapAddVertex(microstepEp); // 4 + // SPU + osapAddVertex(spuEp); // 5 + // cscale + osapAddVertex(cScaleEp); // 6 + // homing + osapAddVertex(homeEp); // 7 + osapAddVertex(homeStateEp); // 8 + // stepper init + stepper_hw->init(false, c_scale); +} + +// have available, +// stepper_hw->setCurrent(currentChunks[AXIS_PICK].f); + +//#define TEST_TX + +unsigned long lastTx = 0; +uint8_t tstTx[14] = {1, 2, 3, 4, 5, 0, 7, 8, 9, 10, 11, 12, 13, 14}; + +void loop() { + osapLoop(); + stepper_hw->dacRefresh(); + #ifdef TEST_TX + if(millis() > lastTx + 500){ + lastTx = millis(); + if(ucBusDrop_ctsB()){ + ERRLIGHT_TOGGLE; + ucBusDrop_transmitB(tstTx, 14); + for(uint8_t i = 0; i < 14; i ++){ + //tstTx[i] ++; + } + } + } + #endif + /* + // still not sure: this registers as "made" on init (?) + if(limitIsMade()){ + ERRLIGHT_ON; + } else { + ERRLIGHT_OFF; + } + */ +} // end loop + + +volatile float current_floating_pos = 0.0F; +volatile int32_t current_step_pos = 0; +volatile uint32_t delta_steps = 0; + +volatile float vel = 0.0F; +volatile float move_counter = 0.0F; + +volatile boolean setBlock = false; + +void ucBusDrop_onPacketARx(uint8_t* inBufferA, volatile uint16_t len){ + #warning debug-d out + return; + // don't execute when we have been given a set-position block + if(setBlock) return; + // don't execute if we are currently homing + if(homing) return; + //DEBUG2PIN_TOGGLE; + // last move is done, convert back steps -> float, + if(spu_was_set){ + current_floating_pos = current_step_pos / old_spu; + current_step_pos = lroundf(current_floating_pos * spu); + spu_was_set = false; + } else { + current_floating_pos = current_step_pos / spu; + } + vel = 0.0F; // reset zero in case packet is not move + uint8_t bptr = 0; + // switch bus packet types + switch(inBufferA[0]){ + case UB_AK_GOTOPOS: + { + bptr = axis_pick * 4 + 1; + chunk_float32 target = { + .bytes = { inBufferA[bptr], inBufferA[bptr + 1], inBufferA[bptr + 2], inBufferA[bptr + 3] } + }; + /* + chunk_float64 target = { + .bytes = { inBuffer[bptr], inBuffer[bptr + 1], inBuffer[bptr + 2], inBuffer[bptr + 3], + inBuffer[bptr + 4], inBuffer[bptr + 5], inBuffer[bptr + 6], inBuffer[bptr + 7] }}; + */ + float delta = target.f - current_floating_pos; + // update, + //move_counter = 0.0F; // shouldn't need this ? try deleting + // direction swop, + if(delta > 0){ + stepper_hw->dir(true); + } else { + stepper_hw->dir(false); + } + // how many steps, + delta_steps = lroundf(abs(delta * spu)); + // what speed + vel = abs(delta * spu) / TICKS_PER_PACKET; + // for str8 r8 + /* + if(delta_steps == 0){ + vel = 0.0F; + } else { + vel = abs(delta * SPU) / TICKS_PER_PACKET; + } + */ + break; // end gotopos + } + case UB_AK_SETPOS: + { + // reqest is to set position, not go to it... + bptr = axis_pick * 4 + 1; + chunk_float32 target = { + .bytes = { inBufferA[bptr], inBufferA[bptr + 1], inBufferA[bptr + 2], inBufferA[bptr + 3] } + }; + float target_current_pos = target.f; + int32_t target_current_steps = lroundf(target_current_pos * spu); + setBlock = true; // don't do step work while these are modified + current_floating_pos = target_current_pos; + current_step_pos = target_current_steps; + vel = 0; + delta_steps = 0; + setBlock = false; + break; + } + default: + break; + } + //DEBUG2PIN_OFF; +} + +void ucBusDrop_onRxISR(void){ + #warning debug-d out + return; + // no-op when given a set block, + if(setBlock) return; + // incremental motion if is homing + if(homing != 0){ + switch(homing){ + case HOME_FIRST: + if(limitIsMade()){ + // traaaaaaansition -> backoff + stepper_hw->dir(!homeDir); + homeStepCounter = 0.0F; + homePos = 0.0F; + homing = HOME_BACKOFF; + } else { + homeStepCounter += homeStepRate; + if(homeStepCounter >= 1.0F){ + homeStepCounter -= 1.0F; + stepper_hw->step(); + } + } + break; + case HOME_BACKOFF: + homeStepCounter += homeStepRate; + homePos += homePosRate; + if(homeStepCounter >= 1.0F){ // backoff motion + homeStepCounter -= 1.0F; + stepper_hw->step(); + } + if(homePos >= homeOffset){ // until more than 2mm away + // traaaaaaaaaaaaaansition -> end + homing = 0; + } + break; + default: + homing = 0; + } + return; + } + // normal step operation + //DEBUG2PIN_TOGGLE; + move_counter += vel; + boolean move_check = (move_counter > 1.0F); + //DEBUG2PIN_TOGGLE; + if(move_check){ + move_counter -= 1.0F; + if(delta_steps == 0){ + // nothing + } else { + //DEBUG1PIN_TOGGLE; + stepper_hw->step(); + delta_steps --; + if(stepper_hw->getDir()){ + current_step_pos ++; + } else { + current_step_pos --; + } + } + } +} + diff --git a/firmware/stepper-drop/src/osape-d51 b/firmware/stepper-drop/src/osape-d51 new file mode 160000 index 0000000..e926d28 --- /dev/null +++ b/firmware/stepper-drop/src/osape-d51 @@ -0,0 +1 @@ +Subproject commit e926d28ae33499d1bbb96df5933fba47e7d5eec7 diff --git a/firmware/stepper-drop/src/syserror.cpp b/firmware/stepper-drop/src/syserror.cpp new file mode 100644 index 0000000..f38d38b --- /dev/null +++ b/firmware/stepper-drop/src/syserror.cpp @@ -0,0 +1,93 @@ +#include "syserror.h" +#include "osape-d51/osape/osap/ts.h" +#include "osape-d51/vertices/ucBusDrop.h" +#include "osape-d51/osape/utils/cobs.h" +#include "config.h" +#include "indicators.h" + +uint8_t errBuf[1028]; +uint8_t errEncoded[1028]; + +/* +boolean writeString(unsigned char* dest, uint16_t* dptr, String msg){ + uint16_t len = msg.length(); + dest[(*dptr) ++] = TS_STRING_KEY; + writeLenBytes(dest, dptr, len); + msg.getBytes(dest, len + 1); + return true; +} + +boolean writeLenBytes(unsigned char* dest, uint16_t* dptr, uint16_t len){ + dest[(*dptr) ++] = len; + dest[(*dptr) ++] = (len >> 8) & 255; + return true; +} +*/ + +#ifdef UCBUS_IS_DROP + +//uint8_t escape[512]; +//uint8_t escapeHeader[10] = { PK_BUSF_KEY, 0, 0, 0, 0, PK_PTR, PK_PORTF_KEY, 0, 0, PK_DEST }; + +// config-your-own-ll-escape-hatch +void sysError(String msg){ + //ERRLIGHT_ON; + /* + uint32_t len = msg.length(); + errBuf[0] = PK_LLERR; // the ll-errmsg-key + errBuf[1] = len & 255; + errBuf[2] = (len >> 8) & 255; + errBuf[3] = (len >> 16) & 255; + errBuf[4] = (len >> 24) & 255; + msg.getBytes(&(errBuf[5]), len + 1); + // write header, + memcpy(escape, escapeHeader, 10); + // write segsize, checksum + uint16_t wptr = 10; + ts_writeUint16(128, escape, &wptr); + ts_writeUint16(len + 5, escape, &wptr); + memcpy(&(escape[wptr]), errBuf, len + 5); + // transmit on ucbus + // potential here to hang-up and do while(!(ucBusDrop->cts())) ... I *think* that would clear on an interrupt + ucBusDrop->transmit(escape, len + wptr + 5); + */ +} + +#else + +// config-your-own-ll-escape-hatch +void sysError(String msg){ + // whatever you want, + //ERRLIGHT_ON; + uint32_t len = msg.length(); + errBuf[0] = 0; // serport looks for acks in each msg, this is not one + errBuf[1] = PK_PTR; + errBuf[2] = PK_LLESCAPE_KEY; // the ll-errmsg-key + errBuf[3] = len & 255; + errBuf[4] = (len >> 8) & 255; + errBuf[5] = (len >> 16) & 255; + errBuf[6] = (len >> 24) & 255; + msg.getBytes(&(errBuf[7]), len + 1); + size_t ecl = cobsEncode(errBuf, len + 7, errEncoded); + // direct escape + //if(Serial.availableForWrite() > (int64_t)ecl){ + Serial.write(errEncoded, ecl); + Serial.flush(); + //} else { + // ERRLIGHT_ON; + //} +} + +#endif + +void logPacket(uint8_t* pck, uint16_t len){ + String errmsg; + errmsg.reserve(1024); + errmsg = "pck: "; // max 64 + for(uint8_t i = 0; i < 64; i ++){ + if(i >= len) break; + errmsg += String(pck[i]); + errmsg += ", "; + } + sysError(errmsg); +} \ No newline at end of file diff --git a/firmware/stepper-drop/src/syserror.h b/firmware/stepper-drop/src/syserror.h new file mode 100644 index 0000000..3645dbc --- /dev/null +++ b/firmware/stepper-drop/src/syserror.h @@ -0,0 +1,10 @@ +#ifndef SYSERROR_H_ +#define SYSERROR_H_ + +#include <Arduino.h> + +void sysError(String msg); +void logPacket(uint8_t* pck, uint16_t len); +//void sysError(uint8_t* bytes, uint16_t len); + +#endif diff --git a/firmware/stepper-drop/test/README b/firmware/stepper-drop/test/README new file mode 100644 index 0000000..b94d089 --- /dev/null +++ b/firmware/stepper-drop/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Unit Testing and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/page/plus/unit-testing.html -- GitLab