From 48f117e96f380eba0ced9c7b45fba954246e9df2 Mon Sep 17 00:00:00 2001 From: Jake Read <jake.read@cba.mit.edu> Date: Wed, 30 Sep 2020 11:06:56 -0400 Subject: [PATCH] rm firmware --- firmware/osape-smoothieroll-head/.gitignore | 5 - firmware/osape-smoothieroll-head/.travis.yml | 67 --- .../.vscode/extensions.json | 7 - .../.vscode/settings.json | 5 - .../osape-smoothieroll-head/include/README | 39 -- firmware/osape-smoothieroll-head/lib/README | 46 -- .../osape-smoothieroll-head/platformio.ini | 14 - .../src/drivers/indicators.h | 57 -- .../src/drivers/peripheral_nums.h | 18 - .../src/drivers/ucbus_head.cpp | 261 ---------- .../src/drivers/ucbus_head.h | 122 ----- firmware/osape-smoothieroll-head/src/main.cpp | 404 --------------- .../osape-smoothieroll-head/src/osap/osap.cpp | 487 ------------------ .../osape-smoothieroll-head/src/osap/osap.h | 60 --- .../osape-smoothieroll-head/src/osap/ts.cpp | 82 --- .../osape-smoothieroll-head/src/osap/ts.h | 103 ---- .../src/osap/vport.cpp | 40 -- .../osape-smoothieroll-head/src/osap/vport.h | 52 -- .../src/osap/vport_usbserial.cpp | 141 ----- .../src/osap/vport_usbserial.h | 57 -- .../src/smoothie/SmoothieRoll.cpp | 45 -- .../src/smoothie/SmoothieRoll.h | 43 -- .../src/smoothie/libs/HeapRing.cpp | 255 --------- .../src/smoothie/libs/HeapRing.h | 86 ---- .../src/smoothie/libs/StepTicker.cpp | 222 -------- .../src/smoothie/libs/StepTicker.h | 76 --- .../modules/robot/ActuatorCoordinates.h | 16 - .../src/smoothie/modules/robot/Block.cpp | 350 ------------- .../src/smoothie/modules/robot/Block.h | 79 --- .../src/smoothie/modules/robot/Conveyor.cpp | 318 ------------ .../src/smoothie/modules/robot/Conveyor.h | 73 --- .../src/smoothie/modules/robot/Planner.cpp | 351 ------------- .../src/smoothie/modules/robot/Planner.h | 49 -- .../smoothie/modules/robot/StepInterface.cpp | 61 --- .../smoothie/modules/robot/StepInterface.h | 44 -- .../src/utils/clocks_d51_module.cpp | 115 ----- .../src/utils/clocks_d51_module.h | 43 -- .../src/utils/cobs.cpp | 60 --- .../osape-smoothieroll-head/src/utils/cobs.h | 24 - .../src/utils/syserror.cpp | 38 -- .../src/utils/syserror.h | 11 - firmware/osape-smoothieroll-head/test/README | 11 - 42 files changed, 4437 deletions(-) delete mode 100644 firmware/osape-smoothieroll-head/.gitignore delete mode 100644 firmware/osape-smoothieroll-head/.travis.yml delete mode 100644 firmware/osape-smoothieroll-head/.vscode/extensions.json delete mode 100644 firmware/osape-smoothieroll-head/.vscode/settings.json delete mode 100644 firmware/osape-smoothieroll-head/include/README delete mode 100644 firmware/osape-smoothieroll-head/lib/README delete mode 100644 firmware/osape-smoothieroll-head/platformio.ini delete mode 100644 firmware/osape-smoothieroll-head/src/drivers/indicators.h delete mode 100644 firmware/osape-smoothieroll-head/src/drivers/peripheral_nums.h delete mode 100644 firmware/osape-smoothieroll-head/src/drivers/ucbus_head.cpp delete mode 100644 firmware/osape-smoothieroll-head/src/drivers/ucbus_head.h delete mode 100644 firmware/osape-smoothieroll-head/src/main.cpp delete mode 100644 firmware/osape-smoothieroll-head/src/osap/osap.cpp delete mode 100644 firmware/osape-smoothieroll-head/src/osap/osap.h delete mode 100644 firmware/osape-smoothieroll-head/src/osap/ts.cpp delete mode 100644 firmware/osape-smoothieroll-head/src/osap/ts.h delete mode 100644 firmware/osape-smoothieroll-head/src/osap/vport.cpp delete mode 100644 firmware/osape-smoothieroll-head/src/osap/vport.h delete mode 100644 firmware/osape-smoothieroll-head/src/osap/vport_usbserial.cpp delete mode 100644 firmware/osape-smoothieroll-head/src/osap/vport_usbserial.h delete mode 100644 firmware/osape-smoothieroll-head/src/smoothie/SmoothieRoll.cpp delete mode 100644 firmware/osape-smoothieroll-head/src/smoothie/SmoothieRoll.h delete mode 100644 firmware/osape-smoothieroll-head/src/smoothie/libs/HeapRing.cpp delete mode 100644 firmware/osape-smoothieroll-head/src/smoothie/libs/HeapRing.h delete mode 100644 firmware/osape-smoothieroll-head/src/smoothie/libs/StepTicker.cpp delete mode 100644 firmware/osape-smoothieroll-head/src/smoothie/libs/StepTicker.h delete mode 100644 firmware/osape-smoothieroll-head/src/smoothie/modules/robot/ActuatorCoordinates.h delete mode 100644 firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Block.cpp delete mode 100644 firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Block.h delete mode 100644 firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Conveyor.cpp delete mode 100644 firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Conveyor.h delete mode 100644 firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Planner.cpp delete mode 100644 firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Planner.h delete mode 100644 firmware/osape-smoothieroll-head/src/smoothie/modules/robot/StepInterface.cpp delete mode 100644 firmware/osape-smoothieroll-head/src/smoothie/modules/robot/StepInterface.h delete mode 100644 firmware/osape-smoothieroll-head/src/utils/clocks_d51_module.cpp delete mode 100644 firmware/osape-smoothieroll-head/src/utils/clocks_d51_module.h delete mode 100644 firmware/osape-smoothieroll-head/src/utils/cobs.cpp delete mode 100644 firmware/osape-smoothieroll-head/src/utils/cobs.h delete mode 100644 firmware/osape-smoothieroll-head/src/utils/syserror.cpp delete mode 100644 firmware/osape-smoothieroll-head/src/utils/syserror.h delete mode 100644 firmware/osape-smoothieroll-head/test/README diff --git a/firmware/osape-smoothieroll-head/.gitignore b/firmware/osape-smoothieroll-head/.gitignore deleted file mode 100644 index 89cc49c..0000000 --- a/firmware/osape-smoothieroll-head/.gitignore +++ /dev/null @@ -1,5 +0,0 @@ -.pio -.vscode/.browse.c_cpp.db* -.vscode/c_cpp_properties.json -.vscode/launch.json -.vscode/ipch diff --git a/firmware/osape-smoothieroll-head/.travis.yml b/firmware/osape-smoothieroll-head/.travis.yml deleted file mode 100644 index 7c486f1..0000000 --- a/firmware/osape-smoothieroll-head/.travis.yml +++ /dev/null @@ -1,67 +0,0 @@ -# Continuous Integration (CI) is the practice, in software -# engineering, of merging all developer working copies with a shared mainline -# several times a day < https://docs.platformio.org/page/ci/index.html > -# -# Documentation: -# -# * Travis CI Embedded Builds with PlatformIO -# < https://docs.travis-ci.com/user/integration/platformio/ > -# -# * PlatformIO integration with Travis CI -# < https://docs.platformio.org/page/ci/travis.html > -# -# * User Guide for `platformio ci` command -# < https://docs.platformio.org/page/userguide/cmd_ci.html > -# -# -# Please choose one of the following templates (proposed below) and uncomment -# it (remove "# " before each line) or use own configuration according to the -# Travis CI documentation (see above). -# - - -# -# Template #1: General project. Test it using existing `platformio.ini`. -# - -# language: python -# python: -# - "2.7" -# -# sudo: false -# cache: -# directories: -# - "~/.platformio" -# -# install: -# - pip install -U platformio -# - platformio update -# -# script: -# - platformio run - - -# -# Template #2: The project is intended to be used as a library with examples. -# - -# language: python -# python: -# - "2.7" -# -# sudo: false -# cache: -# directories: -# - "~/.platformio" -# -# env: -# - PLATFORMIO_CI_SRC=path/to/test/file.c -# - PLATFORMIO_CI_SRC=examples/file.ino -# - PLATFORMIO_CI_SRC=path/to/test/directory -# -# install: -# - pip install -U platformio -# - platformio update -# -# script: -# - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N diff --git a/firmware/osape-smoothieroll-head/.vscode/extensions.json b/firmware/osape-smoothieroll-head/.vscode/extensions.json deleted file mode 100644 index e80666b..0000000 --- a/firmware/osape-smoothieroll-head/.vscode/extensions.json +++ /dev/null @@ -1,7 +0,0 @@ -{ - // See http://go.microsoft.com/fwlink/?LinkId=827846 - // for the documentation about the extensions.json format - "recommendations": [ - "platformio.platformio-ide" - ] -} diff --git a/firmware/osape-smoothieroll-head/.vscode/settings.json b/firmware/osape-smoothieroll-head/.vscode/settings.json deleted file mode 100644 index adb1e49..0000000 --- a/firmware/osape-smoothieroll-head/.vscode/settings.json +++ /dev/null @@ -1,5 +0,0 @@ -{ - "files.associations": { - "unordered_map": "cpp" - } -} \ No newline at end of file diff --git a/firmware/osape-smoothieroll-head/include/README b/firmware/osape-smoothieroll-head/include/README deleted file mode 100644 index 194dcd4..0000000 --- a/firmware/osape-smoothieroll-head/include/README +++ /dev/null @@ -1,39 +0,0 @@ - -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/osape-smoothieroll-head/lib/README b/firmware/osape-smoothieroll-head/lib/README deleted file mode 100644 index 6debab1..0000000 --- a/firmware/osape-smoothieroll-head/lib/README +++ /dev/null @@ -1,46 +0,0 @@ - -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/osape-smoothieroll-head/platformio.ini b/firmware/osape-smoothieroll-head/platformio.ini deleted file mode 100644 index 50208f0..0000000 --- a/firmware/osape-smoothieroll-head/platformio.ini +++ /dev/null @@ -1,14 +0,0 @@ -; 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/osape-smoothieroll-head/src/drivers/indicators.h b/firmware/osape-smoothieroll-head/src/drivers/indicators.h deleted file mode 100644 index 5a2356a..0000000 --- a/firmware/osape-smoothieroll-head/src/drivers/indicators.h +++ /dev/null @@ -1,57 +0,0 @@ -// for the new one! with the DIP switch! -#define CLKLIGHT_PIN 30 -#define CLKLIGHT_PORT PORT->Group[1] -#define ERRLIGHT_PIN 27 -#define ERRLIGHT_PORT PORT->Group[0] -#define DEBUG1PIN_PIN 13 -#define DEBUG1PIN_PORT PORT->Group[0] -#define DEBUG2PIN_PIN 12 -#define DEBUG2PIN_PORT PORT->Group[0] -#define DEBUG3PIN_PIN 15 -#define DEBUG3PIN_PORT PORT->Group[1] -#define DEBUG4PIN_PIN 14 -#define DEBUG4PIN_PORT PORT->Group[1] - -#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 - -#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 - -#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/osape-smoothieroll-head/src/drivers/peripheral_nums.h b/firmware/osape-smoothieroll-head/src/drivers/peripheral_nums.h deleted file mode 100644 index eed9f18..0000000 --- a/firmware/osape-smoothieroll-head/src/drivers/peripheral_nums.h +++ /dev/null @@ -1,18 +0,0 @@ -#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/osape-smoothieroll-head/src/drivers/ucbus_head.cpp b/firmware/osape-smoothieroll-head/src/drivers/ucbus_head.cpp deleted file mode 100644 index 8b6126d..0000000 --- a/firmware/osape-smoothieroll-head/src/drivers/ucbus_head.cpp +++ /dev/null @@ -1,261 +0,0 @@ -/* -osap/drivers/ucbus_head.cpp - -beginnings of a uart-based clock / bus combo protocol - -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 "ucbus_head.h" - -UCBus_Head* UCBus_Head::instance = 0; - -UCBus_Head* UCBus_Head::getInstance(void){ - if(instance == 0){ - instance = new UCBus_Head(); - } - return instance; -} - -// making this instance globally available, when built, -// recall extern declaration in .h -UCBus_Head* ucBusHead = UCBus_Head::getInstance(); - -UCBus_Head::UCBus_Head(void) {} - -// uart init -void UCBus_Head::startupUART(void){ - // driver output is always on at head, set HI to enable - UBH_DE_PORT.DIRSET.reg = UBH_DE_BM; - UBH_DE_PORT.OUTSET.reg = UBH_DE_BM; - // receive output is always on at head, set LO to enable - UBH_RE_PORT.DIRSET.reg = UBH_RE_BM; - UBH_RE_PORT.OUTCLR.reg = UBH_RE_BM; - // termination resistor for receipt on bus head is on, set LO to enable - UBH_TE_PORT.DIRSET.reg = UBH_TE_BM; - UBH_TE_PORT.OUTCLR.reg = UBH_TE_BM; - // rx pin setup - UBH_COMPORT.DIRCLR.reg = UBH_RXBM; - UBH_COMPORT.PINCFG[UBH_RXPIN].bit.PMUXEN = 1; - if(UBH_RXPIN % 2){ - UBH_COMPORT.PMUX[UBH_RXPIN >> 1].reg |= PORT_PMUX_PMUXO(UBH_RXPERIPHERAL); - } else { - UBH_COMPORT.PMUX[UBH_RXPIN >> 1].reg |= PORT_PMUX_PMUXE(UBH_RXPERIPHERAL); - } - // tx - UBH_COMPORT.DIRCLR.reg = UBH_TXBM; - UBH_COMPORT.PINCFG[UBH_TXPIN].bit.PMUXEN = 1; - if(UBH_TXPIN % 2){ - UBH_COMPORT.PMUX[UBH_TXPIN >> 1].reg |= PORT_PMUX_PMUXO(UBH_TXPERIPHERAL); - } else { - UBH_COMPORT.PMUX[UBH_TXPIN >> 1].reg |= PORT_PMUX_PMUXE(UBH_TXPERIPHERAL); - } - // ok, clocks, first line au manuel - // unmask clocks - MCLK->APBAMASK.bit.SERCOM1_ = 1; - GCLK->GENCTRL[UBH_GCLKNUM_PICK].reg = GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL) | GCLK_GENCTRL_GENEN; - while(GCLK->SYNCBUSY.reg & GCLK_SYNCBUSY_GENCTRL(UBH_GCLKNUM_PICK)); - GCLK->PCHCTRL[UBH_SERCOM_CLK].reg = GCLK_PCHCTRL_CHEN | GCLK_PCHCTRL_GEN(UBH_GCLKNUM_PICK); - // then, sercom - while(UBH_SER_USART.SYNCBUSY.bit.ENABLE); - UBH_SER_USART.CTRLA.bit.ENABLE = 0; - while(UBH_SER_USART.SYNCBUSY.bit.SWRST); - UBH_SER_USART.CTRLA.bit.SWRST = 1; - while(UBH_SER_USART.SYNCBUSY.bit.SWRST); - while(UBH_SER_USART.SYNCBUSY.bit.SWRST || UBH_SER_USART.SYNCBUSY.bit.ENABLE); - // ok, well - UBH_SER_USART.CTRLA.reg = SERCOM_USART_CTRLA_MODE(1) | SERCOM_USART_CTRLA_DORD; // data order and - UBH_SER_USART.CTRLA.reg |= SERCOM_USART_CTRLA_RXPO(UBH_RXPO) | SERCOM_USART_CTRLA_TXPO(0); // rx and tx pinout options - UBH_SER_USART.CTRLA.reg |= SERCOM_USART_CTRLA_FORM(1); // turn on parity: parity is even by default (set in CTRLB), leave that - while(UBH_SER_USART.SYNCBUSY.bit.CTRLB); - UBH_SER_USART.CTRLB.reg = SERCOM_USART_CTRLB_RXEN | SERCOM_USART_CTRLB_TXEN | SERCOM_USART_CTRLB_CHSIZE(0); - // enable interrupts - NVIC_EnableIRQ(SERCOM1_2_IRQn); - NVIC_EnableIRQ(SERCOM1_0_IRQn); - // set baud - UBH_SER_USART.BAUD.reg = UBH_BAUD_VAL; - // and finally, a kickoff - while(UBH_SER_USART.SYNCBUSY.bit.ENABLE); - UBH_SER_USART.CTRLA.bit.ENABLE = 1; - // enable the rx interrupt, - UBH_SER_USART.INTENSET.bit.RXC = 1; -} - -void UCBus_Head::init(void) { - // clear buffers to begin, - for(uint8_t d = 0; d < UBH_DROP_OPS; d ++){ - inBufferLen[d] = 0; - inBufferRp[d] = 0; - } - startupUART(); -} - -void UCBus_Head::timerISR(void){ - // debug zero - if(outReceiveCall == 0){ - //DEBUG1PIN_TOGGLE; - } - // so, would formulate the out bytes, - // in either case we formulate the outByte and outHeader, then bit shift identically into - // the word ... - if(outBufferALen > 0){ // always transmit channel A before B - // have bytes, write word to encapsulate outBuffer[outBufferRp]; the do outBufferRp ++ and check wrap - // mask: 6 bit, - if(outBufferARp >= outBufferALen){ - // this is the EOP frame, - outByte = 0; - outHeader = headerMask & (noTokenWordA | (outReceiveCall & dropIdMask)); - // now it's clear, - outBufferARp = 0; - outBufferALen = 0; - } else { - // this is a regular frame - outByte = outBufferA[outBufferARp]; - outHeader = headerMask & (tokenWordA | (outReceiveCall & dropIdMask)); - outBufferARp ++; // increment for next byte, - } - } else if (outBufferBLen > 0){ - if(outBufferBRp >= outBufferBLen){ - // CHB EOP frame - outByte = 0; - outHeader = headerMask & (noTokenWordB | (outReceiveCall & dropIdMask)); - // now is clear, - outBufferBRp = 0; - outBufferBLen = 0; - } else { - // ahn regular CHB frame - outByte = outBufferB[outBufferBRp]; - outHeader = headerMask & (tokenWordB | (outReceiveCall & dropIdMask)); - outBufferBRp ++; - } - } else { - // no token, no EOP on either channel - // alternate channels, in case spurious packets not closed on one ... ensure close - outByte = 0; - if(lastSpareEOP == 0){ - outHeader = headerMask & (noTokenWordA | (outReceiveCall & dropIdMask)); - lastSpareEOP = 1; - } else { - outHeader = headerMask & (noTokenWordB | (outReceiveCall & dropIdMask)); - lastSpareEOP = 0; - } - } - outWord[0] = 0b00000000 | ((outHeader << 1) & 0b01110000) | (outByte >> 4); - outWord[1] = 0b10000000 | ((outHeader << 4) & 0b01110000) | (outByte & 0b00001111); - // put the UART to work before more clocks on buffer incrementing - UBH_SER_USART.DATA.reg = outWord[0]; - // and setup the interrupt to handle the second, - UBH_SER_USART.INTENSET.bit.DRE = 1; - // and loop through returns, - outReceiveCall ++; - if(outReceiveCall >= UBH_DROP_OPS){ - outReceiveCall = 0; - } -} - -// TX Handler, for second bytes initiated by timer, -void SERCOM1_0_Handler(void){ - ucBusHead->txISR(); -} - -void UCBus_Head::txISR(void){ - UBH_SER_USART.DATA.reg = outWord[1]; // just services the next byte in the word: timer initiates - UBH_SER_USART.INTENCLR.reg = SERCOM_USART_INTENCLR_DRE; // turn this interrupt off -} - -void SERCOM1_2_Handler(void){ - ucBusHead->rxISR(); -} - -void UCBus_Head::rxISR(void){ - // check parity bit, - uint16_t perr = UBH_SER_USART.STATUS.bit.PERR; - if(perr){ - //ERRLIGHT_ON; - uint8_t clear = UBH_SER_USART.DATA.reg; - UBH_SER_USART.STATUS.bit.PERR = 1; // clear parity flag - return; - } - // cleared by reading out, but we are blind feed forward atm - uint8_t data = UBH_SER_USART.DATA.reg; - if((data >> 7) == 0){ - inWord[0] = data; - } else { - inWord[1] = data; - // now decouple, - inHeader = ((inWord[0] >> 1) & 0b00111000) | ((inWord[1] >> 4) & 0b00000111); - inByte = ((inWord[0] << 4) & 0b11110000) | (inWord[1] & 0b00001111); - // the drop reporting - uint8_t drop = inHeader & dropIdMask; - if(drop > UBH_DROP_OPS) return; // unknown drop ? - // otherwise, load it - inBuffer[drop][inBufferRp[drop]] = inByte; - if(inByte == 0){ // eop cobs encoded - inBufferLen[drop] = inBufferRp[drop]; - } else { - inBufferRp[drop] += 1; - } - } -} - -// -------------------------------------------------------- API - -boolean UCBus_Head::ctr(uint8_t drop){ - if(drop >= UBH_DROP_OPS) return false; - if(inBufferLen[drop] > 0){ - return true; - } else { - return false; - } -} - -size_t UCBus_Head::read(uint8_t drop, uint8_t *dest){ - if(!ctr(drop)) return 0; - NVIC_DisableIRQ(SERCOM1_2_IRQn); - size_t decodeLen = cobsDecode(inBuffer[drop], inBufferLen[drop], dest); - inBufferLen[drop] = 0; - inBufferRp[drop] = 0; - NVIC_EnableIRQ(SERCOM1_2_IRQn); - return decodeLen; -} - -// mod cts(channel) and transmit(data, len, channel) -// then do an example for channel-b-write currents, then do drop code, then test - -boolean UCBus_Head::cts_a(void){ - if(outBufferALen != 0){ - return false; - } else { - return true; - } -} - -boolean UCBus_Head::cts_b(void){ - if(outBufferBLen != 0){ - return false; - } else { - return true; - } -} - -void UCBus_Head::transmit_a(uint8_t *data, uint16_t len){ - if(!cts_a()) return; - //size_t encLen = cobsEncode(data, len, outBuffer); - memcpy(outBufferA, data, len); - outBufferALen = len; //encLen; - outBufferARp = 0; -} - -void UCBus_Head::transmit_b(uint8_t *data, uint16_t len){\ - if(!cts_b()) return; - memcpy(outBufferB, data, len); - outBufferBLen = len; - outBufferBRp = 0; -} - diff --git a/firmware/osape-smoothieroll-head/src/drivers/ucbus_head.h b/firmware/osape-smoothieroll-head/src/drivers/ucbus_head.h deleted file mode 100644 index e171d1c..0000000 --- a/firmware/osape-smoothieroll-head/src/drivers/ucbus_head.h +++ /dev/null @@ -1,122 +0,0 @@ -/* -osap/drivers/ucbus_head.h - -beginnings of a uart-based clock / bus combo protocol - -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 UCBUS_HEAD_H_ -#define UCBUS_HEAD_H_ - -#include <arduino.h> - -#include "indicators.h" -#include "peripheral_nums.h" -#include "utils/syserror.h" -#include "utils/clocks_d51_module.h" -#include "utils/cobs.h" - -#define TIMER_A_GCLK_NUM 9 -#define TIMER_B_GCLK_NUM 10 - -#define UBH_SER_USART SERCOM1->USART -#define UBH_SERCOM_CLK SERCOM1_GCLK_ID_CORE -#define UBH_GCLKNUM_PICK 7 -#define UBH_COMPORT PORT->Group[0] -#define UBH_TXPIN 16 // x-0 -#define UBH_TXBM (uint32_t)(1 << UBH_TXPIN) -#define UBH_RXPIN 18 // x-2 -#define UBH_RXBM (uint32_t)(1 << UBH_RXPIN) -#define UBH_RXPO 2 // RX on SER-2 -#define UBH_TXPERIPHERAL PERIPHERAL_C -#define UBH_RXPERIPHERAL PERIPHERAL_C - -// baud bb baud -// 63019 for a very safe 115200 -// 54351 for a go-karting 512000 -// 43690 for a trotting pace of 1MHz -// 21845 for the E30 2MHz -// 0 for max-speed 3MHz -#define UBH_BAUD_VAL 0 - -#define UBH_DE_PIN 16 // driver output enable: set HI to enable, LO to tri-state the driver -#define UBH_DE_BM (uint32_t)(1 << UBH_DE_PIN) -#define UBH_DE_PORT PORT->Group[1] -#define UBH_RE_PIN 19 // receiver output enable, set LO to enable the RO, set HI to tri-state RO -#define UBH_RE_BM (uint32_t)(1 << UBH_RE_PIN) -#define UBH_RE_PORT PORT->Group[0] -#define UBH_TE_PIN 17 // termination enable, drive LO to enable to internal termination resistor, HI to disable -#define UBH_TE_BM (uint32_t)(1 << UBH_TE_PIN) -#define UBH_TE_PORT PORT->Group[0] - -#define UBH_BUFSIZE 1024 - -#define UBH_DROP_OPS 14 - -#define UB_AK_GOTOPOS 91 -#define UB_AK_SETPOS 92 -#define UB_AK_SETRPM 93 - -// PLEASE NOTE: this requires a 100kHz tick, use interrupt timer, -// fire the timerISR there. - -class UCBus_Head { - private: - // singleton-ness - static UCBus_Head* instance; - // input is big for the head, - volatile uint8_t inWord[2]; - volatile uint8_t inHeader; - volatile uint8_t inByte; - uint8_t inBuffer[UBH_DROP_OPS][UBH_BUFSIZE]; - volatile uint16_t inBufferRp[UBH_DROP_OPS]; - volatile uint16_t inBufferLen[UBH_DROP_OPS]; - // transmit buffers for A / B Channels - uint8_t outBufferA[UBH_BUFSIZE]; - volatile uint16_t outBufferARp = 0; - volatile uint16_t outBufferALen = 0; - uint8_t outBufferB[UBH_BUFSIZE]; - volatile uint16_t outBufferBRp = 0; - volatile uint16_t outBufferBLen = 0; - // doublet - volatile uint8_t outWord[2]; - volatile uint8_t outReceiveCall = 0; - volatile uint8_t outHeader; - volatile uint8_t outByte; - const uint8_t headerMask = 0b00111111; - const uint8_t dropIdMask = 0b00001111; - // 0b00|token|channel|4bit id - const uint8_t tokenWordA = 0b00100000; // CHA, data byte present - const uint8_t noTokenWordA = 0b00000000; // CHA, data byte not present - const uint8_t tokenWordB = 0b00110000; // CHB, data byte present - const uint8_t noTokenWordB = 0b00010000; // CHB, data byte not present - volatile uint8_t lastSpareEOP = 0; - // uart - void startupUART(void); - public: - UCBus_Head(); - static UCBus_Head* getInstance(void); - // isrs - void timerISR(void); - void rxISR(void); - void txISR(void); - // handles - void init(void); - boolean ctr(uint8_t drop); // is there ahn packet to read at this drop - size_t read(uint8_t drop, uint8_t *dest); // get 'them bytes fam - boolean cts_a(void); // return true if TX complete / buffer ready - boolean cts_b(void); - void transmit_a(uint8_t *data, uint16_t len); // ship les bytos - void transmit_b(uint8_t *data, uint16_t len); -}; - -extern UCBus_Head* ucBusHead; - -#endif \ No newline at end of file diff --git a/firmware/osape-smoothieroll-head/src/main.cpp b/firmware/osape-smoothieroll-head/src/main.cpp deleted file mode 100644 index c7fe2c5..0000000 --- a/firmware/osape-smoothieroll-head/src/main.cpp +++ /dev/null @@ -1,404 +0,0 @@ -#include <Arduino.h> - -#include "drivers/indicators.h" -#include "utils/cobs.h" -#include "osap/osap.h" - -OSAP* osap = new OSAP("smoothieRoll head"); -#include "osap/vport_usbserial.h" -VPort_USBSerial* vPortSerial = new VPort_USBSerial(); // 8 frames input, 1028 bytes each - -#include "utils/clocks_d51_module.h" -#include "drivers/ucbus_head.h" - -// should eventually just be this, -#include "smoothie/SmoothieRoll.h" - -union chunk_float32 { - uint8_t bytes[4]; - float f; -}; - -union chunk_uint32 { - uint8_t bytes[4]; - uint32_t u; -}; - -// adhoc reply -uint8_t reply[1024]; -uint16_t rl = 0; - -uint8_t replyBlankPck[1024]; -uint16_t replyBlankPl = 0; -uint16_t replyBlankPtr = 0; -uint16_t replyBlankSegsize = 0; -VPort* replyBlankVp; -uint16_t replyBlankVpi = 0; -uint16_t lastQueueSpaceTxd = 0; - -boolean needNewEmptySpaceReply = false; - -boolean smoothie_is_queue_empty(void){ - return conveyor->queue.is_empty(); -} - -boolean smoothie_is_moving(void){ - return (smoothieRoll->actuators[0]->is_moving() - || smoothieRoll->actuators[1]->is_moving() - || smoothieRoll->actuators[2]->is_moving() - || !smoothie_is_queue_empty()); -} - -// pck[ptr] == DK_APP -void OSAP::handleAppPacket(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t segsize, VPort* vp, uint16_t vpi, uint8_t pwp){ - // track end of header, to reply with - uint16_t replyPtr = ptr; - // (a hack) store one app packet, to format our replies with. do once - if(replyBlankPtr == 0){ - for(uint16_t i = 0; i < pl; i++){ - replyBlankPck[i] = pck[i]; - } - replyBlankPl = pl; - replyBlankPtr = ptr; - replyBlankSegsize = segsize; - replyBlankVp = vp; - replyBlankVpi = vpi; - } - // clear out our reply, - rl = 0; - reply[rl ++] = DK_APP; - // do the reading: - ptr ++; // walk appcode DK_APP - switch(pck[ptr]){ - case AK_GOTOPOS: { - ptr ++; // walk mocode - reply[rl ++] = AK_GOTOPOS; - // get positions - chunk_float32 targetChunks[3]; - targetChunks[0] = { .bytes = { pck[ptr ++], pck[ptr ++], pck[ptr ++], pck[ptr ++] } }; - targetChunks[1] = { .bytes = { pck[ptr ++], pck[ptr ++], pck[ptr ++], pck[ptr ++] } }; - targetChunks[2] = { .bytes = { pck[ptr ++], pck[ptr ++], pck[ptr ++], pck[ptr ++] } }; - // get feed - chunk_float32 feedrateChunk = { .bytes = { pck[ptr ++], pck[ptr ++], pck[ptr ++], pck[ptr ++] } }; - if(feedrateChunk.f < 0.01){ - sysError("ZERO FR"); - } - // can load move? - if(!(conveyor->is_queue_full())){ - // do load - // need to get last position from each, to do increment calc for planner - // that can all go in the planner, - float target[3] = {targetChunks[0].f, targetChunks[1].f, targetChunks[2].f}; - //sysError("targets, rate: " + String(target[0], 6) + ", " + String(target[1], 6) + ", " + String(target[2], 6) + ", " + String(feedrateChunk.f, 6)); - planner->append_move(target, 3, feedrateChunk.f); - } else { - // if we flowcontrol properly, this shouldn't appear - sysError("WRITE FULL"); - } - // reply if not full after push, - if(conveyor->is_queue_full()){ - // full, - needNewEmptySpaceReply = true; - } else { - ts_writeBoolean(true, reply, &rl); - if(vp->cts()){ - appReply(pck, pl, replyPtr, segsize, vp, vpi, reply, rl); - } else { - sysError("on reply, not cts, system fails"); - } - } - } - break; - case AK_SETPOS: - // only when queue empty and not moving, set current position - reply[rl ++] = AK_SETPOS; - // these are cancelled for now ... or ? are they ? - if(false){ - reply[rl ++] = AK_ERR; - ts_writeString("setPos remote is cancelled, use deltas from query'd position", reply, &rl); - } else if(smoothie_is_moving()){ - reply[rl ++] = AK_ERR; - ts_writeString("motion is happening, cannot set position on the fly", reply, &rl); - } else { - // will require that you operate a new bus command. - if(ucBusHead->cts_b() && !smoothie_is_moving()){ - ptr ++; - // same as currents, we can forward these posns', - //ucBusHead->transmit_b(&(pck[ptr]), 13); - // but also need to set our own position to this... - chunk_float32 setChunks[3]; - setChunks[0] = { .bytes = { pck[ptr ++], pck[ptr ++], pck[ptr ++], pck[ptr ++] } }; - setChunks[1] = { .bytes = { pck[ptr ++], pck[ptr ++], pck[ptr ++], pck[ptr ++] } }; - setChunks[2] = { .bytes = { pck[ptr ++], pck[ptr ++], pck[ptr ++], pck[ptr ++] } }; - // meaning... - // I'm not 100% on this code, will ofc test when homing is tested - float set[3] = {setChunks[0].f, setChunks[1].f, setChunks[2].f}; - planner->set_position(set, 3); - sysError("SET: " + String(setChunks[0].f, 3) + " " + String(setChunks[1].f, 3) + " " + String(setChunks[2].f, 3)); - // and reply OK - reply[rl ++] = AK_OK; - } - } - if(vp->cts()){ - appReply(pck, pl, replyPtr, segsize, vp, vpi, reply, rl); - } else { - sysError("on reply, not cts: set pos"); - } - break; - case AK_SETWAITTIME:{ - reply[rl ++] = AK_SETWAITTIME; - ptr ++; - chunk_uint32 setChunk = { .bytes = { pck[ptr ++], pck[ptr ++], pck[ptr ++], pck[ptr ++] } }; - conveyor->setWaitTime(setChunk.u); - if(vp->cts()){ - appReply(pck, pl, replyPtr, segsize, vp, vpi, reply, rl); - } else { - sysError("on reply, not cts: set pos"); - } - break; - } - case AK_SETCURRENT: - // should be able to put a new current-write out on the B channel, - // so long as it's clear - reply[rl ++] = AK_SETCURRENT; - if(ucBusHead->cts_b()){ - // this is basically a forward, or should be, - // pck[ptr] == AK_SETCURRENT, + 3*4 wide floats - // we can actually do this direct from pck -> bus outbuffer - ucBusHead->transmit_b(&(pck[ptr]), 13); - reply[rl ++] = AK_OK; - } else { - reply[rl ++] = AK_ERR; - ts_writeString("ucbus b-channel not clear, cannot write currents", reply, &rl); - } - if(vp->cts()){ - appReply(pck, pl, replyPtr, segsize, vp, vpi, reply, rl); - } else { - sysError("on reply, not cts: set current"); - } - break; - case AK_SETRPM: - // spindle rpm change - reply[rl ++] = AK_SETRPM; - if(ucBusHead->cts_b()){ // this is aaaaahn float, or uint32, either way: - ucBusHead->transmit_b(&(pck[ptr]), 5); - reply[rl ++] = AK_OK; - } else { - reply[rl ++] = AK_ERR; - ts_writeString("ucbus b-channel not clear, cannot write rpm", reply, &rl); - } - if(vp->cts()){ - appReply(pck, pl, replyPtr, segsize, vp, vpi, reply, rl); - } else { - sysError("on reply, not cts: set current"); - } - break; - case AK_QUERYMOVING: - // is currently ticking? - reply[rl ++] = AK_QUERYMOVING; - if(smoothieRoll->actuators[0]->is_moving() || smoothieRoll->actuators[1]->is_moving() || smoothieRoll->actuators[2]->is_moving()){ - ts_writeBoolean(true, reply, &rl); - } else { - ts_writeBoolean(false, reply, &rl); - } - if(vp->cts()){ - appReply(pck, pl, replyPtr, segsize, vp, vpi, reply, rl); - } else { - sysError("on reply, not cts: query moving"); - } - break; - case AK_QUERYPOS: - reply[rl ++] = AK_QUERYPOS; - ts_writeFloat32(smoothieRoll->actuators[0]->floating_position, reply, &rl); - ts_writeFloat32(smoothieRoll->actuators[1]->floating_position, reply, &rl); - ts_writeFloat32(smoothieRoll->actuators[2]->floating_position, reply, &rl); - if(vp->cts()){ - appReply(pck, pl, replyPtr, segsize, vp, vpi, reply, rl); - } else { - sysError("on reply, not cts, system fails"); - } - case AK_QUERYQUEUELEN: { - reply[rl ++] = AK_QUERYQUEUELEN; - // length of queue is 64 - available space - uint16_t ql = 64 - conveyor->queue_space(); - ts_writeUint16(ql, reply, &rl); - if(vp->cts()){ - appReply(pck, pl, replyPtr, segsize, vp, vpi, reply, rl); - } else { - sysError("on rpely, not cts: queue len query"); - } - } - break; - default: - sysError("nonreq. appkey " + String(pck[ptr])); - break; - #warning TODO: could do all if(cts) handles here, once, with if(rl > 1) ... - } - // always do, - vp->clearPacket(pwp); - /* - uint16_t qs = conveyor->queue_space(); - lastQueueSpaceTxd = qs; - ts_writeUint16(qs, reply, &rl); - // ship the reply - */ -} - -void setup() { - ERRLIGHT_SETUP; - CLKLIGHT_SETUP; - DEBUG1PIN_SETUP; - DEBUG2PIN_SETUP; - DEBUG3PIN_SETUP; - DEBUG4PIN_SETUP; - // osap - osap->description = "smoothie port and stepper driver"; - osap->addVPort(vPortSerial); - // bus - ucBusHead->init(); - // smoothie - smoothieRoll->init(); - // 100kHz base - d51_clock_boss->start_100kHz_ticker_tc0(); -} - -volatile uint8_t tick_count = 0; - -// TODO: this should be volatile, no? -uint8_t motion_packet[64]; // three floats bb, space - -void TC0_Handler(void){ - //DEBUG1PIN_ON; - TC0->COUNT32.INTFLAG.bit.MC0 = 1; - TC0->COUNT32.INTFLAG.bit.MC1 = 1; - tick_count ++; - // do bus action first: want downstream clocks to be deterministic-ish - ucBusHead->timerISR(); - // do step tick - smoothieRoll->step_tick(); - // every n ticks, ship position? - if(tick_count > 20){ - tick_count = 0; - uint16_t mpptr = 0; // motion packet pointer - if(planner->do_set_position){ - motion_packet[mpptr ++] = UB_AK_SETPOS; - planner->do_set_position = false; - } else { - motion_packet[mpptr ++] = UB_AK_GOTOPOS; - } - ts_writeFloat32(smoothieRoll->actuators[0]->floating_position, motion_packet, &mpptr); - ts_writeFloat32(smoothieRoll->actuators[1]->floating_position, motion_packet, &mpptr); - ts_writeFloat32(smoothieRoll->actuators[2]->floating_position, motion_packet, &mpptr); - // write packet, put on ucbus - //DEBUG3PIN_ON; - ucBusHead->transmit_a(motion_packet, 13); - //DEBUG3PIN_OFF; - } - //DEBUG1PIN_OFF; -} - -uint8_t testTxBytes[4] = {0, 2, 4, 8}; -uint8_t testTxLen = 4; - -uint8_t dropRead = 0; -uint8_t testRxBytes[1024]; -uint16_t testRxLen = 0; - -uint64_t lastTransmit = 0; -uint64_t lastRead = 0; - -uint16_t queueCheck = 0; - -uint32_t time = 0; -uint32_t lastTime = 0; -boolean once = true; - -#define SQUARE_SIDELEN 5.0F -#define OFFSET 0.0F - -// dummy moves -uint8_t squareMove = 3; -float square[4][3] = { - {0.0F + OFFSET, 0.0F + OFFSET, 0.0F + OFFSET}, - {0.0F + OFFSET, SQUARE_SIDELEN + OFFSET, 0.0F + OFFSET}, - {SQUARE_SIDELEN + OFFSET, SQUARE_SIDELEN + OFFSET, 0.0F + OFFSET}, - {SQUARE_SIDELEN + OFFSET, 0.0F + OFFSET, 0.0F + OFFSET} -}; - -float spot = 1; -float dummyTarget = 1; - -void loop() { - //DEBUG2PIN_TOGGLE; - osap->loop(); - conveyor->on_idle(nullptr); - // return new info? - // stepper-at-head spaces return - if(needNewEmptySpaceReply && !(conveyor->is_queue_full())){ - rl = 0; - reply[rl ++] = DK_APP; - reply[rl ++] = AK_GOTOPOS; - ts_writeBoolean(true, reply, &rl); - osap->appReply(replyBlankPck, replyBlankPl, replyBlankPtr, replyBlankSegsize, replyBlankVp, replyBlankVpi, reply, rl); - needNewEmptySpaceReply = false; - } - /* - queueCheck = conveyor->queue_space(); - if(queueCheck > lastQueueSpaceTxd){ - if(replyBlankPl > 0 && replyBlankVp->cts()){ - rl = 0; - reply[rl ++] = DK_APP; - lastQueueSpaceTxd = queueCheck; - ts_writeUint16(lastQueueSpaceTxd, reply, &rl); - osap->appReply(replyBlankPck, replyBlankPl, replyBlankPtr, replyBlankSegsize, replyBlankVp, replyBlankVpi, reply, rl); - } - } - */ - - // for periodic debug, - time = millis(); - if(time > lastTime + 10){ - lastTime = time; - //DEBUG3PIN_TOGGLE; - //DEBUG4PIN_TOGGLE; - if(!(conveyor->is_queue_full()) && false){ - //sysError("position: " + String(smoothieRoll->actuators[0]->floating_position) - // + " " + String(smoothieRoll->actuators[1]->floating_position)); - squareMove ++; - if(squareMove > 3){ - squareMove = 0; - } - //sysError("append: " + String(squareMove) + " " + String(square[squareMove][0]) + " " + String(square[squareMove][1]) + " " + String(square[squareMove][2])); - planner->append_move(square[squareMove], 3, 10); - // set flag for 'process immediate' - // or run another line to remote-set the queue_delay_time_ms - // conveyor::line 223 - /* - // thru append_move - float dummyPos[3]; - for(uint8_t m = 0; m < 3; m ++){ - dummyPos[m] = dummyTarget; - } - dummyTarget += spot; - spot += 1; - if(spot > 5){ - spot = 1; - } - planner->append_move(dummyPos, 3, 1); - */ - /* - // thru append_block - ActuatorCoordinates feedPos; - float sos = powf(spot, 2) * 3; - float dist = sqrtf(sos); - float unit[3] = {1 / dist, 1 / dist, 1 / dist}; - float s_value = 10; - // step, - for(uint8_t i = 0; i < k_max_actuators; i ++){ - feedPos[i] = spot; - } - planner->append_block(feedPos, 3, 100, dist, unit, 10, s_value, true); - */ - } - } -} // end loop diff --git a/firmware/osape-smoothieroll-head/src/osap/osap.cpp b/firmware/osape-smoothieroll-head/src/osap/osap.cpp deleted file mode 100644 index 4b4499c..0000000 --- a/firmware/osape-smoothieroll-head/src/osap/osap.cpp +++ /dev/null @@ -1,487 +0,0 @@ -/* -osap/osap.cpp - -virtual network node - -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 "osap.h" - -uint8_t ringFrame[1028]; - -OSAP::OSAP(String nodeName){ - name = nodeName; -} - -boolean OSAP::addVPort(VPort* vPort){ - if(_numVPorts > OSAP_MAX_VPORTS){ - return false; - } else { - vPort->init(); - _vPorts[_numVPorts ++] = vPort; - return true; - } -} - -void OSAP::forward(uint8_t *pck, uint16_t pl, uint16_t ptr, VPort *vp, uint8_t vpi, uint8_t pwp){ - sysError("NO FWD CODE YET"); - vp->clearPacket(pwp); -} - -void OSAP::write77(uint8_t *pck, VPort *vp){ - uint16_t one = 1; - pck[0] = PK_PPACK; // the '77' - uint16_t bufSpace = vp->getBufSpace(); - ts_writeUint16(bufSpace, pck, &one); - vp->lastRXBufferSpaceTransmitted = bufSpace; - vp->rxSinceTx = 0; -} - -// packet to read from, response to write into, write pointer, maximum response length -// assumes header won't be longer than received max seg length, if it arrived ... -// ptr = DK_x, ptr - 5 = PK_DEST, ptr - 6 = PK_PTR -boolean OSAP::formatResponseHeader(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t segsize, uint16_t checksum, VPort *vp, uint16_t vpi){ - // sanity check, this should be pingreq key - // sysError("FRH pck[ptr]: " + String(pck[ptr])); - // buf[(*ptr) ++] = val & 255 - // pck like: - // [rptr] [rend] [ptr] - // [77:3][dep0][e1][e2][e3][pk_ptr][pk_dest][acksegsize:2][checksum:2][dkey-req] - // response (will be) like: - // [wptr] - // [ptr] - // [77:3][dep0][pk_ptr][p3][p2][p1][pk_dest][acksegsize:2][checksum:2][dkey-res] - // ptr here will always indicate end of the header, - // leaves space until pck[3] for the 77-ack, which will write in later on, - // to do this, we read forwarding steps from e1 (incrementing read-ptr) - // and write in tail-to-head, (decrementing write ptr) - uint16_t wptr = ptr - 5; // to beginning of dest, segsize, checksum block - _res[wptr ++] = PK_DEST; - ts_writeUint16(segsize, _res, &wptr); - ts_writeUint16(checksum, _res, &wptr); - wptr -= 5; // back to start of this block, - // now find rptr beginning, - uint16_t rptr = 3; // departure port was trailing 77, - switch(pck[rptr]){ // walk to e1, ignoring original departure information - case PK_PORTF_KEY: - rptr += PK_PORTF_INC; - break; - case PK_BUSF_KEY: - rptr += PK_BUSF_INC; - break; - case PK_BUSB_KEY: - rptr += PK_BUSB_INC; - break; - default: - sysError("nonreq departure key on reverse route, bailing"); - return false; - } - // end switch, now pck[rptr] is at port-type-key of next fwd instruction - // walk rptr forwards, wptr backwards, copying in forwarding segments, max. 16 hops - uint16_t rend = ptr - 6; // read-end per static pck-at-dest end block: 6 for checksum(2) acksegsize(2), dest and ptr - for(uint8_t h = 0; h < 16; h ++){ - if(rptr >= rend){ // terminate when walking past end, - break; - } - switch(pck[rptr]){ - case PK_PORTF_KEY: - wptr -= PK_PORTF_INC; - for(uint8_t i = 0; i < PK_PORTF_INC; i ++){ - _res[wptr + i] = pck[rptr ++]; - } - break; - case PK_BUSF_KEY: - case PK_BUSB_KEY: - wptr -= PK_BUSF_INC; - for(uint8_t i = 0; i < PK_BUSF_INC; i ++){ - _res[wptr + i] = pck[rptr ++]; - } - default: - sysError("rptr: " + String(rptr) + " key here: " + String(pck[rptr])); - sysError("couldn't reverse this path"); - return false; - } - } - // following route-copy-in, - // TODO mod this for busses, - wptr -= 4; - _res[wptr ++] = PK_PORTF_KEY; /// write in departure key type, - ts_writeUint16(vpi, _res, &wptr); // write in departure port, - _res[wptr ++] = PK_PTR; // ptr follows departure key, - // to check, wptr should now be at 7: for 77(3), departure(3:portf), ptr(1) - if(wptr != 7){ // wptr != 7 - sysError("bad response header write"); - return false; - } else { - return true; - } -} - -/* -await osap.query(nextRoute, 'name', 'numVPorts') -await osap.query(nextRoute, 'vport', np, 'name', 'portTypeKey', 'portStatus', 'maxSegLength') -*/ - -void OSAP::writeQueryDown(uint16_t *wptr){ - sysError("QUERYDOWN"); - _res[(*wptr) ++] = EP_ERRKEY; - _res[(*wptr) ++] = EP_ERRKEY_QUERYDOWN; -} - -void OSAP::writeEmpty(uint16_t *wptr){ - sysError("EMPTY"); - _res[(*wptr) ++] = EP_ERRKEY; - _res[(*wptr) ++] = EP_ERRKEY_EMPTY; -} - -// queries for ahn vport, -void OSAP::readRequestVPort(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t rptr, uint16_t *wptr, uint16_t segsize, VPort* vp){ - // could be terminal, could read into endpoints (input / output) of the vport, - for(uint8_t i = 0; i < 16; i ++){ - if(rptr >= pl){ - return; - } - if(*wptr > segsize){ - sysError("QUERYDOWN wptr: " + String(*wptr) + " segsize: " + String(segsize)); - *wptr = ptr; - writeQueryDown(wptr); - return; - } - switch(pck[rptr]){ - case EP_NUMINPUTS: - _res[(*wptr) ++] = EP_NUMINPUTS; - ts_writeUint16(0, _res, wptr); // TODO: vports can have inputs / outputs, - rptr ++; - break; - case EP_NUMOUTPUTS: - _res[(*wptr) ++] = EP_NUMOUTPUTS; - ts_writeUint16(0, _res, wptr); - rptr ++; - break; - case EP_INPUT: - case EP_OUTPUT: - writeEmpty(wptr); // ATM, these just empty - and then return, further args would be for dive - return; - case EP_NAME: - _res[(*wptr) ++] = EP_NAME; - ts_writeString(vp->name, _res, wptr); - rptr ++; - break; - case EP_DESCRIPTION: - _res[(*wptr) ++] = EP_DESCRIPTION; - ts_writeString(vp->description, _res, wptr); - rptr ++; - break; - case EP_PORTTYPEKEY: - _res[(*wptr) ++] = EP_PORTTYPEKEY; // TODO for busses - _res[(*wptr) ++] = vp->portTypeKey; - rptr ++; - break; - case EP_MAXSEGLENGTH: - _res[(*wptr) ++] = EP_MAXSEGLENGTH; - ts_writeUint32(vp->maxSegLength, _res, wptr); - rptr ++; - break; - case EP_PORTSTATUS: - _res[(*wptr) ++] = EP_PORTSTATUS; - ts_writeBoolean(vp->status, _res, wptr); - rptr ++; - break; - case EP_PORTBUFSPACE: - _res[(*wptr) ++] = EP_PORTBUFSPACE; - ts_writeUint16(vp->getBufSpace(), _res, wptr); - rptr ++; - break; - case EP_PORTBUFSIZE: - _res[(*wptr) ++] = EP_PORTBUFSIZE; - ts_writeUint16(vp->getBufSize(), _res, wptr); - rptr ++; - break; - default: - writeEmpty(wptr); - return; - } - } -} - -void OSAP::handleReadRequest(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t segsize, VPort* vp, uint16_t vpi, uint8_t pwp){ - if(vp->cts()){ - // resp. code, - // readptr, - uint16_t rptr = ptr + 1; // this will pass the RREQ and ID bytes, next is first query key - uint16_t wptr = ptr; - _res[wptr ++] = DK_RRES; - _res[wptr ++] = pck[rptr ++]; - _res[wptr ++] = pck[rptr ++]; - // read items, - // ok, walk those keys - uint16_t indice = 0; - for(uint8_t i = 0; i < 16; i ++){ - if(rptr >= pl){ - goto endwalk; - } - if(wptr > segsize){ - sysError("QUERYDOWN wptr: " + String(wptr) + " segsize: " + String(segsize)); - wptr = ptr; - writeQueryDown(&wptr); - goto endwalk; - } - switch(pck[rptr]){ - // first up, handle dives which downselect the tree - case EP_VPORT: - rptr ++; - ts_readUint16(&indice, pck, &rptr); - if(indice < _numVPorts){ - _res[wptr ++] = EP_VPORT; - ts_writeUint16(indice, _res, &wptr); - readRequestVPort(pck, pl, ptr, rptr, &wptr, segsize, _vPorts[indice]); - } else { - writeEmpty(&wptr); - } - goto endwalk; - case EP_VMODULE: - writeEmpty(&wptr); - goto endwalk; - // for reading any top-level item, - case EP_NUMVPORTS: - _res[wptr ++] = EP_NUMVPORTS; - ts_writeUint16(_numVPorts, _res, &wptr); - rptr ++; - break; - case EP_NUMVMODULES: - _res[wptr ++] = EP_NUMVMODULES; - ts_writeUint16(_numVModules, _res, &wptr); - rptr ++; - break; - case EP_NAME: - _res[wptr ++] = EP_NAME; - ts_writeString(name, _res, &wptr); - rptr ++; - break; - case EP_DESCRIPTION: - _res[wptr ++] = EP_DESCRIPTION; - ts_writeString(description, _res, &wptr); - rptr ++; - break; - // the default: unclear keys nullify entire response - default: - writeEmpty(&wptr); - goto endwalk; - } // end 1st switch - } - endwalk: ; - //sysError("QUERY ENDWALK, ptr: " + String(ptr) + " wptr: " + String(wptr)); - if(formatResponseHeader(pck, pl, ptr, segsize, wptr - ptr, vp, vpi)){ - vp->clearPacket(pwp); - write77(_res, vp); - vp->sendPacket(_res, wptr); - vp->decrimentRecipBufSpace(); - } else { - sysError("bad response format"); - vp->clearPacket(pwp); - } - } else { - vp->clearPacket(pwp); - } -} - -// pck[ptr] == DK_PINGREQ -void OSAP::handlePingRequest(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t segsize, VPort* vp, uint16_t vpi, uint8_t pwp){ - if(vp->cts()){ // resp. path is clear, can write resp. and ship it - // the reversed header will be *the same length* as the received header, - // which was from 0-ptr! - this is great news, we can say: - uint16_t wptr = ptr; // start writing here, leaves room for the header, - _res[wptr ++] = DK_PINGRES; // write in whatever the response is, here just the ping-res key and id - _res[wptr ++] = pck[ptr + 1]; - _res[wptr ++] = pck[ptr + 2]; - // this'll be the 'std' response formatting codes, - // formatResponseHeader doesn't need the _res, that's baked in, and it writes 0-ptr, - // since we wrote into _res following ptr, (header lengths identical), this is safe, - if(formatResponseHeader(pck, pl, ptr, segsize, 3, vp, vpi)){ // write the header: this goes _resp[3] -> _resp[ptr] - vp->clearPacket(pwp); // can now rm the packet, have gleaned all we need from it, - write77(_res, vp); // and *after* rm'ing it, report open space _resp[0] -> _resp[3]; - vp->sendPacket(_res, wptr); // this fn' call should copy-out of our buffer - vp->decrimentRecipBufSpace(); - } else { - sysError("bad response format"); - vp->clearPacket(pwp); - } - } else { - vp->clearPacket(pwp); - } -} - -// write and send ahn reply out, -void OSAP::appReply(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t segsize, VPort* vp, uint16_t vpi, uint8_t *reply, uint16_t rl){ - uint16_t wptr = ptr; - for(uint16_t i = 0; i < rl; i ++){ - _res[wptr ++] = reply[i]; - } - if(formatResponseHeader(pck, pl, ptr, segsize, rl, vp, vpi)){ - write77(_res, vp); - vp->sendPacket(_res, wptr); - vp->decrimentRecipBufSpace(); - } else { - sysError("bad response format"); - } -} - -// frame: the buffer, ptr: the location of the ptr (ack or pack), -// vp: port received on, fwp: frame-write-ptr, -// so vp->frames[fwp] = frame, though that isn't exposed here -void OSAP::instructionSwitch(uint8_t *pck, uint16_t pl, uint16_t ptr, VPort *vp, uint8_t vpi, uint8_t pwp){ - // we must *do* something, and (ideally) pop this thing, - switch(pck[ptr]){ - case PK_PORTF_KEY: - case PK_BUSF_KEY: - case PK_BUSB_KEY: - forward(pck, pl, ptr, vp, vpi, pwp); - break; - case PK_DEST: { - ptr ++; // walk past dest key, - uint16_t segsize = 0; - uint16_t checksum = 0; - ts_readUint16(&segsize, pck, &ptr); - ts_readUint16(&checksum, pck, &ptr); - if(checksum != pl - ptr){ - sysError("bad checksum, count: " + String(pl - ptr) + " checksum: " + String(checksum)); - vp->clearPacket(pwp); - } else { - switch(pck[ptr]){ - case DK_APP: - handleAppPacket(pck, pl, ptr, segsize, vp, vpi, pwp); - break; - case DK_PINGREQ: - handlePingRequest(pck, pl, ptr, segsize, vp, vpi, pwp); - break; - case DK_RREQ: - handleReadRequest(pck, pl, ptr, segsize, vp, vpi, pwp); - break; - break; - case DK_WREQ: // no writing yet, - case DK_PINGRES: // not issuing pings from embedded, shouldn't have deal w/ their responses - case DK_RRES: // not issuing requests from embedded, same - case DK_WRES: // not issuing write requests from embedded, again - sysError("WREQ or RES received in embedded, popping"); - vp->clearPacket(pwp); - break; - default: - sysError("non-recognized destination key, popping"); - vp->clearPacket(pwp); - break; - } - } - } - break; - default: - // packet is unrecognized, - sysError("unrecognized instruction key"); - vp->clearPacket(pwp); - break; - } -} - -void OSAP::loop(){ - /* - Also a note - the vp->getFrame(); (which is called often in the loop) doesn't have to be a virtual f. - VPorts can have private \_frames** ptrs-to, and when we start up a vport class, - point that at some statically allocated heap. - also, set a \_numFrames and ahn \_writePtrs*. - */ - /* - another note - this was measured around 25us (long!) - so it would be *tite* if that coule be decreased, especially in recognizing the no-op cases, - where execution could be very - very - small. - */ - unsigned long pat = 0; // packet arrival time - VPort* vp; // vp of vports - unsigned long now = millis(); - // pull one frame per loop per port, - // TODO: can increase speed by pulling more frames per loop ?? - for(uint8_t p = 0; p < _numVPorts; p ++){ - vp = _vPorts[p]; - vp->loop(); // affordance to run phy code, - for(uint8_t t = 0; t < 4; t ++){ // count # of packets to process per port per turn - uint8_t* pck; // the packet we are handling - uint16_t pl = 0; // length of that packet - uint8_t pwp = 0; // packet write pointer: where it was, to write-back clearance - vp->getPacket(&pck, &pl, &pwp, &pat); // gimme the bytes - if(pl > 0){ // have length, will try, - // check prune stale, - if(pat + OSAP_STALETIMEOUT < now){ - //this untested, but should work, yeah? - //sysError("prune stale message on " + String(vp->name)); - vp->clearPacket(pwp); - continue; - } - // check / handle pck - uint16_t ptr = 0; - // new rcrbx? - if(pck[ptr] == PK_PPACK){ - ptr ++; - uint16_t rcrxs; - ts_readUint16(&rcrxs, pck, &ptr); - vp->setRecipRxBufSpace(rcrxs); - } - // anything else? - if(ptr < pl){ - // walk through for instruction, - for(uint8_t i = 0; i < 16; i ++){ - switch(pck[ptr]){ - case PK_PTR: - instructionSwitch(pck, pl, ptr + 1, vp, p, pwp); - goto endWalk; - case PK_PORTF_KEY: // previous instructions, walk over, - ptr += PK_PORTF_INC; - break; - case PK_BUSF_KEY: - ptr += PK_BUSF_INC; - break; - case PK_BUSB_KEY: - ptr += PK_BUSF_INC; - break; - case PK_LLERR: - // someone forwarded us an err-escape, - // we are kind of helpless to help, just escp. - vp->clearPacket(pwp); - goto endWalk; - default: - sysError("bad walk for ptr: key " + String(pck[ptr]) + " at: " + String(ptr)); - vp->clearPacket(pwp); - goto endWalk; - } // end switch - } // end loop for ptr walk, - } else { - // that was just the rcrbx then, - vp->clearPacket(pwp); - } - } else { - break; - } // no frames in this port, - // end of this-port-scan, - endWalk: ; - } // end up-to-8-packets-per-turn - } // end loop over ports (handling rx) - // loop for keepalive conditions, - for(uint8_t p = 0; p < _numVPorts; p ++){ - vp = _vPorts[p]; - // check if needs to tx keepalive, - uint16_t currentRXBufferSpace = vp->getBufSpace(); - if(currentRXBufferSpace > vp->lastRXBufferSpaceTransmitted || vp->lastTxTime + OSAP_TXKEEPALIVEINTERVAL < now){ - // has open space not reported, or needs to ping for port keepalive - if(vp->cts()){ - write77(_res, vp); - vp->sendPacket(_res, 3); - vp->decrimentRecipBufSpace(); - } - } - } // end loop over ports (keepalive) -} \ No newline at end of file diff --git a/firmware/osape-smoothieroll-head/src/osap/osap.h b/firmware/osape-smoothieroll-head/src/osap/osap.h deleted file mode 100644 index 5242ba0..0000000 --- a/firmware/osape-smoothieroll-head/src/osap/osap.h +++ /dev/null @@ -1,60 +0,0 @@ -/* -osap/osap.h - -virtual network node - -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 OSAP_H_ -#define OSAP_H_ - -#include <arduino.h> -#include "ts.h" -#include "vport.h" -#include "./drivers/indicators.h" -#include "./utils/cobs.h" - -#define OSAP_MAX_VPORTS 16 -#define RES_LENGTH 2048 -#define OSAP_STALETIMEOUT 600 -#define OSAP_TXKEEPALIVEINTERVAL 300 - -class OSAP { -private: - // yonder ports, - VPort* _vPorts[16]; - uint8_t _numVPorts = 0; - // yither vmodules - uint8_t _numVModules = 0; - // dishing output, temp write buffer - uint8_t _res[RES_LENGTH]; -public: - OSAP(String nodeName); - // props - String name; - String description = "undescribed osap node"; - // fns - boolean addVPort(VPort* vPort); - void forward(uint8_t *pck, uint16_t pl, uint16_t ptr, VPort *vp, uint8_t vpi, uint8_t pwp); - void write77(uint8_t *pck, VPort *vp); - boolean formatResponseHeader(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t segsize, uint16_t checksum, VPort *vp, uint16_t vpi); - void writeQueryDown(uint16_t *wptr); - void writeEmpty(uint16_t *wptr); - void readRequestVPort(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t rptr, uint16_t *wptr, uint16_t segsize, VPort* vp); - void handleReadRequest(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t segsize, VPort* vp, uint16_t vpi, uint8_t pwp); - void handlePingRequest(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t segsize, VPort* vp, uint16_t vpi, uint8_t pwp); - void appReply(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t segsize, VPort* vp, uint16_t vpi, uint8_t *reply, uint16_t rl); - void instructionSwitch(uint8_t *pck, uint16_t pl, uint16_t ptr, VPort *vp, uint8_t vpi, uint8_t pwp); - void loop(); - // the handoff, - void handleAppPacket(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t segsize, VPort* vp, uint16_t vpi, uint8_t pwp); -}; - -#endif diff --git a/firmware/osape-smoothieroll-head/src/osap/ts.cpp b/firmware/osape-smoothieroll-head/src/osap/ts.cpp deleted file mode 100644 index 6fb3474..0000000 --- a/firmware/osape-smoothieroll-head/src/osap/ts.cpp +++ /dev/null @@ -1,82 +0,0 @@ -/* -osap/ts.cpp - -typeset / keys / writing / reading - -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 "ts.h" - -void ts_writeBoolean(boolean val, unsigned char *buf, uint16_t *ptr){ - if(val){ - buf[(*ptr) ++] = 1; - } else { - buf[(*ptr) ++] = 0; - } -} - -void ts_readUint16(uint16_t *val, unsigned char *buf, uint16_t *ptr){ - *val = buf[(*ptr) + 1] << 8 | buf[(*ptr)]; - *ptr += 2; -} - -void ts_writeUint16(uint16_t val, unsigned char *buf, uint16_t *ptr){ - buf[(*ptr) ++] = val & 255; - buf[(*ptr) ++] = (val >> 8) & 255; -} - -void ts_writeUint32(uint32_t val, unsigned char *buf, uint16_t *ptr){ - buf[(*ptr) ++] = val & 255; - buf[(*ptr) ++] = (val >> 8) & 255; - buf[(*ptr) ++] = (val >> 16) & 255; - buf[(*ptr) ++] = (val >> 24) & 255; -} - -union chunk_float32 { - uint8_t bytes[4]; - float f; -}; - -void ts_writeFloat32(float val, volatile unsigned char *buf, uint16_t *ptr){ - chunk_float32 chunk; - chunk.f = val; - buf[(*ptr) ++] = chunk.bytes[0]; - buf[(*ptr) ++] = chunk.bytes[1]; - buf[(*ptr) ++] = chunk.bytes[2]; - buf[(*ptr) ++] = chunk.bytes[3]; -} - -union chunk_float64 { - uint8_t bytes[8]; - double f; -}; - -void ts_writeFloat64(double val, volatile unsigned char *buf, uint16_t *ptr){ - chunk_float64 chunk; - chunk.f = val; - buf[(*ptr) ++] = chunk.bytes[0]; - buf[(*ptr) ++] = chunk.bytes[1]; - buf[(*ptr) ++] = chunk.bytes[2]; - buf[(*ptr) ++] = chunk.bytes[3]; - buf[(*ptr) ++] = chunk.bytes[4]; - buf[(*ptr) ++] = chunk.bytes[5]; - buf[(*ptr) ++] = chunk.bytes[6]; - buf[(*ptr) ++] = chunk.bytes[7]; -} - -void ts_writeString(String val, unsigned char *buf, uint16_t *ptr){ - uint32_t len = val.length(); - buf[(*ptr) ++] = len & 255; - buf[(*ptr) ++] = (len >> 8) & 255; - buf[(*ptr) ++] = (len >> 16) & 255; - buf[(*ptr) ++] = (len >> 24) & 255; - val.getBytes(&buf[*ptr], len + 1); - *ptr += len; -} diff --git a/firmware/osape-smoothieroll-head/src/osap/ts.h b/firmware/osape-smoothieroll-head/src/osap/ts.h deleted file mode 100644 index 7cf3f01..0000000 --- a/firmware/osape-smoothieroll-head/src/osap/ts.h +++ /dev/null @@ -1,103 +0,0 @@ -/* -osap/ts.h - -typeset / keys / writing / reading - -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 <arduino.h> - -// -------------------------------------------------------- Routing (Packet) Keys - -#define PK_PPACK 77 -#define PK_PTR 88 -#define PK_DEST 99 -#define PK_LLERR 44 -#define PK_PORTF_KEY 11 -#define PK_PORTF_INC 3 -#define PK_BUSF_KEY 12 -#define PK_BUSF_INC 5 -#define PK_BUSB_KEY 14 -#define PK_BUSB_INC 5 - -// -------------------------------------------------------- Destination Keys (arrival layer) - -#define DK_APP 100 // application codes, go to -> main -#define DK_PINGREQ 101 // ping request -#define DK_PINGRES 102 // ping reply -#define DK_RREQ 111 // read request -#define DK_RRES 112 // read response -#define DK_WREQ 113 // write request -#define DK_WRES 114 // write response - -// -------------------------------------------------------- Application Keys - -#define AK_OK 100 -#define AK_ERR 200 -#define AK_GOTOPOS 101 // goto pos -#define AK_SETPOS 102 // set position to xyz -#define AK_SETCURRENT 103 // set currents xyz -#define AK_SETWAITTIME 104 // set queue wait time -#define AK_SETRPM 105 // set spindle -#define AK_QUERYMOVING 111 // is moving? -#define AK_QUERYPOS 112 // get current pos -#define AK_QUERYQUEUELEN 113 // current queue len? - -// -------------------------------------------------------- MVC Endpoints - -#define EP_ERRKEY 150 -#define EP_ERRKEY_QUERYDOWN 151 -#define EP_ERRKEY_EMPTY 153 -#define EP_ERRKEY_UNCLEAR 154 - -#define EP_NAME 171 -#define EP_DESCRIPTION 172 - -#define EP_NUMVPORTS 181 -#define EP_VPORT 182 -#define EP_PORTTYPEKEY 183 -#define EP_MAXSEGLENGTH 184 -#define EP_PORTSTATUS 185 -#define EP_PORTBUFSPACE 186 -#define EP_PORTBUFSIZE 187 - -#define EP_NUMVMODULES 201 -#define EP_VMODULE 202 - -#define EP_NUMINPUTS 211 -#define EP_INPUT 212 - -#define EP_NUMOUTPUTS 221 -#define EP_OUTPUT 222 - -#define EP_TYPE 231 -#define EP_VALUE 232 -#define EP_STATUS 233 - -#define EP_NUMROUES 243 -#define EP_ROUTE 235 - -// ... etc, later - -// -------------------------------------------------------- Reading and Writing - -void ts_writeBoolean(boolean val, unsigned char *buf, uint16_t *ptr); - -void ts_readUint16(uint16_t *val, uint8_t *buf, uint16_t *ptr); - -void ts_writeUint16(uint16_t val, unsigned char *buf, uint16_t *ptr); - -void ts_writeUint32(uint32_t val, unsigned char *buf, uint16_t *ptr); - -void ts_writeFloat32(float val, volatile unsigned char *buf, uint16_t *ptr); - -void ts_writeFloat64(double val, volatile unsigned char *buf, uint16_t *ptr); - -void ts_writeString(String val, unsigned char *buf, uint16_t *ptr); diff --git a/firmware/osape-smoothieroll-head/src/osap/vport.cpp b/firmware/osape-smoothieroll-head/src/osap/vport.cpp deleted file mode 100644 index e064991..0000000 --- a/firmware/osape-smoothieroll-head/src/osap/vport.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/* -osap/vport.cpp - -virtual port, p2p - -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 "vport.h" - -VPort::VPort(String vPortName){ - name = vPortName; -} - -void VPort::setRecipRxBufSpace(uint16_t len){ - _recipRxBufSpace = len; -} - -void VPort::decrimentRecipBufSpace(void){ - if(_recipRxBufSpace < 1){ - _recipRxBufSpace = 0; - } else { - _recipRxBufSpace --; - } - lastTxTime = millis(); -} - -boolean VPort::cts(void){ - if(_recipRxBufSpace > 0 && status){ - return true; - } else { - return false; - } -} diff --git a/firmware/osape-smoothieroll-head/src/osap/vport.h b/firmware/osape-smoothieroll-head/src/osap/vport.h deleted file mode 100644 index b705e91..0000000 --- a/firmware/osape-smoothieroll-head/src/osap/vport.h +++ /dev/null @@ -1,52 +0,0 @@ -/* -osap/vport.h - -virtual port, p2p - -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 VPORT_H_ -#define VPORT_H_ - -#include <arduino.h> -#include "./utils/syserror.h" - -class VPort { -private: - uint16_t _recipRxBufSpace = 1; -public: - VPort(String vPortName); - String name; - String description = "undescribed vport"; - uint8_t portTypeKey = PK_PORTF_KEY; - uint16_t maxSegLength = 0; - virtual void init(void) = 0; - virtual void loop(void) = 0; - // keepalive log - uint16_t lastRXBufferSpaceTransmitted = 0; - uint16_t rxSinceTx = 0; // debugging: count packets received since last spaces txd - unsigned long lastTxTime = 0; - // handling incoming frames, - virtual void getPacket(uint8_t** pck, uint16_t* pl, uint8_t* pwp, unsigned long* pat) = 0; - // *be sure* that getPacket sets pl to zero if no packet emerges, - // consider making boolean return, true if packet? - virtual void clearPacket(uint8_t pwp) = 0; - virtual uint16_t getBufSpace(void) = 0; - virtual uint16_t getBufSize(void) = 0; - // dish outgoing frames, and check if open to send them? - boolean status = false; // open / closed-ness -> OSAP can set, VP can set. - virtual boolean cts(void); // is a connection established & is the reciprocal buffer nonzero? - virtual void sendPacket(uint8_t* pck, uint16_t pl) = 0; // take this frame, copying out of the buffer I pass you - // internal state, - void setRecipRxBufSpace(uint16_t len); - void decrimentRecipBufSpace(void); -}; - -#endif diff --git a/firmware/osape-smoothieroll-head/src/osap/vport_usbserial.cpp b/firmware/osape-smoothieroll-head/src/osap/vport_usbserial.cpp deleted file mode 100644 index ddbc9fd..0000000 --- a/firmware/osape-smoothieroll-head/src/osap/vport_usbserial.cpp +++ /dev/null @@ -1,141 +0,0 @@ -/* -osap/vport.cpp - -virtual port, p2p - -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 "vport_usbserial.h" - -VPort_USBSerial::VPort_USBSerial() -:VPort("usb serial"){ - description = "vport wrap on arduino Serial object"; - // ok, just calls super-constructor -} - -void VPort_USBSerial::init(void){ - // set frame lengths to zero, - for(uint8_t i = 0; i < VPUSB_NUM_SPACES; i ++){ - _pl[i] = 0; - } - Serial.begin(9600); -} - -void VPort_USBSerial::loop(void){ - while(Serial.available()){ - _encodedPacket[_pwp][_bwp] = Serial.read(); - if(_encodedPacket[_pwp][_bwp] == 0){ - rxSinceTx ++; - // sysError(String(getBufSpace()) + " " + String(_bwp)); - // indicate we recv'd zero - // CLKLIGHT_TOGGLE; - // decode from rx-ing frame to interface frame, - status = true; // re-assert open whenever received packet incoming - size_t dcl = cobsDecode(_encodedPacket[_pwp], _bwp, _packet[_pwp]); - _pl[_pwp] = dcl; // this frame now available, has this length, - _packetArrivalTimes[_pwp] = millis(); // time this thing arrived - // reset byte write pointer - _bwp = 0; - // find next empty frame, that's new frame write pointer - boolean set = false; - for(uint8_t i = 0; i <= VPUSB_NUM_SPACES; i ++){ - _pwp ++; - if(_pwp >= VPUSB_NUM_SPACES){ _pwp = 0; } - if(_pl[_pwp] == 0){ // if this frame-write-ptr hasn't been set to occupied, - set = true; - break; // this _pwp is next empty frame, - } - } - if(!set){ - sysError("no empty slot for serial read, protocol error!"); - uint16_t apparentSpace = getBufSpace(); - sysError("reads: " + String(apparentSpace)); - sysError("last txd recip: " + String(lastRXBufferSpaceTransmitted)); - sysError("rxd since last tx: " + String(rxSinceTx)); - sysError(String(_pl[0])); - sysError(String(_pl[1])); - sysError(String(_pl[2])); - sysError(String(_pl[3])); - sysError(String(_pl[4])); - sysError(String(_pl[5])); - sysError(String(_pl[6])); - sysError(String(_pl[7])); - sysError(String(_pl[8])); - delay(5000); - } - } else { - _bwp ++; - } - } -} - -void VPort_USBSerial::getPacket(uint8_t **pck, uint16_t *pl, uint8_t *pwp, unsigned long* pat){ - uint8_t p = _lastPacket; // the last one we delivered, - boolean retrieved = false; - for(uint8_t i = 0; i <= VPUSB_NUM_SPACES; i ++){ - p ++; - if(p >= VPUSB_NUM_SPACES) { p = 0; } - if(_pl[p] > 0){ // this is an occupied packet, deliver that - *pck = _packet[p]; // same, is this passing the ptr, yeah? - *pl = _pl[p]; // I *think* this is how we do this in c? - *pwp = p; // packet write pointer ? the indice of the packet, to clear - *pat = _packetArrivalTimes[p]; - _lastPacket = p; - retrieved = true; - break; - } - } - if(!retrieved){ - *pl = 0; - } -} - -void VPort_USBSerial::clearPacket(uint8_t pwp){ - // frame consumed, clear to write-in, - //sysError("clear " + String(pwp)); - _pl[pwp] = 0; - _packetArrivalTimes[pwp] = 0; -} - -uint16_t VPort_USBSerial::getBufSize(void){ - return VPUSB_NUM_SPACES; -} - -uint16_t VPort_USBSerial::getBufSpace(void){ - uint16_t sum = 0; - // any zero-length frame is not full, - for(uint16_t i = 0; i < VPUSB_NUM_SPACES; i++){ - if(_pl[i] == 0){ - sum ++; - } - } - // but one is being written into, - //if(_bwp > 0){ - sum --; - //} - // if we're very full this might wrap / invert, so - if(sum > VPUSB_NUM_SPACES){ - sum = 0; - } - // arrivaderci - return sum; -} - -void VPort_USBSerial::sendPacket(uint8_t *pck, uint16_t pl){ - size_t encLen = cobsEncode(pck, pl, _encodedOut); - if(Serial.availableForWrite()){ - //DEBUG1PIN_ON; - Serial.write(_encodedOut, encLen); - Serial.flush(); - //DEBUG1PIN_OFF; - } else { - sysError("NOT AVAILABLE"); - } -} diff --git a/firmware/osape-smoothieroll-head/src/osap/vport_usbserial.h b/firmware/osape-smoothieroll-head/src/osap/vport_usbserial.h deleted file mode 100644 index 4a72d1c..0000000 --- a/firmware/osape-smoothieroll-head/src/osap/vport_usbserial.h +++ /dev/null @@ -1,57 +0,0 @@ -/* -osap/vport_usbserial.h - -virtual port, p2p - -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 VPORT_USBSERIAL_H_ -#define VPORT_USBSERIAL_H_ - -#include <arduino.h> -#include "vport.h" -#include "./utils/cobs.h" -#include "./drivers/indicators.h" - -#define VPUSB_NUM_SPACES 64 -#define VPUSB_SPACE_SIZE 1028 - -class VPort_USBSerial : public VPort { -private: - // unfortunately, looks like we need to write-in to temp, - // and decode out of that - uint8_t _encodedPacket[VPUSB_NUM_SPACES][VPUSB_SPACE_SIZE]; - uint8_t _packet[VPUSB_NUM_SPACES][VPUSB_SPACE_SIZE]; - volatile uint16_t _pl[VPUSB_NUM_SPACES]; - unsigned long _packetArrivalTimes[VPUSB_NUM_SPACES]; - uint8_t _pwp = 0; // packet write pointer, - uint16_t _bwp = 0; // byte write pointer, - uint8_t _lastPacket = 0; // last packet written into - // outgoing cobs-copy-in, - uint8_t _encodedOut[VPUSB_SPACE_SIZE]; - // this is just for debug, - uint8_t _ringPacket[VPUSB_SPACE_SIZE]; -public: - VPort_USBSerial(); - // props - uint16_t maxSegLength = VPUSB_SPACE_SIZE - 6; - // code - void init(void); - void loop(void); - // handle incoming frames - void getPacket(uint8_t** pck, uint16_t* pl, uint8_t* pwp, unsigned long* pat); - void clearPacket(uint8_t pwp); - uint16_t getBufSize(void); - uint16_t getBufSpace(void); - // dish outgoing frames, and check if cts - void sendPacket(uint8_t *pck, uint16_t pl); -}; - -#endif diff --git a/firmware/osape-smoothieroll-head/src/smoothie/SmoothieRoll.cpp b/firmware/osape-smoothieroll-head/src/smoothie/SmoothieRoll.cpp deleted file mode 100644 index 41ec8ee..0000000 --- a/firmware/osape-smoothieroll-head/src/smoothie/SmoothieRoll.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/* -SmoothieRoll.cpp - -bottle & state container for the SmoothieWare instance running here - -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 "SmoothieRoll.h" - -SmoothieRoll* SmoothieRoll::instance = 0; - -SmoothieRoll* SmoothieRoll::getInstance(void){ - if(instance == 0){ - instance = new SmoothieRoll(); - } - return instance; -} - -SmoothieRoll* smoothieRoll = SmoothieRoll::getInstance(); - -SmoothieRoll::SmoothieRoll(void){ - -} - -void SmoothieRoll::init(void){ - // make motors - for(uint8_t m = 0; m < SMOOTHIEROLL_NUM_MOTORS; m ++){ - actuators[m] = new StepInterface(); - } - stepTicker->init(); - stepTicker->start(); - conveyor->on_module_loaded(); - conveyor->start(3); -} - -void SmoothieRoll::step_tick(void){ - stepTicker->step_tick(); -} \ No newline at end of file diff --git a/firmware/osape-smoothieroll-head/src/smoothie/SmoothieRoll.h b/firmware/osape-smoothieroll-head/src/smoothie/SmoothieRoll.h deleted file mode 100644 index c914581..0000000 --- a/firmware/osape-smoothieroll-head/src/smoothie/SmoothieRoll.h +++ /dev/null @@ -1,43 +0,0 @@ -/* -SmoothieRoll.h - -bottle & state container for the SmoothieWare instance running here - -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 SMOOTHIEROLL_H_ -#define SMOOTHIEROLL_H_ - -#include <Arduino.h> -// get top level stepTicker, conveyor... -#include "smoothie/libs/StepTicker.h" // has singleton 'stepTicker' -#include "smoothie/modules/robot/Conveyor.h" // has singleton 'conveyor' -#include "smoothie/modules/robot/Planner.h" // has singleton 'planner' -// motor state-trackers -#include "modules/robot/StepInterface.h" - -#define SMOOTHIEROLL_NUM_MOTORS 3 - -class SmoothieRoll{ - public: - SmoothieRoll(void); - static SmoothieRoll* getInstance(void); - void init(void); - void step_tick(void); - - StepInterface* actuators[SMOOTHIEROLL_NUM_MOTORS]; - - private: - static SmoothieRoll* instance; -}; - -extern SmoothieRoll* smoothieRoll; - -#endif \ No newline at end of file diff --git a/firmware/osape-smoothieroll-head/src/smoothie/libs/HeapRing.cpp b/firmware/osape-smoothieroll-head/src/smoothie/libs/HeapRing.cpp deleted file mode 100644 index fbc3955..0000000 --- a/firmware/osape-smoothieroll-head/src/smoothie/libs/HeapRing.cpp +++ /dev/null @@ -1,255 +0,0 @@ -#include "HeapRing.h" - -#include <cstdlib> -#include <Arduino.h> -//#include "cmsis.h" // this was just for enable / disable IRQ I think... - -/* - * constructors - */ - -template<class kind> HeapRing<kind>::HeapRing() -{ - head_i = tail_i = length = 0; - isr_tail_i = tail_i; - ring = NULL; -} - -template<class kind> HeapRing<kind>::HeapRing(unsigned int length) -{ - head_i = tail_i = 0; - isr_tail_i = tail_i; - ring = new kind[length]; - // TODO: handle allocation failure - this->length = length; -} - -/* - * destructor - */ - -template<class kind> HeapRing<kind>::~HeapRing() -{ - head_i = tail_i = length = 0; - isr_tail_i = tail_i; - if (ring) - delete [] ring; - ring = NULL; -} - -/* - * index accessors (protected) - */ - -template<class kind> unsigned int HeapRing<kind>::next(unsigned int item) const -{ - if (length == 0) - return 0; - - if (++item >= length) - return 0; - - return item; -} - -template<class kind> unsigned int HeapRing<kind>::prev(unsigned int item) const -{ - if (length == 0) - return 0; - - if (item == 0) - return (length - 1); - else - return (item - 1); -} - -/* - * reference accessors - */ - -template<class kind> kind& HeapRing<kind>::head() -{ - return ring[head_i]; -} - -template<class kind> kind& HeapRing<kind>::tail() -{ - return ring[tail_i]; -} - -template<class kind> kind& HeapRing<kind>::item(unsigned int i) -{ - return ring[i]; -} - -template<class kind> void HeapRing<kind>::push_front(kind& item) -{ - ring[head_i] = item; - head_i = next(head_i); -} - -template<class kind> kind& HeapRing<kind>::pop_back() -{ - kind& r = ring[tail_i]; - tail_i = next(tail_i); - return r; -} - -/* - * pointer accessors - */ - -template<class kind> kind* HeapRing<kind>::head_ref() -{ - return &ring[head_i]; -} - -template<class kind> kind* HeapRing<kind>::tail_ref() -{ - return &ring[tail_i]; -} - -template<class kind> kind* HeapRing<kind>::item_ref(unsigned int i) -{ - return &ring[i]; -} - -template<class kind> void HeapRing<kind>::produce_head() -{ - while (is_full()); - head_i = next(head_i); -} - -template<class kind> void HeapRing<kind>::consume_tail() -{ - if (!is_empty()) - tail_i = next(tail_i); -} - -/* - * queue status accessors - */ - -template<class kind> bool HeapRing<kind>::is_full() const -{ - // these IRQ disable / enable calls were commented out by smoothie devs - //__disable_irq(); - bool r = (next(head_i) == tail_i); - //__enable_irq(); - - return r; -} - -template<class kind> bool HeapRing<kind>::is_empty() const -{ - //__disable_irq(); - bool r = (head_i == tail_i); - //__enable_irq(); - - return r; -} - -template <class kind> unsigned int HeapRing<kind>::space(void){ - // I *think* this is sound - if(next(head_i) == tail_i){ - return 0; - } else if (head_i == tail_i){ - return length; - } else if (head_i > tail_i){ - return (length - head_i + tail_i); - } else if (tail_i > head_i){ - return (length - tail_i + head_i); - } else { - return 0; // ?? shouldn't - } -} - -/* - * resize - */ - -template<class kind> bool HeapRing<kind>::resize(unsigned int length) -{ - if (is_empty()) - { - if (length == 0) - { - // pretty sure these work w/ D51 same as w/ STM - __disable_irq(); - - if (is_empty()) // check again in case something was pushed - { - head_i = tail_i = this->length = 0; - - __enable_irq(); - - if (ring) - delete [] ring; - ring = NULL; - - return true; - } - - __enable_irq(); - - return false; - } - - // Note: we don't use realloc so we can fall back to the existing ring if allocation fails - kind* newring = new kind[length]; - - if (newring != NULL) - { - kind* oldring = ring; - - __disable_irq(); - - if (is_empty()) // check again in case something was pushed while malloc did its thing - { - ring = newring; - this->length = length; - head_i = tail_i = 0; - - __enable_irq(); - - if (oldring) - delete [] oldring; - - return true; - } - - __enable_irq(); - - delete [] newring; - } - } - - return false; -} - -template<class kind> bool HeapRing<kind>::provide(kind* buffer, unsigned int length) -{ - __disable_irq(); - - if (is_empty()) - { - kind* oldring = ring; - - if ((buffer != NULL) && (length > 0)) - { - ring = buffer; - this->length = length; - head_i = tail_i = 0; - - __enable_irq(); - - if (oldring) - delete [] oldring; - return true; - } - } - - __enable_irq(); - - return false; -} diff --git a/firmware/osape-smoothieroll-head/src/smoothie/libs/HeapRing.h b/firmware/osape-smoothieroll-head/src/smoothie/libs/HeapRing.h deleted file mode 100644 index cd20af6..0000000 --- a/firmware/osape-smoothieroll-head/src/smoothie/libs/HeapRing.h +++ /dev/null @@ -1,86 +0,0 @@ -#ifndef _HEAPRING_H -#define _HEAPRING_H - -#include "../../utils/syserror.h" - -template<class kind> class HeapRing { - - // smoothie-specific friend classes - friend class Planner; - friend class Conveyor; - friend class Block; - -public: - HeapRing(); - HeapRing(unsigned int length); - - ~HeapRing(); - - /* - * direct accessors - */ - kind& head(); - kind& tail(); - - void push_front(kind&) __attribute__ ((warning("Not thread-safe if pop_back() is used in ISR context!"))); // instead, prepare(head_ref()); produce_head(); - kind& pop_back(void) __attribute__ ((warning("Not thread-safe if head_ref() is used to prepare new items, or push_front() is used in ISR context!"))); // instead, consume(tail_ref()); consume_tail(); - - /* - * pointer accessors - */ - kind* head_ref(); - kind* tail_ref(); - - void produce_head(void); - void consume_tail(void); - - /* - * queue status - */ - bool is_empty(void) const; - bool is_full(void) const; - unsigned int space(void); - - /* - * resize - * - * returns true on success, or false if queue is not empty or not enough memory available - */ - bool resize(unsigned int); - - /* - * provide - * kind* - new buffer pointer - * int length - number of items in buffer (NOT size in bytes!) - * - * cause HeapRing to use a specific memory location instead of allocating its own - * - * returns true on success, or false if queue is not empty - */ - bool provide(kind*, unsigned int length); - -protected: - /* - * these functions are protected as they should only be used internally - * or in extremely specific circumstances - */ - kind& item(unsigned int); - kind* item_ref(unsigned int); - - unsigned int next(unsigned int) const; - unsigned int prev(unsigned int) const; - - /* - * buffer variables - */ - unsigned int length; - - volatile unsigned int head_i; - volatile unsigned int tail_i; - volatile unsigned int isr_tail_i; - -private: - kind* ring; -}; - -#endif /* _HEAPRING_H */ diff --git a/firmware/osape-smoothieroll-head/src/smoothie/libs/StepTicker.cpp b/firmware/osape-smoothieroll-head/src/smoothie/libs/StepTicker.cpp deleted file mode 100644 index 463bfc0..0000000 --- a/firmware/osape-smoothieroll-head/src/smoothie/libs/StepTicker.cpp +++ /dev/null @@ -1,222 +0,0 @@ -/* - This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). - Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - Smoothie 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 General Public License for more details. - You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>. -*/ - - -#include "StepTicker.h" -#include "../modules/robot/Block.h" -//#include "../modules/robot/Conveyor.h" -/* -#include "libs/nuts_bolts.h" -#include "libs/Module.h" -#include "libs/Kernel.h" -#include "StepperMotor.h" -#include "StreamOutputPool.h" -#include "Block.h" -#include "Conveyor.h" - -#include "system_LPC17xx.h" // mbed.h lib -#include <math.h> -#include <mri.h> -*/ - -StepTicker* StepTicker::instance = 0; - -StepTicker* StepTicker::getInstance(void){ - if(instance == 0){ - instance = new StepTicker(); - } - return instance; -} - -StepTicker* stepTicker = StepTicker::getInstance(); - -StepTicker::StepTicker(){ -} - -void StepTicker::init(void){ - // Default start values - this->set_frequency(100000); - - this->num_motors = 3; - - for(uint8_t m = 0; m < 3; m ++){ - motor[m] = smoothieRoll->actuators[m]; - } - - this->running = false; - this->current_block = nullptr; -} - -//called when everything is setup and interrupts can start -void StepTicker::start(){ - //NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler - //NVIC_EnableIRQ(TIMER1_IRQn); // Enable interrupt handler - current_tick= 0; -} - -// Set the base stepping frequency -#warning This leaves config as is, see startup to 'make proper'. -void StepTicker::set_frequency( float frequency ){ - this->frequency = frequency; - this->period = 80; // set manually above, floorf((SystemCoreClock / 4.0F) / frequency); // SystemCoreClock/4 = Timer increments in a second -} - -// step clock -void StepTicker::step_tick (void){ - if(running){ - //DEBUG3PIN_ON; - } else { - //DEBUG3PIN_OFF; - } - // if nothing has been setup we ignore the ticks - if(!running){ - // check if anything new available - if(conveyor->get_next_block(¤t_block)) { // returns false if no new block is available - running = start_next_block(); // returns true if there is at least one motor with steps to issue - if(!running) return; - } else { - return; - } - } - - /* - eliminated this, but here's a halting case - if(THEKERNEL->is_halted()) { - running= false; - current_tick = 0; - current_block= nullptr; - return; - } - */ - - bool still_moving= false; - // foreach motor, if it is active see if time to issue a step to that motor - for (uint8_t m = 0; m < num_motors; m++) { - if(current_block->tick_info[m].steps_to_move == 0) continue; // not active - - current_block->tick_info[m].steps_per_tick += current_block->tick_info[m].acceleration_change; - - if(current_tick == current_block->tick_info[m].next_accel_event) { - if(current_tick == current_block->accelerate_until) { // We are done accelerating, deceleration becomes 0 : plateau - current_block->tick_info[m].acceleration_change = 0; - if(current_block->decelerate_after < current_block->total_move_ticks) { - current_block->tick_info[m].next_accel_event = current_block->decelerate_after; - if(current_tick != current_block->decelerate_after) { // We are plateauing - // steps/sec / tick frequency to get steps per tick - current_block->tick_info[m].steps_per_tick = current_block->tick_info[m].plateau_rate; - } - } - } - - if(current_tick == current_block->decelerate_after) { // We start decelerating - current_block->tick_info[m].acceleration_change = current_block->tick_info[m].deceleration_change; - } - } - - // protect against rounding errors and such - if(current_block->tick_info[m].steps_per_tick <= 0) { - current_block->tick_info[m].counter = STEPTICKER_FPSCALE; // we force completion this step by setting to 1.0 - current_block->tick_info[m].steps_per_tick = 0; - } - - current_block->tick_info[m].counter += current_block->tick_info[m].steps_per_tick; - - if(current_block->tick_info[m].counter >= STEPTICKER_FPSCALE) { // >= 1.0 step time - current_block->tick_info[m].counter -= STEPTICKER_FPSCALE; // -= 1.0F; - ++current_block->tick_info[m].step_count; - - // step the motor - bool ismoving = motor[m]->step(); // returns false if the moving flag was set to false externally (probes, endstops etc) - if(m == 0 && ismoving){ - DEBUG2PIN_TOGGLE; - //stepper_hw->step(); - } else if (m == 1 && ismoving){ - DEBUG4PIN_TOGGLE; - } - - if(!ismoving || current_block->tick_info[m].step_count == current_block->tick_info[m].steps_to_move) { - // done - current_block->tick_info[m].steps_to_move = 0; - motor[m]->stop_moving(); // let motor know it is no longer moving - } - } - - // see if any motors are still moving after this tick - if(motor[m]->is_moving()) still_moving = true; - } - - // do this after so we start at tick 0 - current_tick++; // count number of ticks - - // see if any motors are still moving - if(!still_moving) { - // block transition - //DEBUG3PIN_TOGGLE; - - // all moves finished - current_tick = 0; - - // get next block - // do it here so there is no delay in ticks - conveyor->block_finished(); - - if(conveyor->get_next_block(¤t_block)) { // returns false if no new block is available - running = start_next_block(); // returns true if there is at least one motor with steps to issue - } else { - current_block = nullptr; - running = false; - } - // all moves finished - // queue needs to be incremented, that happens on the conveyor's idle cycle - } -} // end step_tick - -// only called from the step tick ISR (single consumer) -bool StepTicker::start_next_block() -{ - if(current_block == nullptr){ - return false; - } - bool ok = false; - // need to prepare each active motor - for (uint8_t m = 0; m < num_motors; m++) { - if(m == 0){ - if(current_block->direction_bits[m]){ - DEBUG1PIN_ON; - } else { - DEBUG1PIN_OFF; - } - //stepper_hw->dir(current_block->direction_bits[m]); - } else if (m == 1){ - if(current_block->direction_bits[m]){ - DEBUG3PIN_ON; - } else { - DEBUG3PIN_OFF; - } - } - if(current_block->tick_info[m].steps_to_move == 0) continue; - ok = true; // mark at least one motor is moving - // set direction bit here - // NOTE this would be at least 10us before first step pulse. - // TODO does this need to be done sooner, if so how without delaying next tick - motor[m]->set_direction(current_block->direction_bits[m]); - motor[m]->start_moving(); // also let motor know it is moving now - } - - current_tick= 0; - - if(ok) { - //SET_STEPTICKER_DEBUG_PIN(1); - return true; - } else { - // this is an edge condition that should never happen, but we need to discard this block if it ever does - // basically it is a block that has zero steps for all motors - conveyor->block_finished(); - } - - return false; -} \ No newline at end of file diff --git a/firmware/osape-smoothieroll-head/src/smoothie/libs/StepTicker.h b/firmware/osape-smoothieroll-head/src/smoothie/libs/StepTicker.h deleted file mode 100644 index fedb812..0000000 --- a/firmware/osape-smoothieroll-head/src/smoothie/libs/StepTicker.h +++ /dev/null @@ -1,76 +0,0 @@ -/* - This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). - Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - Smoothie 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 General Public License for more details. - You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>. -*/ - - - -#pragma once - -// smoothie includes -#include <stdint.h> -#include <array> -#include <bitset> -#include <functional> -#include <atomic> -#include "../modules/robot/ActuatorCoordinates.h" -#include "../modules/robot/Conveyor.h" -// step state interface hack -#include "../modules/robot/StepInterface.h" -/* -#include "TSRingBuffer.h" -*/ -class Block; - -// osap includes -#include <Arduino.h> -#include "../../drivers/indicators.h" -#include "../../utils/clocks_d51_module.h" -#include "../SmoothieRoll.h" - -// handle 2.30 Fixed point -#define STEPTICKER_FPSCALE (1<<30) -#define STEPTICKER_TOFP(x) ((int32_t)roundf((float)(x)*STEPTICKER_FPSCALE)) -#define STEPTICKER_FROMFP(x) ((float)(x)/STEPTICKER_FPSCALE) - -class StepTicker{ - private: - static StepTicker* instance; - bool start_next_block(); - - float frequency; - uint32_t period; - // not using motor refs, direct stepping std::array<StepperMotor*, k_max_actuators> motor; - // do adhoc hack - StepInterface* motor[3]; - - Block *current_block; - uint32_t current_tick{0}; - - struct { - volatile bool running:1; - uint8_t num_motors:4; - }; - public: - StepTicker(); - static StepTicker* getInstance(); - void init(void); - // go - void set_frequency( float frequency ); - // deleted this void set_unstep_time( float microseconds ); - // deleted this int register_motor(StepperMotor* motor); - float get_frequency() const { return frequency; } - void unstep_tick(); - const Block *get_current_block() const { return current_block; } - - void step_tick (void); - void handle_finish (void); - void start(); - - // whatever setup the block should register this to know when it is done - std::function<void()> finished_fnc{nullptr}; -}; - -extern StepTicker* stepTicker; diff --git a/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/ActuatorCoordinates.h b/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/ActuatorCoordinates.h deleted file mode 100644 index 135dd04..0000000 --- a/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/ActuatorCoordinates.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). - Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - Smoothie 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 General Public License for more details. - You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>. -*/ - -#pragma once - -#include <array> - -#define MAX_ROBOT_ACTUATORS 3 - -// Keep MAX_ROBOT_ACTUATORS as small as practical it impacts block size and therefore free memory. -const size_t k_max_actuators = MAX_ROBOT_ACTUATORS; -typedef struct std::array<float, k_max_actuators> ActuatorCoordinates; diff --git a/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Block.cpp b/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Block.cpp deleted file mode 100644 index 460e584..0000000 --- a/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Block.cpp +++ /dev/null @@ -1,350 +0,0 @@ -/* - This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). - Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - Smoothie 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 General Public License for more details. - You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>. -*/ - -#include "Block.h" -#include "../../libs/StepTicker.h" -#include <math.h> - -/* -#include "libs/Module.h" -#include "libs/Kernel.h" -#include "libs/nuts_bolts.h" -#include <math.h> -#include <string> -#include "Block.h" -#include "Planner.h" -#include "Conveyor.h" -#include "Gcode.h" -#include "libs/StreamOutputPool.h" -#include "StepTicker.h" - -#include "mri.h" -*/ - -using std::string; -#include <vector> - -#define STEP_TICKER_FREQUENCY stepTicker->get_frequency() -//THEKERNEL->step_ticker->get_frequency() -#define STEP_TICKER_FREQUENCY_2 (STEP_TICKER_FREQUENCY*STEP_TICKER_FREQUENCY) - -uint8_t Block::n_actuators = 0; - -// A block represents a movement, it's length for each stepper motor, and the corresponding acceleration curves. -// It's stacked on a queue, and that queue is then executed in order, to move the motors. -// Most of the accel math is also done in this class -// And GCode objects for use in on_gcode_execute are also help in here - -Block::Block() -{ - clear(); -} - -void Block::clear() -{ - is_ready = false; - - this->steps.fill(0); - - steps_event_count = 0; - nominal_rate = 0.0F; - nominal_speed = 0.0F; - millimeters = 0.0F; - entry_speed = 0.0F; - exit_speed = 0.0F; - acceleration = 100.0F; // we don't want to get divide by zeroes if this is not set - initial_rate = 0.0F; - accelerate_until = 0; - decelerate_after = 0; - direction_bits = 0; - recalculate_flag = false; - nominal_length_flag = false; - max_entry_speed = 0.0F; - is_ticking = false; - is_g123 = false; - locked = false; - s_value = 0.0F; - - acceleration_per_tick= 0; - deceleration_per_tick= 0; - total_move_ticks= 0; - if(tick_info.size() != n_actuators) { - tick_info.resize(n_actuators); - } - for(auto &i : tick_info) { - i.steps_per_tick= 0; - i.counter= 0; - i.acceleration_change= 0; - i.deceleration_change= 0; - i.plateau_rate= 0; - i.steps_to_move= 0; - i.step_count= 0; - i.next_accel_event= 0; - } -} - -void Block::debug() const -{ - /* - THEKERNEL->streams->printf("%p: steps-X:%lu Y:%lu Z:%lu ", this, this->steps[0], this->steps[1], this->steps[2]); - for (size_t i = E_AXIS; i < n_actuators; ++i) { - THEKERNEL->streams->printf("E%d:%lu ", i-E_AXIS, this->steps[i]); - } - THEKERNEL->streams->printf("(max:%lu) nominal:r%1.4f/s%1.4f mm:%1.4f acc:%1.2f accu:%lu decu:%lu ticks:%lu rates:%1.4f entry/max:%1.4f/%1.4f exit:%1.4f primary:%d ready:%d locked:%d ticking:%d recalc:%d nomlen:%d time:%f\r\n", - this->steps_event_count, - this->nominal_rate, - this->nominal_speed, - this->millimeters, - this->acceleration, - this->accelerate_until, - this->decelerate_after, - this->total_move_ticks, - this->initial_rate, - this->entry_speed, - this->max_entry_speed, - this->exit_speed, - this->primary_axis, - this->is_ready, - this->locked, - this->is_ticking, - recalculate_flag ? 1 : 0, - nominal_length_flag ? 1 : 0, - total_move_ticks/STEP_TICKER_FREQUENCY - ); - */ -} - - -/* Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. -// The factors represent a factor of braking and must be in the range 0.0-1.0. -// +--------+ <- nominal_rate -// / \ -// nominal_rate*entry_factor -> + \ -// | + <- nominal_rate*exit_factor -// +-------------+ -// time --> -*/ -void Block::calculate_trapezoid( float entryspeed, float exitspeed ) -{ - // if block is currently executing, don't touch anything! - if (is_ticking) return; - - float initial_rate = this->nominal_rate * (entryspeed / this->nominal_speed); // steps/sec - float final_rate = this->nominal_rate * (exitspeed / this->nominal_speed); - //printf("Initial rate: %f, final_rate: %f\n", initial_rate, final_rate); - // How many steps ( can be fractions of steps, we need very precise values ) to accelerate and decelerate - // This is a simplification to get rid of rate_delta and get the steps/s² accel directly from the mm/s² accel - float acceleration_per_second = (this->acceleration * this->steps_event_count) / this->millimeters; - - float maximum_possible_rate = sqrtf( ( this->steps_event_count * acceleration_per_second ) + ( ( powf(initial_rate, 2) + powf(final_rate, 2) ) / 2.0F ) ); - - //printf("id %d: acceleration_per_second: %f, maximum_possible_rate: %f steps/sec, %f mm/sec\n", this->id, acceleration_per_second, maximum_possible_rate, maximum_possible_rate/100); - - // Now this is the maximum rate we'll achieve this move, either because - // it's the higher we can achieve, or because it's the higher we are - // allowed to achieve - this->maximum_rate = std::min(maximum_possible_rate, this->nominal_rate); - - // Now figure out how long it takes to accelerate in seconds - float time_to_accelerate = ( this->maximum_rate - initial_rate ) / acceleration_per_second; - - // Now figure out how long it takes to decelerate - float time_to_decelerate = ( final_rate - this->maximum_rate ) / -acceleration_per_second; - - // Now we know how long it takes to accelerate and decelerate, but we must - // also know how long the entire move takes so we can figure out how long - // is the plateau if there is one - float plateau_time = 0; - - // Only if there is actually a plateau ( we are limited by nominal_rate ) - if(maximum_possible_rate > this->nominal_rate) { - // Figure out the acceleration and deceleration distances ( in steps ) - float acceleration_distance = ( ( initial_rate + this->maximum_rate ) / 2.0F ) * time_to_accelerate; - float deceleration_distance = ( ( this->maximum_rate + final_rate ) / 2.0F ) * time_to_decelerate; - - // Figure out the plateau steps - float plateau_distance = this->steps_event_count - acceleration_distance - deceleration_distance; - - // Figure out the plateau time in seconds - plateau_time = plateau_distance / this->maximum_rate; - } - - // Figure out how long the move takes total ( in seconds ) - float total_move_time = time_to_accelerate + time_to_decelerate + plateau_time; - //puts "total move time: #{total_move_time}s time to accelerate: #{time_to_accelerate}, time to decelerate: #{time_to_decelerate}" - - // We now have the full timing for acceleration, plateau and deceleration, - // yay \o/ Now this is very important these are in seconds, and we need to - // round them into ticks. This means instead of accelerating in 100.23 - // ticks we'll accelerate in 100 ticks. Which means to reach the exact - // speed we want to reach, we must figure out a new/slightly different - // acceleration/deceleration to be sure we accelerate and decelerate at - // the exact rate we want - - // First off round total time, acceleration time and deceleration time in ticks - uint32_t acceleration_ticks = floorf( time_to_accelerate * STEP_TICKER_FREQUENCY ); - uint32_t deceleration_ticks = floorf( time_to_decelerate * STEP_TICKER_FREQUENCY ); - uint32_t total_move_ticks = floorf( total_move_time * STEP_TICKER_FREQUENCY ); - - // Now deduce the plateau time for those new values expressed in tick - //uint32_t plateau_ticks = total_move_ticks - acceleration_ticks - deceleration_ticks; - - // Now we figure out the acceleration value to reach EXACTLY maximum_rate(steps/s) in EXACTLY acceleration_ticks(ticks) amount of time in seconds - float acceleration_time = acceleration_ticks / STEP_TICKER_FREQUENCY; // This can be moved into the operation below, separated for clarity, note we need to do this instead of using time_to_accelerate(seconds) directly because time_to_accelerate(seconds) and acceleration_ticks(seconds) do not have the same value anymore due to the rounding - float deceleration_time = deceleration_ticks / STEP_TICKER_FREQUENCY; - - float acceleration_in_steps = (acceleration_time > 0.0F ) ? ( this->maximum_rate - initial_rate ) / acceleration_time : 0; - float deceleration_in_steps = (deceleration_time > 0.0F ) ? ( this->maximum_rate - final_rate ) / deceleration_time : 0; - - // we have a potential race condition here as we could get interrupted anywhere in the middle of this call, we need to lock - // the updates to the blocks to get around it - this->locked = true; - // Now figure out the two acceleration ramp change events in ticks - this->accelerate_until = acceleration_ticks; - this->decelerate_after = total_move_ticks - deceleration_ticks; - - // Now figure out the acceleration PER TICK, this should ideally be held as a float, even a double if possible as it's very critical to the block timing - // steps/tick^2 - - this->acceleration_per_tick = acceleration_in_steps / STEP_TICKER_FREQUENCY_2; - this->deceleration_per_tick = deceleration_in_steps / STEP_TICKER_FREQUENCY_2; - - // We now have everything we need for this block to call a Steppermotor->move method !!!! - // Theorically, if accel is done per tick, the speed curve should be perfect. - this->total_move_ticks = total_move_ticks; - - //puts "accelerate_until: #{this->accelerate_until}, decelerate_after: #{this->decelerate_after}, acceleration_per_tick: #{this->acceleration_per_tick}, total_move_ticks: #{this->total_move_ticks}" - - this->initial_rate = initial_rate; - this->exit_speed = exitspeed; - - // prepare the block for stepticker - this->prepare(); - this->locked = false; -} - -// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the -// acceleration within the allotted distance. -float Block::max_allowable_speed(float acceleration, float target_velocity, float distance) -{ - return sqrtf(target_velocity * target_velocity - 2.0F * acceleration * distance); -} - -// Called by Planner::recalculate() when scanning the plan from last to first entry. -float Block::reverse_pass(float exit_speed) -{ - // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. - // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and - // check for maximum allowable speed reductions to ensure maximum possible planned speed. - if (this->entry_speed != this->max_entry_speed) { - // If nominal length true, max junction speed is guaranteed to be reached. Only compute - // for max allowable speed if block is decelerating and nominal length is false. - if ((!this->nominal_length_flag) && (this->max_entry_speed > exit_speed)) { - float max_entry_speed = max_allowable_speed(-this->acceleration, exit_speed, this->millimeters); - - this->entry_speed = min(max_entry_speed, this->max_entry_speed); - - return this->entry_speed; - } else - this->entry_speed = this->max_entry_speed; - } - - return this->entry_speed; -} - - -// Called by Planner::recalculate() when scanning the plan from first to last entry. -// returns maximum exit speed of this block -float Block::forward_pass(float prev_max_exit_speed) -{ - // If the previous block is an acceleration block, but it is not long enough to complete the - // full speed change within the block, we need to adjust the entry speed accordingly. Entry - // speeds have already been reset, maximized, and reverse planned by reverse planner. - // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck. - - // TODO: find out if both of these checks are necessary - if (prev_max_exit_speed > nominal_speed) - prev_max_exit_speed = nominal_speed; - if (prev_max_exit_speed > max_entry_speed) - prev_max_exit_speed = max_entry_speed; - - if (prev_max_exit_speed <= entry_speed) { - // accel limited - entry_speed = prev_max_exit_speed; - // since we're now acceleration or cruise limited - // we don't need to recalculate our entry speed anymore - recalculate_flag = false; - } - // else - // // decel limited, do nothing - - return max_exit_speed(); -} - -float Block::max_exit_speed() -{ - // if block is currently executing, return cached exit speed from calculate_trapezoid - // this ensures that a block following a currently executing block will have correct entry speed - if(is_ticking) - return this->exit_speed; - - // if nominal_length_flag is asserted - // we are guaranteed to reach nominal speed regardless of entry speed - // thus, max exit will always be nominal - if (nominal_length_flag) - return nominal_speed; - - // otherwise, we have to work out max exit speed based on entry and acceleration - float max = max_allowable_speed(-this->acceleration, this->entry_speed, this->millimeters); - - return min(max, nominal_speed); -} - -// prepare block for the step ticker, called everytime the block changes -// this is done during planning so does not delay tick generation and step ticker can simply grab the next block during the interrupt -void Block::prepare() -{ - float inv = 1.0F / this->steps_event_count; - for (uint8_t m = 0; m < n_actuators; m++) { - uint32_t steps = this->steps[m]; - this->tick_info[m].steps_to_move = steps; - if(steps == 0) continue; - - float aratio = inv * steps; - this->tick_info[m].steps_per_tick = STEPTICKER_TOFP((this->initial_rate * aratio) / STEP_TICKER_FREQUENCY); // steps/sec / tick frequency to get steps per tick in 2.30 fixed point - this->tick_info[m].counter = 0; // 2.30 fixed point - this->tick_info[m].step_count = 0; - this->tick_info[m].next_accel_event = this->total_move_ticks + 1; - - float acceleration_change = 0; - if(this->accelerate_until != 0) { // If the next accel event is the end of accel - this->tick_info[m].next_accel_event = this->accelerate_until; - acceleration_change = this->acceleration_per_tick; - - } else if(this->decelerate_after == 0 /*&& this->accelerate_until == 0*/) { - // we start off decelerating - acceleration_change = -this->deceleration_per_tick; - - } else if(this->decelerate_after != this->total_move_ticks /*&& this->accelerate_until == 0*/) { - // If the next event is the start of decel ( don't set this if the next accel event is accel end ) - this->tick_info[m].next_accel_event = this->decelerate_after; - } - - // convert to fixed point after scaling - this->tick_info[m].acceleration_change= STEPTICKER_TOFP(acceleration_change * aratio); - this->tick_info[m].deceleration_change= -STEPTICKER_TOFP(this->deceleration_per_tick * aratio); - this->tick_info[m].plateau_rate= STEPTICKER_TOFP((this->maximum_rate * aratio) / STEP_TICKER_FREQUENCY); - } -} - -// returns current rate (steps/sec) for the given actuator -float Block::get_trapezoid_rate(int i) const -{ - // convert steps per tick from fixed point to float and convert to steps/sec - // FIXME steps_per_tick can change at any time, potential race condition if it changes while being read here - return STEPTICKER_FROMFP(tick_info[i].steps_per_tick) * STEP_TICKER_FREQUENCY; -} diff --git a/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Block.h b/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Block.h deleted file mode 100644 index 76884db..0000000 --- a/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Block.h +++ /dev/null @@ -1,79 +0,0 @@ -/* - This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). - Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - Smoothie 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 General Public License for more details. - You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>. -*/ - -#pragma once - -#include <vector> -#include <bitset> -#include "ActuatorCoordinates.h" - -class Block { - public: - Block(); - void calculate_trapezoid( float entry_speed, float exit_speed ); - float max_allowable_speed( float acceleration, float target_velocity, float distance); - - float reverse_pass(float exit_speed); - float forward_pass(float next_entry_speed); - float max_exit_speed(); - void debug() const; - void ready() { is_ready= true; } - void clear(); - void prepare(); - - float get_trapezoid_rate(int i) const; - - std::array<uint32_t, k_max_actuators> steps; // Number of steps for each axis for this block - uint32_t steps_event_count; // Steps for the longest axis - float nominal_rate; // Nominal rate in steps per second - float nominal_speed; // Nominal speed in mm per second - float millimeters; // Distance for this move - float entry_speed; - float exit_speed; - float acceleration; // the acceleration for this block - float initial_rate; // Initial rate in steps per second - float maximum_rate; - - float acceleration_per_tick {0}; - float deceleration_per_tick {0}; - - float max_entry_speed; - - // this is tick info needed for this block. applies to all motors - uint32_t accelerate_until; - uint32_t decelerate_after; - uint32_t total_move_ticks; - std::bitset<k_max_actuators> direction_bits; // Direction for each axis in bit form, relative to the direction port's mask - - // this is the data needed to determine when each motor needs to be issued a step - using tickinfo_t= struct { - int32_t steps_per_tick; // 2.30 fixed point - int32_t counter; // 2.30 fixed point - int32_t acceleration_change; // 2.30 fixed point signed - int32_t deceleration_change; // 2.30 fixed point - int32_t plateau_rate; // 2.30 fixed point - uint32_t steps_to_move; - uint32_t step_count; - uint32_t next_accel_event; - }; - - // need info for each active motor - //std::array<tickinfo_t, k_max_actuators> tick_info; - std::vector<tickinfo_t> tick_info; - static uint8_t n_actuators; - - struct { - bool recalculate_flag:1; // Planner flag to recalculate trapezoids on entry junction - bool nominal_length_flag:1; // Planner flag for nominal speed always reached - bool is_ready:1; - bool primary_axis:1; // set if this move is a primary axis - bool is_g123:1; // set if this is a G1, G2 or G3 - volatile bool is_ticking:1; // set when this block is being actively ticked by the stepticker - volatile bool locked:1; // set to true when the critical data is being updated, stepticker will have to skip if this is set - uint16_t s_value:12; // for laser 1.11 Fixed point - }; -}; diff --git a/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Conveyor.cpp b/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Conveyor.cpp deleted file mode 100644 index c0e1876..0000000 --- a/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Conveyor.cpp +++ /dev/null @@ -1,318 +0,0 @@ -/* - This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl) with additions from Sungeun K. Jeon (https://github.com/chamnit/grbl) - Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - Smoothie 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 General Public License for more details. - You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>. -*/ - -#include <functional> -#include <vector> -#include <Arduino.h> -#include "Conveyor.h" -#include "Block.h" -//#include "Planner.h" - -/* -#include "libs/nuts_bolts.h" -#include "libs/RingBuffer.h" -#include "../communication/utils/Gcode.h" -#include "libs/Module.h" -#include "libs/Kernel.h" -#include "Timer.h" // mbed.h lib -#include "wait_api.h" // mbed.h lib -#include "Block.h" -#include "Conveyor.h" -#include "Planner.h" -#include "mri.h" -#include "checksumm.h" -#include "Config.h" -#include "libs/StreamOutputPool.h" -#include "ConfigValue.h" -#include "StepTicker.h" -#include "Robot.h" -#include "StepperMotor.h" - -#include <functional> -#include <vector> - -#include "mbed.h" -*/ - -/* -rmd' these: have to do with smoothie's config-grabbing system -#define planner_queue_size_checksum CHECKSUM("planner_queue_size") -#define queue_delay_time_ms_checksum CHECKSUM("queue_delay_time_ms") -*/ - -/* - * The conveyor holds the queue of blocks, takes care of creating them, and starting the executing chain of blocks - * - * The Queue is implemented as a ringbuffer- with a twist - * - * Since delete() is not thread-safe, we must marshall deletable items out of ISR context - * - * To do this, we have implmented a *double* ringbuffer- two ringbuffers sharing the same ring, and one index pointer - * - * as in regular ringbuffers, HEAD always points to a clean, free block. We are free to prepare it as we see fit, at our leisure. - * When the block is fully prepared, we increment the head pointer, and from that point we must not touch it anymore. - * - * also, as in regular ringbuffers, we can 'use' the TAIL block, and increment tail pointer when we're finished with it - * - * Both of these are implemented here- see queue_head_block() (where head is pushed) and on_idle() (where tail is consumed) - * - * The double ring is implemented by adding a third index pointer that lives in between head and tail. We call it isr_tail_i. - * - * in ISR context, we use HEAD as the head pointer, and isr_tail_i as the tail pointer. - * As HEAD increments, ISR context can consume the new blocks which appear, and when we're finished with a block, we increment isr_tail_i to signal that they're finished, and ready to be cleaned - * - * in IDLE context, we use isr_tail_i as the head pointer, and TAIL as the tail pointer. - * When isr_tail_i != tail, we clean up the tail block (performing ISR-unsafe delete operations) and consume it (increment tail pointer), returning it to the pool of clean, unused blocks which HEAD is allowed to prepare for queueing - * - * Thus, our two ringbuffers exist sharing the one ring of blocks, and we safely marshall used blocks from ISR context to IDLE context for safe cleanup. - */ - -Conveyor* Conveyor::instance = 0; - -Conveyor* Conveyor::getInstance(void){ - if(instance == 0){ - instance = new Conveyor(); - } - return instance; -} - -Conveyor* conveyor = Conveyor::getInstance(); - -Conveyor::Conveyor(){ - running = false; - halted = false; - allow_fetch = false; - flush = false; -} - -// we allocate the queue here after config is completed so we do not run out of memory during config -void Conveyor::start(uint8_t n) -{ - Block::n_actuators= n; // set the number of motors which determines how big the tick info vector is - queue.resize(queue_size); - running = true; -} - -void Conveyor::on_module_loaded(){ - //register_for_event(ON_IDLE); - //register_for_event(ON_HALT); - // Attach to the end_of_move stepper event: this next line *is* commented out in smoothieware main - //THEKERNEL->step_ticker->finished_fnc = std::bind( &Conveyor::all_moves_finished, this); - queue_size = 64; //THEKERNEL->config->value(planner_queue_size_checksum)->by_default(32)->as_number(); - queue_delay_time_ms = 1000; //THEKERNEL->config->value(queue_delay_time_ms_checksum)->by_default(100)->as_number(); -} - -void Conveyor::on_idle(void*) -{ - if (running) { - check_queue(); - } - - // we can garbage collect the block queue here - if (queue.tail_i != queue.isr_tail_i) { - if (queue.is_empty()) { - // rm'd this smoothie debug line - //__debugbreak(); - } else { - // Cleanly delete block - Block* block = queue.tail_ref(); - //sysError("block: steps-X: " + String(block->steps[0])); - //block->debug(); - block->clear(); - queue.consume_tail(); - } - } -} - -void Conveyor::on_halt(void* argument) -{ - if(argument == nullptr) { - halted = true; - flush_queue(); - } else { - halted = false; - } -} - -unsigned int Conveyor::queue_space(void){ - // need this fn in order to build flow control - return queue.space(); - return 0; -} - -// see if we are idle -// this checks the block queue is empty, and that the step queue is empty and -// checks that all motors are no longer moving -bool Conveyor::is_idle() const -{ - if(queue.is_empty()) { - // rm'd this, but current integration doesn't call this ever... - // I think is used to allow moves to finish when a halt is called - /* - for(auto &a : THEROBOT->actuators) { - if(a->is_moving()) return false; - } - */ - return true; - } - - return false; -} - -// Wait for the queue to be empty and for all the jobs to finish in step ticker -void Conveyor::wait_for_idle(bool wait_for_motors) -{ - // wait for the job queue to empty, this means cycling everything on the block queue into the job queue - // forcing them to be jobs - #warning modified this, unsure of events: deleted call_event and motor wait - running = false; // stops on_idle calling check_queue - while (!queue.is_empty()) { - check_queue(true); // forces queue to be made available to stepticker - on_idle(nullptr); // replaced call below with this line's direct call - //THEKERNEL->call_event(ON_IDLE, this); - } - - /* - if(wait_for_motors) { - // now we wait for all motors to stop moving - while(!is_idle()) { - THEKERNEL->call_event(ON_IDLE, this); - } - } - */ - - running = true; - // returning now means that everything has totally finished -} - -/* - * push the pre-prepared head block onto the queue - */ -void Conveyor::queue_head_block() -{ - // upstream caller will block on this until there is room in the queue - while (queue.is_full() && !halted) { - //check_queue(); // smoothieware's comment-out - //THEKERNEL->call_event(ON_IDLE, this); // will call check_queue(); - #warning replaced above with direct call to this, - on_idle(nullptr); - - } - - /* - jake removed this call, not using halt function - if(halted) { - // we do not want to stick more stuff on the queue if we are in halt state - // clear and release the block on the head - queue.head_ref()->clear(); - return; // if we got a halt then we are done here - } - */ - - queue.produce_head(); - - // jake removed this call as well, - // not sure if this is the correct place but we need to turn on the motors if they were not already on - // THEKERNEL->call_event(ON_ENABLE, (void*)1); // turn all enable pins on -} - -void Conveyor::setWaitTime(uint32_t ms){ - if(ms > 5000) ms = 5000; - queue_delay_time_ms = ms; -} - -void Conveyor::check_queue(bool force) -{ - static uint32_t last_time_check = micros(); - - if(queue.is_empty()) { - allow_fetch = false; - last_time_check = micros(); // reset timeout - return; - } - - // if we have been waiting for more than the required waiting time and the queue is not empty, - // or the queue is full, then allow stepticker to get the tail - // we do this to allow an idle system to pre load the queue a bit so the first few blocks run smoothly. - if(force || queue.is_full() || (micros() - last_time_check) >= (queue_delay_time_ms * 1000)) { - last_time_check = micros(); // reset timeout - if(!flush) allow_fetch = true; - return; - } -} - -// called from step ticker ISR -bool Conveyor::get_next_block(Block **block){ - // mark entire queue for GC if flush flag is asserted - if (flush){ - while (queue.isr_tail_i != queue.head_i) { - queue.isr_tail_i = queue.next(queue.isr_tail_i); - } - } - - // default the feerate to zero if there is no block available - this->current_feedrate= 0; - - if(halted || queue.isr_tail_i == queue.head_i) return false; // we do not have anything to give - - // wait for queue to fill up, optimizes planning - if(!allow_fetch) return false; - - Block *b = queue.item_ref(queue.isr_tail_i); - // we cannot use this now if it is being updated - if(!b->locked) { - // removed the smoothie debugbreak below - if(!b->is_ready); // __debugbreak(); // should never happen - b->is_ticking = true; - b->recalculate_flag = false; - this->current_feedrate = b->nominal_speed; - *block = b; - return true; - } - - return false; -} - -// called from step ticker ISR when block is finished, do not do anything slow here -void Conveyor::block_finished(){ - // we increment the isr_tail_i so we can get the next block - queue.isr_tail_i= queue.next(queue.isr_tail_i); -} - -/* - In most cases this will not totally flush the queue, as when streaming - gcode there is one stalled waiting for space in the queue, in - queue_head_block() so after this flush, once main_loop runs again one more - gcode gets stuck in the queue, this is bad. Current work around is to call - this when the queue in not full and streaming has stopped -*/ -void Conveyor::flush_queue(){ - allow_fetch = false; - flush= true; - - // TODO force deceleration of last block - - // now wait until the block queue has been flushed - wait_for_idle(false); - - flush= false; -} - -// Debug function -void Conveyor::dump_queue(){ - for (unsigned int index = queue.tail_i, i = 0; true; index = queue.next(index), i++ ) { - // THEKERNEL->streams->printf("block %03d > ", i); - // queue.item_ref(index)->debug(); - - if (index == queue.head_i) - break; - } -} - -// feels hacky, but apparently the way to do it -#include "../../libs/HeapRing.cpp" -template class HeapRing<Block>; diff --git a/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Conveyor.h b/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Conveyor.h deleted file mode 100644 index 096e3ab..0000000 --- a/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Conveyor.h +++ /dev/null @@ -1,73 +0,0 @@ -/* - This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). - Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - Smoothie 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 General Public License for more details. - You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>. -*/ - -#pragma once - -//#include "libs/Module.h" -#include "../../libs/HeapRing.h" -#include "../../../drivers/indicators.h" - -using namespace std; -#include <string> -#include <vector> - -class Gcode; -class Block; - -class Conveyor //: public Module -{ -public: - Conveyor(); - static Conveyor* getInstance(void); - void start(uint8_t n_actuators); - - void on_module_loaded(void); - void on_idle(void *); - void on_halt(void *); - - void setWaitTime(uint32_t ms); - void wait_for_idle(bool wait_for_motors=true); - bool is_queue_empty() { return queue.is_empty(); }; - bool is_queue_full() { return queue.is_full(); }; - unsigned int queue_space(void); - bool is_idle() const; - - // returns next available block writes it to block and returns true - bool get_next_block(Block **block); - void block_finished(); - - void dump_queue(void); - void flush_queue(void); - float get_current_feedrate() const { return current_feedrate; } - - friend class Planner; // for queue: friends can access private vars - - using Queue_t = HeapRing<Block>; - Queue_t queue; // Queue of Blocks - -private: - static Conveyor* instance; - // void all_moves_finished(); // commented out by smoothie devs - void check_queue(bool force= false); - void queue_head_block(void); - - //volatile unsigned int gc_pending; - - uint32_t queue_delay_time_ms; - size_t queue_size; - float current_feedrate{0}; // actual nominal feedrate that current block is running at in mm/sec - - struct { - volatile bool running:1; - volatile bool halted:1; - volatile bool allow_fetch:1; - bool flush:1; - }; - -}; - -extern Conveyor* conveyor; \ No newline at end of file diff --git a/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Planner.cpp b/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Planner.cpp deleted file mode 100644 index 4c41bbd..0000000 --- a/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Planner.cpp +++ /dev/null @@ -1,351 +0,0 @@ -/* - This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl) with additions from Sungeun K. Jeon (https://github.com/chamnit/grbl) - Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - Smoothie 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 General Public License for more details. - You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>. -*/ - -using namespace std; -#include <vector> - -#include "Planner.h" - -/* -#include "mri.h" -#include "nuts_bolts.h" -#include "RingBuffer.h" -#include "Gcode.h" -#include "Module.h" -#include "Kernel.h" -#include "Block.h" -#include "Planner.h" -#include "Conveyor.h" -#include "StepperMotor.h" -#include "Config.h" -#include "checksumm.h" -#include "Robot.h" -#include "ConfigValue.h" -*/ - -#include <math.h> -#include <algorithm> - -// The Planner does the acceleration math for the queue of Blocks ( movements ). -// It makes sure the speed stays within the configured constraints ( acceleration, junction_deviation, etc ) -// It goes over the list in both direction, every time a block is added, re-doing the math to make sure everything is optimal - -Planner* Planner::instance = 0; - -Planner* Planner::getInstance(void){ - if(instance == 0){ - instance = new Planner(); - } - return instance; -} - -Planner* planner = Planner::getInstance(); - -Planner::Planner() -{ - memset(this->previous_unit_vec, 0, sizeof this->previous_unit_vec); - config_load(); -} - -// Configure acceleration -void Planner::config_load() -{ - #warning replaced with defaults - this->junction_deviation = 0.05F; //THEKERNEL->config->value(junction_deviation_checksum)->by_default(0.05F)->as_number(); - this->z_junction_deviation = 0.05F; //THEKERNEL->config->value(z_junction_deviation_checksum)->by_default(NAN)->as_number(); // disabled by default - this->minimum_planner_speed = 0.00F; //THEKERNEL->config->value(minimum_planner_speed_checksum)->by_default(0.0f)->as_number(); -} - -void Planner::set_position(float* pos, uint8_t n_motors){ - for(uint8_t i = 0; i < n_motors; i ++){ - last_position[i] = pos[i]; - smoothieRoll->actuators[i]->set_position(pos[i]); // not totally sure about this - } - do_set_position = true; -} - -// motion packet -> block -void Planner::append_move( float* target, uint8_t n_motors, float rate){ - // do increments on last milestone appended, checking if moves are abs. or inc. - // ... - ActuatorCoordinates feedPos; - ActuatorCoordinates deltas; - for(uint8_t m = 0; m < 3; m ++){ - feedPos[m] = target[m]; // - last_position[m]; - deltas[m] = target[m] - last_position[m]; - last_position[m] = target[m]; - } - // calc sum of squares on increments - float sos = powf(deltas[0], 2) + powf(deltas[1], 2) + powf(deltas[2], 2); - float dist = sqrtf(sos); - float unit[3] = {deltas[0] / dist, deltas[1] / dist, deltas[2] / dist}; - // zero rates are rejected - if(rate < 0.0001F){ - sysError("rejecting small rate"); - return; - } - // very small moves are rejected - if(dist < 0.00001F) return; - // append it - // would also do: set accel for lowest in move (with per motor accel) - // and set speed for lowest max. speed per motor - append_block(feedPos, 3, rate, dist, unit, 100.0F, 1.0F, true); - //feedPos, 3, 100, dist, unit, 100, s_value, true -} - -// Append a block to the queue, compute it's speed factors -bool Planner::append_block( ActuatorCoordinates &actuator_pos, uint8_t n_motors, float rate_mm_s, float distance, float *unit_vec, float acceleration, float s_value, bool g123) -{ - // Create ( recycle ) a new block - Block* block = conveyor->queue.head_ref(); - // Direction bits - bool has_steps = false; - for (size_t i = 0; i < n_motors; i++) { - // hell yeah, ok, the actuators know where they are in step space, i.e. how many ticks they have ticked - // so this asks them to produce a # of steps to the end of this target, given that current state - int32_t steps = smoothieRoll->actuators[i]->steps_to_target(actuator_pos[i]); - if(i == 0 && false){ - sysError("steps-x: " + String(steps) - + " t: " + String(actuator_pos[i], 6) - + " lm: " + String(smoothieRoll->actuators[i]->get_last_milestone_mm())); - } - //THEROBOT->actuators[i]->steps_to_target(actuator_pos[i]); - // Update current position - if(steps != 0) { - // here we update that state - smoothieRoll->actuators[i]->update_last_milestones(actuator_pos[i], steps); - //THEROBOT->actuators[i]->update_last_milestones(actuator_pos[i], steps); - has_steps = true; - } - // find direction - block->direction_bits[i] = (steps < 0) ? 1 : 0; - // save actual steps in block - block->steps[i] = labs(steps); - } - - // sometimes even though there is a detectable movement it turns out there are no steps to be had from such a small move - if(!has_steps) { - block->clear(); - return false; - } - - // info needed by laser - block->s_value = 0; //roundf(s_value*(1<<11)); // 1.11 fixed point - block->is_g123 = g123; - - // use default JD - float junction_deviation = this->junction_deviation; - - // use either regular junction deviation or z specific and see if a primary axis move - block->primary_axis = true; - if(block->steps[0] == 0 && block->steps[1] == 0) { - if(block->steps[2] != 0) { - // z only move - if(!isnan(this->z_junction_deviation)) junction_deviation = this->z_junction_deviation; - } else { - // is not a primary axis move - block->primary_axis = false; - } - } - - block->acceleration = acceleration; // save in block - - // Max number of steps, for all axes - auto mi = std::max_element(block->steps.begin(), block->steps.end()); - block->steps_event_count = *mi; - - block->millimeters = distance; - - // Calculate speed in mm/sec for each axis. No divide by zero due to previous checks. - if( distance > 0.0F ) { - block->nominal_speed = rate_mm_s; // (mm/s) Always > 0 - block->nominal_rate = block->steps_event_count * rate_mm_s / distance; // (step/s) Always > 0 - } else { - block->nominal_speed = 0.0F; - block->nominal_rate = 0; - } - - // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line - // average travel per step event changes. For a line along one axis the travel per step event - // is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both - // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2). - - // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. - // Let a circle be tangent to both previous and current path line segments, where the junction - // deviation is defined as the distance from the junction to the closest edge of the circle, - // colinear with the circle center. The circular segment joining the two paths represents the - // path of centripetal acceleration. Solve for max velocity based on max acceleration about the - // radius of the circle, defined indirectly by junction deviation. This may be also viewed as - // path width or max_jerk in the previous grbl version. This approach does not actually deviate - // from path, but used as a robust way to compute cornering speeds, as it takes into account the - // nonlinearities of both the junction angle and junction velocity. - - // NOTE however it does not take into account independent axis, in most cartesian X and Y and Z are totally independent - // and this allows one to stop with little to no decleration in many cases. - // This is particualrly bad on leadscrew based systems that will skip steps. - float vmax_junction = minimum_planner_speed; // Set default max junction speed - - // if unit_vec was null then it was not a primary axis move so we skip the junction deviation stuff - if (unit_vec != nullptr && !conveyor->is_queue_empty()) { - Block *prev_block = conveyor->queue.item_ref(conveyor->queue.prev(conveyor->queue.head_i)); - float previous_nominal_speed = prev_block->primary_axis ? prev_block->nominal_speed : 0; - - if (junction_deviation > 0.0F && previous_nominal_speed > 0.0F) { - // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) - // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. - float cos_theta = - this->previous_unit_vec[0] * unit_vec[0] - - this->previous_unit_vec[1] * unit_vec[1] - - this->previous_unit_vec[2] * unit_vec[2] ; - - // Skip and use default max junction speed for 0 degree acute junction. - if (cos_theta < 0.95F) { - vmax_junction = std::min(previous_nominal_speed, block->nominal_speed); - // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. - if (cos_theta > -0.95F) { - // Compute maximum junction velocity based on maximum acceleration and junction deviation - float sin_theta_d2 = sqrtf(0.5F * (1.0F - cos_theta)); // Trig half angle identity. Always positive. - vmax_junction = std::min(vmax_junction, sqrtf(acceleration * junction_deviation * sin_theta_d2 / (1.0F - sin_theta_d2))); - } - } - } - } - block->max_entry_speed = vmax_junction; - - // Initialize block entry speed. Compute based on deceleration to user-defined minimum_planner_speed. - float v_allowable = max_allowable_speed(-acceleration, minimum_planner_speed, block->millimeters); - block->entry_speed = std::min(vmax_junction, v_allowable); - - // Initialize planner efficiency flags - // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds. - // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then - // the current block and next block junction speeds are guaranteed to always be at their maximum - // junction speeds in deceleration and acceleration, respectively. This is due to how the current - // block nominal speed limits both the current and next maximum junction speeds. Hence, in both - // the reverse and forward planners, the corresponding block junction speed will always be at the - // the maximum junction speed and may always be ignored for any speed reduction checks. - if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; } - else { block->nominal_length_flag = false; } - - // Always calculate trapezoid for new block - block->recalculate_flag = true; - - // Update previous path unit_vector and nominal speed - if(unit_vec != nullptr) { - memcpy(previous_unit_vec, unit_vec, sizeof(previous_unit_vec)); // previous_unit_vec[] = unit_vec[] - } else { - memset(previous_unit_vec, 0, sizeof(previous_unit_vec)); - } - - // Math-heavy re-computing of the whole queue to take the new - this->recalculate(); - - // The block can now be used - block->ready(); - - // here's where the hang / while occurs - conveyor->queue_head_block(); - - return true; -} - -void Planner::recalculate() -{ - Conveyor::Queue_t &queue = conveyor->queue; - - unsigned int block_index; - - Block* previous; - Block* current; - - /* - * a newly added block is decel limited - * - * we find its max entry speed given its exit speed - * - * for each block, walking backwards in the queue: - * - * if max entry speed == current entry speed - * then we can set recalculate to false, since clearly adding another block didn't allow us to enter faster - * and thus we don't need to check entry speed for this block any more - * - * once we find an accel limited block, we must find the max exit speed and walk the queue forwards - * - * for each block, walking forwards in the queue: - * - * given the exit speed of the previous block and our own max entry speed - * we can tell if we're accel or decel limited (or coasting) - * - * if prev_exit > max_entry - * then we're still decel limited. update previous trapezoid with our max entry for prev exit - * if max_entry >= prev_exit - * then we're accel limited. set recalculate to false, work out max exit speed - * - * finally, work out trapezoid for the final (and newest) block. - */ - - /* - * Step 1: - * For each block, given the exit speed and acceleration, find the maximum entry speed - */ - - float entry_speed = minimum_planner_speed; - - block_index = queue.head_i; - current = queue.item_ref(block_index); - - if (!queue.is_empty()) { - while ((block_index != queue.tail_i) && current->recalculate_flag) { - entry_speed = current->reverse_pass(entry_speed); - - block_index = queue.prev(block_index); - current = queue.item_ref(block_index); - } - - /* - * Step 2: - * now current points to either tail or first non-recalculate block - * and has not had its reverse_pass called - * or its calculate_trapezoid - * entry_speed is set to the *exit* speed of current. - * each block from current to head has its entry speed set to its max entry speed- limited by decel or nominal_rate - */ - - float exit_speed = current->max_exit_speed(); - - while (block_index != queue.head_i) { - previous = current; - block_index = queue.next(block_index); - current = queue.item_ref(block_index); - - // we pass the exit speed of the previous block - // so this block can decide if it's accel or decel limited and update its fields as appropriate - exit_speed = current->forward_pass(exit_speed); - - previous->calculate_trapezoid(previous->entry_speed, current->entry_speed); - } - } - - /* - * Step 3: - * work out trapezoid for final (and newest) block - */ - - // now current points to the head item - // which has not had calculate_trapezoid run yet - current->calculate_trapezoid(current->entry_speed, minimum_planner_speed); -} - - -// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the -// acceleration within the allotted distance. -float Planner::max_allowable_speed(float acceleration, float target_velocity, float distance) -{ - // Was acceleration*60*60*distance, in case this breaks, but here we prefer to use seconds instead of minutes - return(sqrtf(target_velocity * target_velocity - 2.0F * acceleration * distance)); -} - - diff --git a/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Planner.h b/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Planner.h deleted file mode 100644 index c0ed704..0000000 --- a/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Planner.h +++ /dev/null @@ -1,49 +0,0 @@ -/* - This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). - Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - Smoothie 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 General Public License for more details. - You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>. -*/ - -#ifndef PLANNER_H -#define PLANNER_H - -#include "ActuatorCoordinates.h" -#include <Arduino.h> -#include "../../../drivers/indicators.h" - -#include "Block.h" -#include "Conveyor.h" -#include "../../SmoothieRoll.h" - -class Block; - -class Planner -{ -public: - Planner(); - static Planner* getInstance(void); - float max_allowable_speed( float acceleration, float target_velocity, float distance); - bool append_block(ActuatorCoordinates &target, uint8_t n_motors, float rate_mm_s, float distance, float unit_vec[], float accleration, float s_value, bool g123); - void append_move(float *target, uint8_t n_motors, float rate); - void set_position(float *target, uint8_t n_motors); - volatile boolean do_set_position = false; - - // track for increments - float last_position[3] = {0,0,0}; - - friend class Robot; // for acceleration, junction deviation, minimum_planner_speed - -private: - static Planner* instance; - void recalculate(); - void config_load(); - float previous_unit_vec[3]; - float junction_deviation; // Setting - float z_junction_deviation; // Setting - float minimum_planner_speed; // Setting -}; - -extern Planner* planner; - -#endif diff --git a/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/StepInterface.cpp b/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/StepInterface.cpp deleted file mode 100644 index 7cf29a0..0000000 --- a/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/StepInterface.cpp +++ /dev/null @@ -1,61 +0,0 @@ -/* - -StepInterface.cpp - -driver interface hack for smoothie port - -*/ - -#include "StepInterface.h" - -StepInterface::StepInterface(void){} - -int32_t StepInterface::steps_to_target(float target){ - int32_t target_steps = lroundf(target * steps_per_mm); - return target_steps - last_milestone_steps; -} - -void StepInterface::update_last_milestones(float mm, int32_t steps){ - last_milestone_steps += steps; - last_milestone_mm = mm; -} - -void StepInterface::set_position(float mm){ - last_milestone_mm = mm; - last_milestone_steps = lroundf(mm * steps_per_mm); - // values *jake* tracks in rt - stepwise_position = last_milestone_steps; - floating_position = last_milestone_mm; -} - -float StepInterface::get_last_milestone_mm(void){ - return last_milestone_mm; -} - -boolean StepInterface::step(void){ - // upd8 position, - if(direction){ - stepwise_position --; - } else { - stepwise_position ++; - } - floating_position = stepwise_position * mm_per_step; - // do step things - return moving; -} - -void StepInterface::set_direction(boolean dir){ - direction = dir; -} - -void StepInterface::start_moving(void){ - moving = true; -} - -void StepInterface::stop_moving(void){ - moving = false; -} - -boolean StepInterface::is_moving(void){ - return moving; -} \ No newline at end of file diff --git a/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/StepInterface.h b/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/StepInterface.h deleted file mode 100644 index b9d2f6d..0000000 --- a/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/StepInterface.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - -motor.h - -driver interface hack for smoothie port - -*/ - -#ifndef STEPINTERFACE_H_ -#define STEPINTERFACE_H_ - -#include <Arduino.h> - -class StepInterface { - public: - StepInterface(void); - - int32_t steps_to_target(float target); - void update_last_milestones(float mm, int32_t steps); - void set_position(float mm); - float get_last_milestone_mm(void); - - boolean step(void); - void set_direction(boolean dir); - void start_moving(void); - void stop_moving(void); - boolean is_moving(void); - // could also use this structure to setup steps / mm, max accel, max rate per actuator - - // for net interface, - volatile int32_t stepwise_position = 0; - volatile float floating_position = 0.0F; - - private: - volatile boolean direction = false; - volatile boolean moving = false; - - float steps_per_mm = 400.0F; - float mm_per_step = 1 / steps_per_mm; - int32_t last_milestone_steps = 0; - float last_milestone_mm = 0; -}; - -#endif \ No newline at end of file diff --git a/firmware/osape-smoothieroll-head/src/utils/clocks_d51_module.cpp b/firmware/osape-smoothieroll-head/src/utils/clocks_d51_module.cpp deleted file mode 100644 index af2c83c..0000000 --- a/firmware/osape-smoothieroll-head/src/utils/clocks_d51_module.cpp +++ /dev/null @@ -1,115 +0,0 @@ -/* -utils/clocks_d51_module.cpp - -clock utilities for the D51 as moduuularized, adhoc! -i.e. xtals present on module board or otherwise - -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 "clocks_d51_module.h" - -D51_Clock_Boss* D51_Clock_Boss::instance = 0; - -D51_Clock_Boss* D51_Clock_Boss::getInstance(void){ - if(instance == 0){ - instance = new D51_Clock_Boss(); - } - return instance; -} - -D51_Clock_Boss* d51_clock_boss = D51_Clock_Boss::getInstance(); - -D51_Clock_Boss::D51_Clock_Boss(){} - -void D51_Clock_Boss::setup_16mhz_xtal(void){ - if(mhz_xtal_is_setup) return; // already done, - // let's make a clock w/ that xtal: - OSCCTRL->XOSCCTRL[0].bit.RUNSTDBY = 0; - OSCCTRL->XOSCCTRL[0].bit.XTALEN = 1; - // set oscillator current.. - OSCCTRL->XOSCCTRL[0].reg |= OSCCTRL_XOSCCTRL_IMULT(4) | OSCCTRL_XOSCCTRL_IPTAT(3); - OSCCTRL->XOSCCTRL[0].reg |= OSCCTRL_XOSCCTRL_STARTUP(5); - OSCCTRL->XOSCCTRL[0].bit.ENALC = 1; - OSCCTRL->XOSCCTRL[0].bit.ENABLE = 1; - // make the peripheral clock available on this ch - GCLK->GENCTRL[MHZ_XTAL_GCLK_NUM].reg = GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_XOSC0) | GCLK_GENCTRL_GENEN; // GCLK_GENCTRL_SRC_DFLL - while (GCLK->SYNCBUSY.reg & GCLK_SYNCBUSY_GENCTRL(MHZ_XTAL_GCLK_NUM)){ - DEBUG2PIN_TOGGLE; - }; - mhz_xtal_is_setup = true; -} - -void D51_Clock_Boss::start_100kHz_ticker_tc0(void){ - setup_16mhz_xtal(); - // ok - TC0->COUNT32.CTRLA.bit.ENABLE = 0; - TC1->COUNT32.CTRLA.bit.ENABLE = 0; - // unmask clocks - MCLK->APBAMASK.reg |= MCLK_APBAMASK_TC0 | MCLK_APBAMASK_TC1; - // ok, clock to these channels... - GCLK->PCHCTRL[TC0_GCLK_ID].reg = GCLK_PCHCTRL_CHEN | GCLK_PCHCTRL_GEN(d51_clock_boss->mhz_xtal_gclk_num); - GCLK->PCHCTRL[TC1_GCLK_ID].reg = GCLK_PCHCTRL_CHEN | GCLK_PCHCTRL_GEN(d51_clock_boss->mhz_xtal_gclk_num); - // turn them ooon... - TC0->COUNT32.CTRLA.reg = TC_CTRLA_MODE_COUNT32 | TC_CTRLA_PRESCSYNC_PRESC | TC_CTRLA_PRESCALER_DIV2 | TC_CTRLA_CAPTEN0; - TC1->COUNT32.CTRLA.reg = TC_CTRLA_MODE_COUNT32 | TC_CTRLA_PRESCSYNC_PRESC | TC_CTRLA_PRESCALER_DIV2 | TC_CTRLA_CAPTEN0; - // going to set this up to count at some time, we will tune - // that freq. with - TC0->COUNT32.WAVE.reg = TC_WAVE_WAVEGEN_MFRQ; - TC1->COUNT32.WAVE.reg = TC_WAVE_WAVEGEN_MFRQ; - // allow interrupt to trigger on this event (overflow) - TC0->COUNT32.INTENSET.bit.MC0 = 1; - TC0->COUNT32.INTENSET.bit.MC1 = 1; - // set the period, - while (TC0->COUNT32.SYNCBUSY.bit.CC0); - TC0->COUNT32.CC[0].reg = 80; // at DIV2, 240 for 10us ('realtime') (with - // DFLL), 80 for 10us (with XTAL 16MHZ) - // 400 for 50us, - // enable, sync for enable write - while (TC0->COUNT32.SYNCBUSY.bit.ENABLE); - TC0->COUNT32.CTRLA.bit.ENABLE = 1; - while (TC0->COUNT32.SYNCBUSY.bit.ENABLE); - TC1->COUNT32.CTRLA.bit.ENABLE = 1; - // enable the IRQ - NVIC_EnableIRQ(TC0_IRQn); -} - -void D51_Clock_Boss::start_100kHz_ticker_tc2(void){ - setup_16mhz_xtal(); - // ok - TC2->COUNT32.CTRLA.bit.ENABLE = 0; - TC3->COUNT32.CTRLA.bit.ENABLE = 0; - // unmask clocks - MCLK->APBBMASK.reg |= MCLK_APBBMASK_TC2 | MCLK_APBBMASK_TC3; - // ok, clock to these channels... - GCLK->PCHCTRL[TC2_GCLK_ID].reg = GCLK_PCHCTRL_CHEN | GCLK_PCHCTRL_GEN(d51_clock_boss->mhz_xtal_gclk_num); - GCLK->PCHCTRL[TC3_GCLK_ID].reg = GCLK_PCHCTRL_CHEN | GCLK_PCHCTRL_GEN(d51_clock_boss->mhz_xtal_gclk_num); - // turn them ooon... - TC2->COUNT32.CTRLA.reg = TC_CTRLA_MODE_COUNT32 | TC_CTRLA_PRESCSYNC_PRESC | TC_CTRLA_PRESCALER_DIV2 | TC_CTRLA_CAPTEN0; - TC3->COUNT32.CTRLA.reg = TC_CTRLA_MODE_COUNT32 | TC_CTRLA_PRESCSYNC_PRESC | TC_CTRLA_PRESCALER_DIV2 | TC_CTRLA_CAPTEN0; - // going to set this up to count at some time, we will tune - // that freq. with - TC2->COUNT32.WAVE.reg = TC_WAVE_WAVEGEN_MFRQ; - TC3->COUNT32.WAVE.reg = TC_WAVE_WAVEGEN_MFRQ; - // allow interrupt to trigger on this event (overflow) - TC2->COUNT32.INTENSET.bit.MC0 = 1; - TC2->COUNT32.INTENSET.bit.MC1 = 1; - // set the period, - while (TC2->COUNT32.SYNCBUSY.bit.CC0); - TC2->COUNT32.CC[0].reg = 80; // at DIV2, 240 for 10us ('realtime') (with - // DFLL), 80 for 10us (with XTAL 16MHZ) - // 400 for 50us, - // enable, sync for enable write - while (TC2->COUNT32.SYNCBUSY.bit.ENABLE); - TC2->COUNT32.CTRLA.bit.ENABLE = 1; - while (TC2->COUNT32.SYNCBUSY.bit.ENABLE); - TC3->COUNT32.CTRLA.bit.ENABLE = 1; - // enable the IRQ - NVIC_EnableIRQ(TC2_IRQn); -} \ No newline at end of file diff --git a/firmware/osape-smoothieroll-head/src/utils/clocks_d51_module.h b/firmware/osape-smoothieroll-head/src/utils/clocks_d51_module.h deleted file mode 100644 index 084141e..0000000 --- a/firmware/osape-smoothieroll-head/src/utils/clocks_d51_module.h +++ /dev/null @@ -1,43 +0,0 @@ -/* -utils/clocks_d51_module.h - -clock utilities for the D51 as moduuularized, adhoc! -i.e. xtals present on module board or otherwise - -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 CLOCKS_D51_MODULE_H_ -#define CLOCKS_D51_MODULE_H_ - -#include <Arduino.h> - -#include "../drivers/indicators.h" - -#define MHZ_XTAL_GCLK_NUM 9 - -class D51_Clock_Boss { - private: - static D51_Clock_Boss* instance; - public: - D51_Clock_Boss(); - static D51_Clock_Boss* getInstance(void); - // xtal - volatile boolean mhz_xtal_is_setup = false; - uint32_t mhz_xtal_gclk_num = 9; - void setup_16mhz_xtal(void); - // builds 100kHz clock on TC0 or TC2 - // todo: tell these fns a frequency, - void start_100kHz_ticker_tc0(void); - void start_100kHz_ticker_tc2(void); -}; - -extern D51_Clock_Boss* d51_clock_boss; - -#endif \ No newline at end of file diff --git a/firmware/osape-smoothieroll-head/src/utils/cobs.cpp b/firmware/osape-smoothieroll-head/src/utils/cobs.cpp deleted file mode 100644 index a2d7378..0000000 --- a/firmware/osape-smoothieroll-head/src/utils/cobs.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/* -utils/cobs.cpp - -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 "cobs.h" -// str8 crib from -// https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing - -#define StartBlock() (code_ptr = dst++, code = 1) -#define FinishBlock() (*code_ptr = code) - -size_t cobsEncode(uint8_t *ptr, size_t length, uint8_t *dst){ - const uint8_t *start = dst, *end = ptr + length; - uint8_t code, *code_ptr; /* Where to insert the leading count */ - - StartBlock(); - while (ptr < end) { - if (code != 0xFF) { - uint8_t c = *ptr++; - if (c != 0) { - *dst++ = c; - code++; - continue; - } - } - FinishBlock(); - StartBlock(); - } - FinishBlock(); - // write the actual zero, - *dst++ = 0; - return dst - start; -} - -size_t cobsDecode(uint8_t *ptr, size_t length, uint8_t *dst) -{ - const uint8_t *start = dst, *end = ptr + length; - uint8_t code = 0xFF, copy = 0; - - for (; ptr < end; copy--) { - if (copy != 0) { - *dst++ = *ptr++; - } else { - if (code != 0xFF) - *dst++ = 0; - copy = code = *ptr++; - if (code == 0) - break; /* Source length too long */ - } - } - return dst - start; -} diff --git a/firmware/osape-smoothieroll-head/src/utils/cobs.h b/firmware/osape-smoothieroll-head/src/utils/cobs.h deleted file mode 100644 index a9e5874..0000000 --- a/firmware/osape-smoothieroll-head/src/utils/cobs.h +++ /dev/null @@ -1,24 +0,0 @@ -/* -utils/cobs.h - -consistent overhead byte stuffing implementation - -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 UTIL_COBS_H_ -#define UTIL_COBS_H_ - -#include <arduino.h> - -size_t cobsEncode(uint8_t *src, size_t len, uint8_t *dest); - -size_t cobsDecode(uint8_t *src, size_t len, uint8_t *dest); - -#endif diff --git a/firmware/osape-smoothieroll-head/src/utils/syserror.cpp b/firmware/osape-smoothieroll-head/src/utils/syserror.cpp deleted file mode 100644 index a2473fe..0000000 --- a/firmware/osape-smoothieroll-head/src/utils/syserror.cpp +++ /dev/null @@ -1,38 +0,0 @@ -#include "syserror.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; -} -*/ - -// config-your-own-ll-escape-hatch -void sysError(String msg){ - // whatever you want, - //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); - size_t ecl = cobsEncode(errBuf, len + 5, errEncoded); - if(Serial.availableForWrite() > (int64_t)ecl){ - Serial.write(errEncoded, ecl); - Serial.flush(); - } -} diff --git a/firmware/osape-smoothieroll-head/src/utils/syserror.h b/firmware/osape-smoothieroll-head/src/utils/syserror.h deleted file mode 100644 index b421cc2..0000000 --- a/firmware/osape-smoothieroll-head/src/utils/syserror.h +++ /dev/null @@ -1,11 +0,0 @@ -#ifndef SYSERROR_H_ -#define SYSERROR_H_ - -#include <arduino.h> -#include "./drivers/indicators.h" -#include "./utils/cobs.h" -#include "./osap/ts.h" - -void sysError(String msg); - -#endif diff --git a/firmware/osape-smoothieroll-head/test/README b/firmware/osape-smoothieroll-head/test/README deleted file mode 100644 index df5066e..0000000 --- a/firmware/osape-smoothieroll-head/test/README +++ /dev/null @@ -1,11 +0,0 @@ - -This directory is intended for PIO 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 PIO Unit Testing: -- https://docs.platformio.org/page/plus/unit-testing.html -- GitLab