diff --git a/circuits/2020-06_ucbus-stepper b/circuits/2020-06_ucbus-stepper new file mode 160000 index 0000000000000000000000000000000000000000..1ce3c0d0c1d5435059e893a62ebdea0f15526987 --- /dev/null +++ b/circuits/2020-06_ucbus-stepper @@ -0,0 +1 @@ +Subproject commit 1ce3c0d0c1d5435059e893a62ebdea0f15526987 diff --git a/circuits/2020-08_esc-breakout b/circuits/2020-08_esc-breakout new file mode 160000 index 0000000000000000000000000000000000000000..7fa3c7a4e6c4da3aecf4cc56bf60e1acb2e5be3f --- /dev/null +++ b/circuits/2020-08_esc-breakout @@ -0,0 +1 @@ +Subproject commit 7fa3c7a4e6c4da3aecf4cc56bf60e1acb2e5be3f diff --git a/circuits/2020-08_psu-breakout b/circuits/2020-08_psu-breakout new file mode 160000 index 0000000000000000000000000000000000000000..771b21af419147e0feb48129fa2bae5ee6823543 --- /dev/null +++ b/circuits/2020-08_psu-breakout @@ -0,0 +1 @@ +Subproject commit 771b21af419147e0feb48129fa2bae5ee6823543 diff --git a/firmware/osape-smoothieroll-drop-esc/.gitignore b/firmware/osape-smoothieroll-drop-esc/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..89cc49cbd652508924b868ea609fa8f6b758ec56 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/firmware/osape-smoothieroll-drop-esc/.vscode/extensions.json b/firmware/osape-smoothieroll-drop-esc/.vscode/extensions.json new file mode 100644 index 0000000000000000000000000000000000000000..e80666bfb11e75aafd3600701dc99c10acce341c --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ] +} diff --git a/firmware/osape-smoothieroll-drop-esc/include/README b/firmware/osape-smoothieroll-drop-esc/include/README new file mode 100644 index 0000000000000000000000000000000000000000..194dcd43252dcbeb2044ee38510415041a0e7b47 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/firmware/osape-smoothieroll-drop-esc/lib/README b/firmware/osape-smoothieroll-drop-esc/lib/README new file mode 100644 index 0000000000000000000000000000000000000000..6debab1e8b4c3faa0d06f4ff44bce343ce2cdcbf --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include <Foo.h> +#include <Bar.h> + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/firmware/osape-smoothieroll-drop-esc/platformio.ini b/firmware/osape-smoothieroll-drop-esc/platformio.ini new file mode 100644 index 0000000000000000000000000000000000000000..50208f0cadb2e924ce3a661e45b5c88422dc71af --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/platformio.ini @@ -0,0 +1,14 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:adafruit_feather_m4] +platform = atmelsam +board = adafruit_feather_m4 +framework = arduino diff --git a/firmware/osape-smoothieroll-drop-esc/src/drivers/dip_ucbus_config.cpp b/firmware/osape-smoothieroll-drop-esc/src/drivers/dip_ucbus_config.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e0316b389b72c91a640a75883c4f426b9e5c07cf --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/drivers/dip_ucbus_config.cpp @@ -0,0 +1,55 @@ +// DIPs + +#include "dip_ucbus_config.h" + +void dip_init(void){ + // set direction in, + DIP_PORT.DIRCLR.reg = D_BM(D0_PIN) | D_BM(D1_PIN) | D_BM(D2_PIN) | D_BM(D3_PIN) | D_BM(D4_PIN) | D_BM(D5_PIN) | D_BM(D6_PIN) | D_BM(D7_PIN); + // enable in, + DIP_PORT.PINCFG[D0_PIN].bit.INEN = 1; + DIP_PORT.PINCFG[D1_PIN].bit.INEN = 1; + DIP_PORT.PINCFG[D2_PIN].bit.INEN = 1; + DIP_PORT.PINCFG[D3_PIN].bit.INEN = 1; + DIP_PORT.PINCFG[D4_PIN].bit.INEN = 1; + DIP_PORT.PINCFG[D5_PIN].bit.INEN = 1; + DIP_PORT.PINCFG[D6_PIN].bit.INEN = 1; + DIP_PORT.PINCFG[D7_PIN].bit.INEN = 1; + // enable pull, + DIP_PORT.PINCFG[D0_PIN].bit.PULLEN = 1; + DIP_PORT.PINCFG[D1_PIN].bit.PULLEN = 1; + DIP_PORT.PINCFG[D2_PIN].bit.PULLEN = 1; + DIP_PORT.PINCFG[D3_PIN].bit.PULLEN = 1; + DIP_PORT.PINCFG[D4_PIN].bit.PULLEN = 1; + DIP_PORT.PINCFG[D5_PIN].bit.PULLEN = 1; + DIP_PORT.PINCFG[D6_PIN].bit.PULLEN = 1; + DIP_PORT.PINCFG[D7_PIN].bit.PULLEN = 1; + // 'pull' references the value set in the 'out' register, so to pulldown: + DIP_PORT.OUTCLR.reg = D_BM(D0_PIN) | D_BM(D1_PIN) | D_BM(D2_PIN) | D_BM(D3_PIN) | D_BM(D4_PIN) | D_BM(D5_PIN) | D_BM(D6_PIN) | D_BM(D7_PIN); +} + +uint8_t dip_read_lower_five(void){ + uint32_t bits[5] = {0,0,0,0,0}; + if(DIP_PORT.IN.reg & D_BM(D7_PIN)) { bits[0] = 1; } + if(DIP_PORT.IN.reg & D_BM(D6_PIN)) { bits[1] = 1; } + if(DIP_PORT.IN.reg & D_BM(D5_PIN)) { bits[2] = 1; } + if(DIP_PORT.IN.reg & D_BM(D4_PIN)) { bits[3] = 1; } + if(DIP_PORT.IN.reg & D_BM(D3_PIN)) { bits[4] = 1; } + /* + bits[0] = (DIP_PORT.IN.reg & D_BM(D7_PIN)) >> D7_PIN; + bits[1] = (DIP_PORT.IN.reg & D_BM(D6_PIN)) >> D6_PIN; + bits[2] = (DIP_PORT.IN.reg & D_BM(D5_PIN)) >> D5_PIN; + bits[3] = (DIP_PORT.IN.reg & D_BM(D4_PIN)) >> D4_PIN; + bits[4] = (DIP_PORT.IN.reg & D_BM(D3_PIN)) >> D3_PIN; + */ + uint32_t word = 0; + word = word | (bits[4] << 4) | (bits[3] << 3) | (bits[2] << 2) | (bits[1] << 1) | (bits[0] << 0); + return (uint8_t)word; +} + +boolean dip_read_pin_0(void){ + return DIP_PORT.IN.reg & D_BM(D0_PIN); +} + +boolean dip_read_pin_1(void){ + return DIP_PORT.IN.reg & D_BM(D1_PIN); +} \ No newline at end of file diff --git a/firmware/osape-smoothieroll-drop-esc/src/drivers/dip_ucbus_config.h b/firmware/osape-smoothieroll-drop-esc/src/drivers/dip_ucbus_config.h new file mode 100644 index 0000000000000000000000000000000000000000..44eaa00d1fb1fd0f30118308ba436cd136cc344a --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/drivers/dip_ucbus_config.h @@ -0,0 +1,24 @@ +// DIP switch HAL macros +// pardon the mis-labeling: on board, and in the schem, these are 1-8, +// here they will be 0-7 + +// note: these are 'on' hi by default, from the factory. +// to set low, need to turn the internal pulldown on + +#include <Arduino.h> + +#define D0_PIN 5 +#define D1_PIN 4 +#define D2_PIN 3 +#define D3_PIN 2 +#define D4_PIN 1 +#define D5_PIN 0 +#define D6_PIN 31 +#define D7_PIN 30 +#define DIP_PORT PORT->Group[1] +#define D_BM(val) ((uint32_t)(1 << val)) + +void dip_init(void); +uint8_t dip_read_lower_five(void); +boolean dip_read_pin_0(void); // bus-head (hi) or bus-drop (lo) +boolean dip_read_pin_1(void); // if bus-drop, te-enable (hi) or no (lo) \ No newline at end of file diff --git a/firmware/osape-smoothieroll-drop-esc/src/drivers/indicators.h b/firmware/osape-smoothieroll-drop-esc/src/drivers/indicators.h new file mode 100644 index 0000000000000000000000000000000000000000..7b191d290b38becc583dadf8e1bcb1c160633a37 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/drivers/indicators.h @@ -0,0 +1,42 @@ +// 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 20 +#define DEBUG1PIN_PORT PORT->Group[0] +#define DEBUG2PIN_PIN 6 +#define DEBUG2PIN_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 \ No newline at end of file diff --git a/firmware/osape-smoothieroll-drop-esc/src/drivers/peripheral_nums.h b/firmware/osape-smoothieroll-drop-esc/src/drivers/peripheral_nums.h new file mode 100644 index 0000000000000000000000000000000000000000..eed9f188afacfb0da271d43603f833f61ec61191 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/drivers/peripheral_nums.h @@ -0,0 +1,18 @@ +#ifndef PERIPHERAL_NUMS_H_ +#define PERIPHERAL_NUMS_H_ + +#define PERIPHERAL_A 0 +#define PERIPHERAL_B 1 +#define PERIPHERAL_C 2 +#define PERIPHERAL_D 3 +#define PERIPHERAL_E 4 +#define PERIPHERAL_F 5 +#define PERIPHERAL_G 6 +#define PERIPHERAL_H 7 +#define PERIPHERAL_I 8 +#define PERIPHERAL_K 9 +#define PERIPHERAL_L 10 +#define PERIPHERAL_M 11 +#define PERIPHERAL_N 12 + +#endif \ No newline at end of file diff --git a/firmware/osape-smoothieroll-drop-esc/src/drivers/pulse_counter.cpp b/firmware/osape-smoothieroll-drop-esc/src/drivers/pulse_counter.cpp new file mode 100644 index 0000000000000000000000000000000000000000..5edd44f695c8532943346ce5a7dec9873af239d7 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/drivers/pulse_counter.cpp @@ -0,0 +1,116 @@ +/* +drivers/pulse_counter.cpp + +count input width: developed to measure RPM of rotor bell past hall sensor + +Jake Read at the Center for Bits and Atoms +(c) Massachusetts Institute of Technology 2020 + +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 "pulse_counter.h" +#include "../utils/clocks_d51_module.h" +#include "indicators.h" +#include "peripheral_nums.h" + +Pulse_Counter* Pulse_Counter::instance = 0; + +Pulse_Counter* Pulse_Counter::getInstance(void){ + if(instance == 0){ + instance = new Pulse_Counter(); + } + return instance; +} + +Pulse_Counter* pulse_counter = Pulse_Counter::getInstance(); + +Pulse_Counter::Pulse_Counter(){} + +// ok, currently setup with push/pull to input on PB14, which has TC4-0 on Peripheral E + +#define PULSE_PORT PORT->Group[1] +#define PULSE_PIN 12 +#define PULSE_PIN_BM (uint32_t)(1 << PULSE_PIN) + +void Pulse_Counter::init(void){ + // setup PB14 to this peripheral + PULSE_PORT.DIRCLR.reg = PULSE_PIN_BM; // not-output + PULSE_PORT.PINCFG[PULSE_PIN].bit.PMUXEN = 1; // allow peripheral mux + if(PULSE_PIN % 2){ + PULSE_PORT.PMUX[PULSE_PIN >> 1].reg |= PORT_PMUX_PMUXO(PERIPHERAL_E); + } else { + PULSE_PORT.PMUX[PULSE_PIN >> 1].reg |= PORT_PMUX_PMUXE(PERIPHERAL_E); + } + // setup the xtal, will use to send a clock to this timer + d51_clock_boss->setup_16mhz_xtal(); + // disable + TC4->COUNT16.CTRLA.bit.ENABLE = 0; // disable + // TC4->COUNT16.CTRLA.bit.SWRST = 1; // reset (would properly do this, but works without, doesn't work with) + // unmask clocks + MCLK->APBCMASK.reg |= MCLK_APBCMASK_TC4; + // send a clock to the ch + GCLK->PCHCTRL[TC4_GCLK_ID].reg = GCLK_PCHCTRL_CHEN + | GCLK_PCHCTRL_GEN(d51_clock_boss->mhz_xtal_gclk_num); + // setup timer + TC4->COUNT16.CTRLA.reg = TC_CTRLA_MODE_COUNT16 // 16 bit timer + | TC_CTRLA_PRESCSYNC_PRESC // reload / reset on prescaler + | TC_CTRLA_PRESCALER_DIV4 // div the input clock (CALC TPS WITH THIS) + | TC_CTRLA_CAPTEN0 // enable for capture option on ch0 + | TC_CTRLA_COPEN0; // capture on pin + // capture event defaults to the rising edge, that's OK. + // we need to get a capture and overflow interrupt, + TC4->COUNT16.INTENSET.bit.MC0 = 1; + TC4->COUNT16.INTENSET.bit.OVF = 1; + // now enable it, + while(TC4->COUNT16.SYNCBUSY.bit.ENABLE); + TC4->COUNT16.CTRLA.bit.ENABLE = 1; + // enable the IRQ + NVIC_EnableIRQ(TC4_IRQn); +} + +void TC4_Handler(void){ + DEBUG2PIN_TOGGLE; + if(TC4->COUNT16.INTFLAG.bit.MC0){ + uint16_t width; + pulse_counter->_lastWasOVF ? width = 65534 : width = TC4->COUNT16.CC[0].reg; + pulse_counter->_lastWasOVF = false; + // clear, stash, and reset + TC4->COUNT16.INTFLAG.bit.MC0 = 1; // clear + pulse_counter->addPulse(width); // stash + TC4->COUNT16.CTRLBSET.reg = TC_CTRLBSET_CMD_RETRIGGER; // restart (?) + //DEBUG1PIN_TOGGLE; + } + if(TC4->COUNT16.INTFLAG.bit.OVF){ + // clear, stash, reset is already happening + pulse_counter->_lastWasOVF = true; + TC4->COUNT16.INTFLAG.bit.OVF = 1; + pulse_counter->addPulse(65534); // the long nap + //DEBUG2PIN_TOGGLE; + } +} + +void Pulse_Counter::addPulse(uint16_t width){ + _widths[_wh] = width; + _wh ++; + if(_wh >= PULSE_WIDTHS_COUNT){ + _wh = 0; + } +} + +float Pulse_Counter::getAverageWidth(void){ + float sum = 0; + NVIC_DisableIRQ(TC4_IRQn); + for(uint8_t i = 0; i < PULSE_WIDTHS_COUNT; i ++){ + sum += _widths[i]; + } + NVIC_EnableIRQ(TC4_IRQn); + return sum / (float)PULSE_WIDTHS_COUNT; +} + +float Pulse_Counter::getTicksPerSecond(void){ + return PULSE_TICKS_PER_SECOND; +} \ No newline at end of file diff --git a/firmware/osape-smoothieroll-drop-esc/src/drivers/pulse_counter.h b/firmware/osape-smoothieroll-drop-esc/src/drivers/pulse_counter.h new file mode 100644 index 0000000000000000000000000000000000000000..077abc022be830987c44df44d8e3f2a8368e81ae --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/drivers/pulse_counter.h @@ -0,0 +1,40 @@ +/* +drivers/pulse_counter.h + +count input width: developed to measure RPM of rotor bell past hall sensor + +Jake Read at the Center for Bits and Atoms +(c) Massachusetts Institute of Technology 2020 + +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 PULSE_COUNTER_H_ +#define PULSE_COUNTER_H_ + +#include <Arduino.h> + +#define PULSE_WIDTHS_COUNT 16 +#define PULSE_TICKS_PER_SECOND 4000000 // DIV256: 62500, DIV4: 4000000 + +class Pulse_Counter { + private: + static Pulse_Counter* instance; + volatile uint16_t _widths[PULSE_WIDTHS_COUNT]; + volatile uint8_t _wh = 0; // width head (write to) + public: + Pulse_Counter(); + static Pulse_Counter* getInstance(void); + void init(void); + volatile boolean _lastWasOVF = true; + void addPulse(uint16_t width); + float getAverageWidth(void); + float getTicksPerSecond(void); +}; + +extern Pulse_Counter* pulse_counter; + +#endif \ No newline at end of file diff --git a/firmware/osape-smoothieroll-drop-esc/src/drivers/servo_pwm.cpp b/firmware/osape-smoothieroll-drop-esc/src/drivers/servo_pwm.cpp new file mode 100644 index 0000000000000000000000000000000000000000..89665bfc9ad5d80edec5c56eb7bffc29d0bb8a0a --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/drivers/servo_pwm.cpp @@ -0,0 +1,106 @@ +/* +drivers/servo_pwm.cpp + +output servo-type (20ms period, 1-2ms duty cycle) with TC on D51 + +Jake Read at the Center for Bits and Atoms +(c) Massachusetts Institute of Technology 2020 + +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 "servo_pwm.h" +#include "../utils/clocks_d51_module.h" +#include "indicators.h" +#include "peripheral_nums.h" + +Servo_PWM* Servo_PWM::instance = 0; + +Servo_PWM* Servo_PWM::getInstance(void){ + if(instance == 0){ + instance = new Servo_PWM(); + } + return instance; +} + +Servo_PWM* servo_pwm = Servo_PWM::getInstance(); + +Servo_PWM::Servo_PWM(){} + +#define PWM_PORT PORT->Group[0] +#define PWM_PIN 22 +#define PWM_PIN_BM (uint32_t)(1 << PWM_PIN) +#define PWM_PIN_HI PWM_PORT.OUTSET.reg = PWM_PIN_BM +#define PWM_PIN_LO PWM_PORT.OUTCLR.reg = PWM_PIN_BM + +// PWM for servos is 20ms period, 1-2ms duty. +// do 2MHz input clock, wrap at 40000 ticks +// 1ms is 2000 ticks, 2ms is 4000 ticks + +void Servo_PWM::init(void){ + // set the pin as output: this is toggled on interrupt, not hardware TC + PWM_PORT.DIRSET.reg = PWM_PIN_BM; + // setup xtal for timer clk + d51_clock_boss->setup_16mhz_xtal(); + // disable to setup + TC3->COUNT16.CTRLA.bit.ENABLE = 0; + // unmask clock + MCLK->APBBMASK.reg |= MCLK_APBBMASK_TC3; + // send clk to ch + GCLK->PCHCTRL[TC3_GCLK_ID].reg = GCLK_PCHCTRL_CHEN + | GCLK_PCHCTRL_GEN(d51_clock_boss->mhz_xtal_gclk_num); + // setup timer + TC3->COUNT16.CTRLA.reg = TC_CTRLA_MODE_COUNT16 + | TC_CTRLA_PRESCSYNC_PRESC + | TC_CTRLA_PRESCALER_DIV8; + // want match pwm (MPWM) mode, should have a TOP register for period, and CC[x] for the swap + TC3->COUNT16.WAVE.reg = TC_WAVE_WAVEGEN_MPWM; + TC3->COUNT16.CC[0].reg = 5000; // CC0 is 'top' in match pwm: 40k here for 20ms period, 5k for 2.5ms / 400hz + TC3->COUNT16.CC[1].reg = 2000; // this should put the first tick at 1ms + // want interrupts + TC3->COUNT16.INTENSET.bit.MC0 = 1; + TC3->COUNT16.INTENSET.bit.MC1 = 1; + // the rest + // ... + // now enable + while(TC3->COUNT16.SYNCBUSY.bit.ENABLE); + TC3->COUNT16.CTRLA.bit.ENABLE = 1; + // enable the IRQ + NVIC_EnableIRQ(TC3_IRQn); +} + +// need to finish setup above, to do... something, I think compare at duty cycle and wrap at 20ms +// then try write(0) and write(1) at some intervals in loop, +// configure your debugs (currently triggered on pulse_counter events) and watch scope +// then roll PID... tune... and do bus action +// maybe tune after bus action, have pid-config there, not too hard + +void TC3_Handler(void){ + if(TC3->COUNT16.INTFLAG.bit.MC0){ + TC3->COUNT16.INTFLAG.bit.MC0 = 1; + PWM_PIN_HI; + DEBUG1PIN_ON; + // now, running a control loop any faster than on-this-interrupt is silly, + // so it's a handy place to also trigger some code (and is... only 50hz, wowow) + servo_pwm->onPwmTick(); + } + if(TC3->COUNT16.INTFLAG.bit.MC1){ + TC3->COUNT16.INTFLAG.bit.MC1 = 1; + PWM_PIN_LO; + DEBUG1PIN_OFF; + } +} + +void Servo_PWM::setDuty(float duty){ + // 0-1 -> 2000-4000 + if(duty > 1.0F){ + duty = 1.0F; + } else if (duty < 0.0F){ + duty = 0.0F; + } + uint16_t ticks = duty * 2000; + TC3->COUNT16.CCBUF[1].reg = 2000 + ticks; +} \ No newline at end of file diff --git a/firmware/osape-smoothieroll-drop-esc/src/drivers/servo_pwm.h b/firmware/osape-smoothieroll-drop-esc/src/drivers/servo_pwm.h new file mode 100644 index 0000000000000000000000000000000000000000..8e242b137668bd211645c6528cb87738cd0c1245 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/drivers/servo_pwm.h @@ -0,0 +1,33 @@ +/* +drivers/servo_pwm.h + +output servo-type (20ms period, 1-2ms duty cycle) with TC on D51 + +Jake Read at the Center for Bits and Atoms +(c) Massachusetts Institute of Technology 2020 + +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 SERVO_PWM_H_ +#define SERVO_PWM_H_ + +#include <Arduino.h> + +class Servo_PWM { + private: + static Servo_PWM* instance; + public: + Servo_PWM(); + static Servo_PWM* getInstance(void); + void init(void); + void setDuty(float duty); // 0-1 + void onPwmTick(void); +}; + +extern Servo_PWM* servo_pwm; + +#endif \ No newline at end of file diff --git a/firmware/osape-smoothieroll-drop-esc/src/drivers/ucbus_drop.cpp b/firmware/osape-smoothieroll-drop-esc/src/drivers/ucbus_drop.cpp new file mode 100644 index 0000000000000000000000000000000000000000..657919b0a40ca4d5f2ec6a653ff6e94a9b0cf7ae --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/drivers/ucbus_drop.cpp @@ -0,0 +1,281 @@ +/* +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_drop.h" + +UCBus_Drop* UCBus_Drop::instance = 0; + +UCBus_Drop* UCBus_Drop::getInstance(void){ + if(instance == 0){ + instance = new UCBus_Drop(); + } + return instance; +} + +// making this instance globally available, when built, +// recall extern declaration in .h +UCBus_Drop* ucBusDrop = UCBus_Drop::getInstance(); + +UCBus_Drop::UCBus_Drop(void) {} + +// so, first thing, I've some confusion about what might be the best way (again) to implement +// multiple similar drivers. I did this before w/ the cobserialport, I might again want to +// do it, but by writing more static hardware drivers that another parent class (the bus) forwards to +// so the init codes / etc could all be verbatim with hardware registers, instead of this infinite list of defines + +// yeah, this kind of stuff is morning work: focus, tracking little details. go to bed. + +void UCBus_Drop::init(boolean useDipPick, uint8_t ID) { + dip_init(); + if(useDipPick){ + // set our id, + id = dip_read_lower_five(); + } else { + id = ID; + } + if(id > 14){ + id = 14; + } + // set driver output LO to start: tri-state + UBD_DE_PORT.DIRSET.reg = UBD_DE_BM; + UBD_DRIVER_DISABLE; + // set receiver output on, forever: LO to set on + UBD_RE_PORT.DIRSET.reg = UBD_RE_BM; + UBD_RE_PORT.OUTCLR.reg = UBD_RE_BM; + // termination resistor should be set only on one drop, + // or none and physically with a 'tail' cable, or something? + UBD_TE_PORT.DIRSET.reg = UBD_TE_BM; + if(dip_read_pin_1()){ + UBD_TE_PORT.OUTCLR.reg = UBD_TE_BM; + } else { + UBD_TE_PORT.OUTSET.reg = UBD_TE_BM; + } + // rx pin setup + UBD_COMPORT.DIRCLR.reg = UBD_RXBM; + UBD_COMPORT.PINCFG[UBD_RXPIN].bit.PMUXEN = 1; + if(UBD_RXPIN % 2){ + UBD_COMPORT.PMUX[UBD_RXPIN >> 1].reg |= PORT_PMUX_PMUXO(UBD_RXPERIPHERAL); + } else { + UBD_COMPORT.PMUX[UBD_RXPIN >> 1].reg |= PORT_PMUX_PMUXE(UBD_RXPERIPHERAL); + } + // tx + UBD_COMPORT.DIRCLR.reg = UBD_TXBM; + UBD_COMPORT.PINCFG[UBD_TXPIN].bit.PMUXEN = 1; + if(UBD_TXPIN % 2){ + UBD_COMPORT.PMUX[UBD_TXPIN >> 1].reg |= PORT_PMUX_PMUXO(UBD_TXPERIPHERAL); + } else { + UBD_COMPORT.PMUX[UBD_TXPIN >> 1].reg |= PORT_PMUX_PMUXE(UBD_TXPERIPHERAL); + } + // ok, clocks, first line au manuel + // unmask clocks + MCLK->APBAMASK.bit.SERCOM1_ = 1; + GCLK->GENCTRL[UBD_GCLKNUM_PICK].reg = GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL) | GCLK_GENCTRL_GENEN; + while(GCLK->SYNCBUSY.reg & GCLK_SYNCBUSY_GENCTRL(UBD_GCLKNUM_PICK)); + GCLK->PCHCTRL[UBD_SERCOM_CLK].reg = GCLK_PCHCTRL_CHEN | GCLK_PCHCTRL_GEN(UBD_GCLKNUM_PICK); + // then, sercom + while(UBD_SER_USART.SYNCBUSY.bit.ENABLE); + UBD_SER_USART.CTRLA.bit.ENABLE = 0; + while(UBD_SER_USART.SYNCBUSY.bit.SWRST); + UBD_SER_USART.CTRLA.bit.SWRST = 1; + while(UBD_SER_USART.SYNCBUSY.bit.SWRST); + while(UBD_SER_USART.SYNCBUSY.bit.SWRST || UBD_SER_USART.SYNCBUSY.bit.ENABLE); + // ok, well + UBD_SER_USART.CTRLA.reg = SERCOM_USART_CTRLA_MODE(1) | SERCOM_USART_CTRLA_DORD; + UBD_SER_USART.CTRLA.reg |= SERCOM_USART_CTRLA_RXPO(UBD_RXPO) | SERCOM_USART_CTRLA_TXPO(0); + UBD_SER_USART.CTRLA.reg |= SERCOM_USART_CTRLA_FORM(1); // enable even parity + while(UBD_SER_USART.SYNCBUSY.bit.CTRLB); + UBD_SER_USART.CTRLB.reg = SERCOM_USART_CTRLB_RXEN | SERCOM_USART_CTRLB_TXEN | SERCOM_USART_CTRLB_CHSIZE(0); + // enable interrupts + NVIC_EnableIRQ(SERCOM1_2_IRQn); // rx, + NVIC_EnableIRQ(SERCOM1_1_IRQn); // txc ? + NVIC_EnableIRQ(SERCOM1_0_IRQn); // tx, + // set baud + UBD_SER_USART.BAUD.reg = UBD_BAUD_VAL; + // and finally, a kickoff + while(UBD_SER_USART.SYNCBUSY.bit.ENABLE); + UBD_SER_USART.CTRLA.bit.ENABLE = 1; + // enable rx interrupt + UBD_SER_USART.INTENSET.bit.RXC = 1; +} + +void SERCOM1_2_Handler(void){ + //ERRLIGHT_TOGGLE; + ucBusDrop->rxISR(); +} + +void UCBus_Drop::rxISR(void){ + // check parity bit, + uint16_t perr = UBD_SER_USART.STATUS.bit.PERR; + if(perr){ + //DEBUGPIN_ON; + uint8_t clear = UBD_SER_USART.DATA.reg; + UBD_SER_USART.STATUS.bit.PERR = 1; // clear parity error + return; + } + // cleared by reading out, but we are blind feed forward atm + uint8_t data = UBD_SER_USART.DATA.reg; + // for now, just running the clk, on every 0th bit + if((data >> 7) == 0){ // timer bit, on every 0th bit, and beginning of word + inWord[0] = data; + timeTick ++; + timeBlink ++; + if(timeBlink >= blinkTime){ + CLKLIGHT_TOGGLE; + timeBlink = 0; + } + onRxISR(); // on start of each word + } else { // end of word on every 0th bit + inWord[1] = data; + // now decouple, + inHeader = ((inWord[0] >> 1) & 0b00111000) | ((inWord[1] >> 4) & 0b00000111); + inByte = ((inWord[0] << 4) & 0b11110000) | (inWord[1] & 0b00001111); + // now check incoming data, + if((inHeader & 0b00110000) == 0b00100000){ // has-token, CHA + lastWordAHadToken = true; + if(inBufferALen != 0){ // in this case, we didn't read-out of the buffer in time, + inBufferALen = 0; // so we reset it, missing the last packet ! + inBufferARp = 0; + } + inBufferA[inBufferARp] = inByte; + inBufferARp ++; + } else if ((inHeader & 0b00110000) == 0b00000000) { // no-token, CHA + if(lastWordAHadToken){ + inBufferALen = inBufferARp - 1; + onPacketARx(); + } + lastWordAHadToken = false; + } else if ((inHeader & 0b00110000) == 0b00110000) { // has-token, CHB + //DEBUG1PIN_ON; + lastWordBHadToken = true; + if(inBufferBLen != 0){ + inBufferBLen = 0; + inBufferBRp = 0; + } + inBufferB[inBufferBRp] = inByte; + inBufferBRp ++; + } else if ((inHeader & 0b00110000) == 0b00010000) { // no-token, CHB + if(lastWordBHadToken){ + inBufferBLen = inBufferARp - 1; + //onPacketBRx(); // b-channel handled in loop, yah + } + lastWordBHadToken = false; + } + // now check if outgoing tick is ours, + if((inHeader & dropIdMask) == id){ + // our transmit + if(outBufferLen > 0){ + outByte = outBuffer[outBufferRp]; + outHeader = headerMask & (tokenWord | (id & 0b00011111)); + } else { + outByte = 0; + outHeader = headerMask & (noTokenWord | (id & 0b00011111)); + } + outWord[0] = 0b00000000 | ((outHeader << 1) & 0b01110000) | (outByte >> 4); + outWord[1] = 0b10000000 | ((outHeader << 4) & 0b01110000) | (outByte & 0b00001111); + // now transmit, + UBD_DRIVER_ENABLE; + UBD_SER_USART.DATA.reg = outWord[0]; + UBD_SER_USART.INTENSET.bit.DRE = 1; + // now do this, + if(outBufferLen > 0){ + outBufferRp ++; + if(outBufferRp >= outBufferLen){ + outBufferRp = 0; + outBufferLen = 0; + } + } + } else if ((inHeader & dropIdMask) == UBD_CLOCKRESET){ + // reset + timeTick = 0; + } + } // end 1th bit case, + // do every-tick stuff +} + +void SERCOM1_0_Handler(void){ + ucBusDrop->dreISR(); +} + +void UCBus_Drop::dreISR(void){ + UBD_SER_USART.DATA.reg = outWord[1]; // tx the next word, + UBD_SER_USART.INTENCLR.reg = SERCOM_USART_INTENCLR_DRE; // clear this interrupt + UBD_SER_USART.INTENSET.reg = SERCOM_USART_INTENSET_TXC; // now watch transmit complete +} + +void SERCOM1_1_Handler(void){ + ucBusDrop->txcISR(); +} + +void UCBus_Drop::txcISR(void){ + UBD_SER_USART.INTFLAG.bit.TXC = 1; // clear the flag by writing 1 + UBD_SER_USART.INTENCLR.reg = SERCOM_USART_INTENCLR_TXC; // turn off the interrupt + UBD_DRIVER_DISABLE; // turn off the driver, + //DEBUGPIN_TOGGLE; +} + +// -------------------------------------------------------- ASYNC API + +boolean UCBus_Drop::ctr_a(void){ + if(inBufferALen > 0){ + return true; + } else { + return false; + } +} + +boolean UCBus_Drop::ctr_b(void){ + if(inBufferBLen > 0){ + return true; + } else { + return false; + } +} + +size_t UCBus_Drop::read_a(uint8_t *dest){ + if(!ctr_a()) return 0; + NVIC_DisableIRQ(SERCOM1_2_IRQn); + // copy out, irq disabled, TODO safety on missing an interrupt in this case?? clock jitter? + memcpy(dest, inBufferA, inBufferALen); + size_t len = inBufferALen; + inBufferALen = 0; + inBufferARp = 0; + NVIC_EnableIRQ(SERCOM1_2_IRQn); + return len; +} + +size_t UCBus_Drop::read_b(uint8_t *dest){ + if(!ctr_b()) return 0; + NVIC_DisableIRQ(SERCOM1_2_IRQn); + memcpy(dest, inBufferB, inBufferBLen); + size_t len = inBufferBLen; + inBufferBLen = 0; + inBufferBRp = 0; + NVIC_EnableIRQ(SERCOM1_2_IRQn); + return len; +} + +boolean UCBus_Drop::cts(void){ + if(outBufferLen > 0){ + return false; + } else { + return true; + } +} + +void UCBus_Drop::transmit(uint8_t *data, uint16_t len){ + if(!cts()) return; + size_t encLen = cobsEncode(data, len, outBuffer); + outBufferLen = encLen; + outBufferRp = 0; +} \ No newline at end of file diff --git a/firmware/osape-smoothieroll-drop-esc/src/drivers/ucbus_drop.h b/firmware/osape-smoothieroll-drop-esc/src/drivers/ucbus_drop.h new file mode 100644 index 0000000000000000000000000000000000000000..de1d55eae2f771880b5f140450cf6033fa0d001c --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/drivers/ucbus_drop.h @@ -0,0 +1,123 @@ +/* +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_DROP_H_ +#define UCBUS_DROP_H_ + +#include <arduino.h> + +#include "indicators.h" +#include "dip_ucbus_config.h" +#include "peripheral_nums.h" +#include "utils/syserror.h" +#include "utils/cobs.h" + +#define UBD_SER_USART SERCOM1->USART +#define UBD_SERCOM_CLK SERCOM1_GCLK_ID_CORE +#define UBD_GCLKNUM_PICK 7 +#define UBD_COMPORT PORT->Group[0] +#define UBD_TXPIN 16 // x-0 +#define UBD_TXBM (uint32_t)(1 << UBD_TXPIN) +#define UBD_RXPIN 18 // x-2 +#define UBD_RXBM (uint32_t)(1 << UBD_RXPIN) +#define UBD_RXPO 2 +#define UBD_TXPERIPHERAL PERIPHERAL_C +#define UBD_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 UBD_BAUD_VAL 0 + +#define UBD_DE_PIN 16 // driver output enable: set HI to enable, LO to tri-state the driver +#define UBD_DE_BM (uint32_t)(1 << UBD_DE_PIN) +#define UBD_DE_PORT PORT->Group[1] +#define UBD_DRIVER_ENABLE UBD_DE_PORT.OUTSET.reg = UBD_DE_BM +#define UBD_DRIVER_DISABLE UBD_DE_PORT.OUTCLR.reg = UBD_DE_BM +#define UBD_RE_PIN 19 // receiver output enable, set LO to enable the RO, set HI to tri-state RO +#define UBD_RE_BM (uint32_t)(1 << UBD_RE_PIN) +#define UBD_RE_PORT PORT->Group[0] +#define UBD_TE_PIN 17 // termination enable, drive LO to enable to internal termination resistor, HI to disable +#define UBD_TE_BM (uint32_t)(1 << UBD_TE_PIN) +#define UBD_TE_PORT PORT->Group[0] + +#define UBD_BUFSIZE 1024 + +#define UBD_CLOCKRESET 15 + +#define UB_AK_GOTOPOS 91 +#define UB_AK_SETPOS 92 +#define UB_AK_SETRPM 93 + +class UCBus_Drop { + private: + // singleton-ness + static UCBus_Drop* instance; + // timing, + volatile uint64_t timeBlink = 0; + uint16_t blinkTime = 10000; + // input buffer & word + volatile uint8_t inWord[2]; + volatile uint8_t inHeader; + volatile uint8_t inByte; + volatile boolean lastWordAHadToken = false; + uint8_t inBufferA[UBD_BUFSIZE]; + volatile uint16_t inBufferARp = 0; + volatile uint16_t inBufferALen = 0; // writes at terminal zero, + volatile boolean lastWordBHadToken = false; + uint8_t inBufferB[UBD_BUFSIZE]; + volatile uint16_t inBufferBRp = 0; + volatile uint16_t inBufferBLen = 0; + // output, + volatile uint8_t outWord[2]; + volatile uint8_t outHeader; + volatile uint8_t outByte; + uint8_t outBuffer[UBD_BUFSIZE]; + volatile uint16_t outBufferRp = 0; + volatile uint16_t outBufferLen = 0; + const uint8_t headerMask = 0b00111111; + const uint8_t dropIdMask = 0b00001111; + const uint8_t tokenWord = 0b00100000; + const uint8_t noTokenWord = 0b00000000; + public: + UCBus_Drop(); + static UCBus_Drop* getInstance(void); + // available time count, + volatile uint16_t timeTick = 0; + // isrs + void rxISR(void); + void dreISR(void); + void txcISR(void); + // handlers + void onRxISR(void); + void onPacketARx(void); + //void onPacketBRx(void); + // our physical bus address, + volatile uint8_t id = 0; + // the api, eh + void init(boolean useDipPick, uint8_t ID); + boolean ctr_a(void); // return true if RX complete / buffer ready + boolean ctr_b(void); + size_t read_a(uint8_t *dest); // ship les bytos + size_t read_b(uint8_t *dest); + boolean cts(void); // true if tx buffer empty, + void transmit(uint8_t *data, uint16_t len); +}; + +extern UCBus_Drop* ucBusDrop; + +#endif \ No newline at end of file diff --git a/firmware/osape-smoothieroll-drop-esc/src/main.cpp b/firmware/osape-smoothieroll-drop-esc/src/main.cpp new file mode 100644 index 0000000000000000000000000000000000000000..90ae351c2c11cf270cb823616192892bea264cf3 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/main.cpp @@ -0,0 +1,140 @@ +#include <Arduino.h> + +#include "drivers/indicators.h" +#include "utils/cobs.h" +#include "osap/osap.h" + +OSAP* osap = new OSAP("spindle esc drop"); + +#include "drivers/ucbus_drop.h" + +#include "utils/clocks_d51_module.h" + +#include "drivers/servo_pwm.h" + +union chunk_float32 { + uint8_t bytes[4]; + float f; +}; + +union chunk_float64 { + uint8_t bytes[8]; + double f; +}; + +#define BUS_DROP 0 + +void setup() { + ERRLIGHT_SETUP; + CLKLIGHT_SETUP; + DEBUG1PIN_SETUP; + DEBUG2PIN_SETUP; + // osap + osap->description = "remote esc drop"; + // bus + ucBusDrop->init(false, BUS_DROP); + // pwm out + servo_pwm->init(); + servo_pwm->setDuty(0.0F); +} + +uint8_t bChPck[64]; +uint16_t lpCnt = 0; + +// 0.1 -> nok RPM +// 0.15 -> 2.5k RPM ~ unstable stops +// 0.2 -> 6k RPM +// 0.3 -> 12k RPM 6k comfortable, low harmonic +// 0.4 -> 18k RPM 6k p loud +// 0.5 -> 23k RPM 5k loud AF +// 0.6 -> don't do this + +void loop() { + /* + if(millis() > 5000 && run_pwm){ + servo_pwm->setDuty(0.3F); + run_pwm = false; + } + */ + osap->loop(); + // do packet B grab / handle + if(ucBusDrop->ctr_b()){ + uint16_t len = ucBusDrop->read_b(bChPck); + uint16_t ptr = 0; + switch(bChPck[ptr]){ + case AK_SETCURRENT: + break; + case AK_SETPOS: + break; + case AK_SETRPM: { + ptr ++; + chunk_float32 rpm_c; // this is actually just the 0-1 value, direct, the mapping from RPM -> PWM is done in JS + rpm_c.bytes[0] = bChPck[ptr ++]; + rpm_c.bytes[1] = bChPck[ptr ++]; + rpm_c.bytes[2] = bChPck[ptr ++]; + rpm_c.bytes[3] = bChPck[ptr ++]; + servo_pwm->setDuty(rpm_c.f); + break; + } + default: + // noop, + break; + } + } + // do periodic hello-to-above + /* + lpCnt ++; + if(lpCnt > 10000){ + lpCnt = 0; + float widths = pulse_counter->getAverageWidth(); + float tps = pulse_counter->getTicksPerSecond(); + // capture on rising edges, 14 poles, 7 edges per revolution, + float rpm = (tps * 60) / (widths * 7 + 1); + sysError("RPM: " + String(rpm, 6)); + } + */ +} // end loop + +// the following is leftover from an attempt to speed control this thing, might revisit... + +volatile float rpm_target = 10000.0F; +volatile float p_term = -0.002F; +volatile float pid_out = 0.0F; + +volatile float err = 0.0F; +volatile float widths = 0.0F; +volatile float tps = 0.0F; +volatile float rpm = 0.0F; + +void Servo_PWM::onPwmTick(void){ + /* + //DEBUG2PIN_ON; + widths = pulse_counter->getAverageWidth(); + // avoid div / low + if(widths < 100) widths = 100; + tps = pulse_counter->getTicksPerSecond(); + rpm = (tps * 60) / (widths * 7); + // presuming full width is 0-60k rpm, and outputs 0-1, + // I want to scale RPM err into -1 -> 1 domain, so rpm from 0-1 + // this should help (?) with numerical stability, not multiplying 60k w/ 0.00002 p term + rpm = rpm / 60000; + err = rpm - (rpm_target / 60000); // err could span -60k -> 60k + pid_out = err * p_term; + // no small outputs, + if(pid_out < 0.2F) pid_out = 0.2F; + if(run_pwm) servo_pwm->setDuty(pid_out); + //DEBUG2PIN_OFF; + */ +} + +void UCBus_Drop::onRxISR(void){ + // 100kHz (?) or is it 50? +} + +void UCBus_Drop::onPacketARx(void){ + // not really for us, +} + +void OSAP::handleAppPacket(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t segsize, VPort* vp, uint16_t vpi, uint8_t pwp){ + vp->clearPacket(pwp); +} diff --git a/firmware/osape-smoothieroll-drop-esc/src/osap/osap.cpp b/firmware/osape-smoothieroll-drop-esc/src/osap/osap.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4b4499c5c2cd3fbd93ca95f0f43608b0f613aa2c --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/osap/osap.cpp @@ -0,0 +1,487 @@ +/* +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-drop-esc/src/osap/osap.h b/firmware/osape-smoothieroll-drop-esc/src/osap/osap.h new file mode 100644 index 0000000000000000000000000000000000000000..5242ba0ae6cdc77538ef57f119a6691fb46188fa --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/osap/osap.h @@ -0,0 +1,60 @@ +/* +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-drop-esc/src/osap/ts.cpp b/firmware/osape-smoothieroll-drop-esc/src/osap/ts.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1a399002651a3f22be9cb8c51e373af13d25163d --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/osap/ts.cpp @@ -0,0 +1,64 @@ +/* +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]; +} + +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-drop-esc/src/osap/ts.h b/firmware/osape-smoothieroll-drop-esc/src/osap/ts.h new file mode 100644 index 0000000000000000000000000000000000000000..1029171322097efa534c83d8db9c2ee835024ffc --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/osap/ts.h @@ -0,0 +1,100 @@ +/* +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_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_writeString(String val, unsigned char *buf, uint16_t *ptr); diff --git a/firmware/osape-smoothieroll-drop-esc/src/osap/vport.cpp b/firmware/osape-smoothieroll-drop-esc/src/osap/vport.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e064991454d718657dbc30027b4d6b0f9c8b7d8f --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/osap/vport.cpp @@ -0,0 +1,40 @@ +/* +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-drop-esc/src/osap/vport.h b/firmware/osape-smoothieroll-drop-esc/src/osap/vport.h new file mode 100644 index 0000000000000000000000000000000000000000..b705e91a150ff544b2efa0ceba17f4f6b3b515eb --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/osap/vport.h @@ -0,0 +1,52 @@ +/* +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-drop-esc/src/osap/vport_usbserial.cpp b/firmware/osape-smoothieroll-drop-esc/src/osap/vport_usbserial.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ddbc9fd308439d946194f2a75364b4613bb1f2a4 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/osap/vport_usbserial.cpp @@ -0,0 +1,141 @@ +/* +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-drop-esc/src/osap/vport_usbserial.h b/firmware/osape-smoothieroll-drop-esc/src/osap/vport_usbserial.h new file mode 100644 index 0000000000000000000000000000000000000000..4a72d1ce9462ca8bf4ee76992d56c62a8a54a0cf --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/osap/vport_usbserial.h @@ -0,0 +1,57 @@ +/* +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-drop-esc/src/utils/clocks_d51_module.cpp b/firmware/osape-smoothieroll-drop-esc/src/utils/clocks_d51_module.cpp new file mode 100644 index 0000000000000000000000000000000000000000..317c40390d35e1e947eab6ff27c05cb466124db8 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/utils/clocks_d51_module.cpp @@ -0,0 +1,115 @@ +/* +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-drop-esc/src/utils/clocks_d51_module.h b/firmware/osape-smoothieroll-drop-esc/src/utils/clocks_d51_module.h new file mode 100644 index 0000000000000000000000000000000000000000..084141e09bd4ca38bc56eb3505c31075358c8fb5 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/utils/clocks_d51_module.h @@ -0,0 +1,43 @@ +/* +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-drop-esc/src/utils/cobs.cpp b/firmware/osape-smoothieroll-drop-esc/src/utils/cobs.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a2d73787ba6f2626405e92aa1f72dc6da8ea43d5 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/utils/cobs.cpp @@ -0,0 +1,60 @@ +/* +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-drop-esc/src/utils/cobs.h b/firmware/osape-smoothieroll-drop-esc/src/utils/cobs.h new file mode 100644 index 0000000000000000000000000000000000000000..a9e587463dc4cfc6f8175d0ff48b53174970133c --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/utils/cobs.h @@ -0,0 +1,24 @@ +/* +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-drop-esc/src/utils/syserror.cpp b/firmware/osape-smoothieroll-drop-esc/src/utils/syserror.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a2473fe296a6744d233b39d217bfef523aa6f776 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/utils/syserror.cpp @@ -0,0 +1,38 @@ +#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-drop-esc/src/utils/syserror.h b/firmware/osape-smoothieroll-drop-esc/src/utils/syserror.h new file mode 100644 index 0000000000000000000000000000000000000000..b421cc2a08eb1d482a9f298ed9dbefaa90531f9d --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/src/utils/syserror.h @@ -0,0 +1,11 @@ +#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-drop-esc/test/README b/firmware/osape-smoothieroll-drop-esc/test/README new file mode 100644 index 0000000000000000000000000000000000000000..b94d0890faa00a63737892509a5ca77ad3bdc6c3 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-esc/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Unit Testing and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/page/plus/unit-testing.html diff --git a/firmware/osape-smoothieroll-drop-stepper/.gitignore b/firmware/osape-smoothieroll-drop-stepper/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..89cc49cbd652508924b868ea609fa8f6b758ec56 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/firmware/osape-smoothieroll-drop-stepper/.travis.yml b/firmware/osape-smoothieroll-drop-stepper/.travis.yml new file mode 100644 index 0000000000000000000000000000000000000000..7c486f183c3cefc46ffae9d5768b04f61ab068b7 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/.travis.yml @@ -0,0 +1,67 @@ +# 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-drop-stepper/.vscode/extensions.json b/firmware/osape-smoothieroll-drop-stepper/.vscode/extensions.json new file mode 100644 index 0000000000000000000000000000000000000000..e80666bfb11e75aafd3600701dc99c10acce341c --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ] +} diff --git a/firmware/osape-smoothieroll-drop-stepper/include/README b/firmware/osape-smoothieroll-drop-stepper/include/README new file mode 100644 index 0000000000000000000000000000000000000000..194dcd43252dcbeb2044ee38510415041a0e7b47 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/firmware/osape-smoothieroll-drop-stepper/lib/README b/firmware/osape-smoothieroll-drop-stepper/lib/README new file mode 100644 index 0000000000000000000000000000000000000000..6debab1e8b4c3faa0d06f4ff44bce343ce2cdcbf --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include <Foo.h> +#include <Bar.h> + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/firmware/osape-smoothieroll-drop-stepper/platformio.ini b/firmware/osape-smoothieroll-drop-stepper/platformio.ini new file mode 100644 index 0000000000000000000000000000000000000000..50208f0cadb2e924ce3a661e45b5c88422dc71af --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/platformio.ini @@ -0,0 +1,14 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:adafruit_feather_m4] +platform = atmelsam +board = adafruit_feather_m4 +framework = arduino diff --git a/firmware/osape-smoothieroll-drop-stepper/src/drivers/dacs.cpp b/firmware/osape-smoothieroll-drop-stepper/src/drivers/dacs.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b9dbf277cb341d4d1c36240456da9fd240eaa5c1 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/drivers/dacs.cpp @@ -0,0 +1,122 @@ +/* +osap/drivers/dacs.cpp + +dacs on the d51 + +Jake Read at the Center for Bits and Atoms +(c) Massachusetts Institute of Technology 2019 + +This work may be reproduced, modified, distributed, performed, and +displayed for any purpose, but must acknowledge the squidworks and ponyo +projects. Copyright is retained and must be preserved. The work is provided as +is; no warranty is provided, and users accept all liability. +*/ + +#include "dacs.h" +//#include "ucbus_drop.h" + +DACs* DACs::instance = 0; + +DACs* DACs::getInstance(void){ + if(instance == 0){ + instance = new DACs(); + } + return instance; +} + +DACs* dacs = DACs::getInstance(); + +DACs::DACs() {} + +void DACs::init(){ + /* + // the below code was an attempt to scrape from + // scrape https://github.com/adafruit/ArduinoCore-samd/blob/master/cores/arduino/startup.c (clock) + // scrape https://github.com/adafruit/ArduinoCore-samd/blob/master/cores/arduino/wiring.c (peripheral clock) + // scrape https://github.com/adafruit/ArduinoCore-samd/blob/master/cores/arduino/wiring_analog.c + // to setup the DAC 'from scratch' - of course it occurred to me later that this + // setup already happens in arduino's boot. so I omitted this and just used + // the messy per-analogWrite-call config below, and wrote small write-to-dac functions + // to operate under the assumption that this init happens once. + + // ... + // put the pins on the peripheral, + // DAC0 is PA02, Peripheral B + // DAC1 is PA05, Peripheral B + //PORT->Group[0].DIRSET.reg = (uint32_t)(1 << 2); + //PORT->Group[0].DIRCLR.reg = (uint32_t)(1 << 2); + PORT->Group[0].PINCFG[2].bit.PMUXEN = 1; + PORT->Group[0].PMUX[2 >> 1].reg |= PORT_PMUX_PMUXE(1); + //PORT->Group[0].DIRSET.reg = (uint32_t)(1 << 5); + //PORT->Group[0].DIRCLR.reg = (uint32_t)(1 << 5); + PORT->Group[0].PINCFG[5].bit.PMUXEN = 1; + PORT->Group[0].PMUX[5 >> 1].reg |= PORT_PMUX_PMUXO(1); + + // unmask the DAC peripheral + MCLK->APBDMASK.bit.DAC_ = 1; + + // DAC needs a clock, + GCLK->GENCTRL[GENERIC_CLOCK_GENERATOR_12M].reg = GCLK_GENCTRL_SRC_DFLL | + GCLK_GENCTRL_IDC | + GCLK_GENCTRL_DIV(4) | + GCLK_GENCTRL_GENEN; + while(GCLK->SYNCBUSY.reg & GENERIC_CLOCK_GENERATOR_12M_SYNC); + // feed that clock to the DAC, + GCLK->PCHCTRL[DAC_GCLK_ID].reg = GCLK_PCHCTRL_CHEN | GCLK_PCHCTRL_GEN(GENERIC_CLOCK_GENERATOR_12M_SYNC); + while(GCLK->PCHCTRL[DAC_GCLK_ID].bit.CHEN == 0); + + // software reset the DAC + while(DAC->SYNCBUSY.bit.SWRST == 1); + DAC->CTRLA.bit.SWRST = 1; + while(DAC->SYNCBUSY.bit.SWRST == 1); + // and finally the DAC itself, + while(DAC->SYNCBUSY.bit.ENABLE || DAC->SYNCBUSY.bit.SWRST); + DAC->CTRLA.bit.ENABLE = 0; + // enable both channels + while(DAC->SYNCBUSY.bit.ENABLE || DAC->SYNCBUSY.bit.SWRST); + DAC->DACCTRL[0].reg = DAC_DACCTRL_ENABLE | DAC_DACCTRL_REFRESH(2); + while(DAC->SYNCBUSY.bit.ENABLE || DAC->SYNCBUSY.bit.SWRST); + DAC->DACCTRL[1].reg = DAC_DACCTRL_ENABLE | DAC_DACCTRL_REFRESH(2); + // voltage out, and select vref + DAC->CTRLB.reg = DAC_CTRLB_REFSEL_VDDANA; + // re-enable dac + while(DAC->SYNCBUSY.bit.ENABLE || DAC->SYNCBUSY.bit.SWRST); + DAC->CTRLA.bit.ENABLE = 1; + // await up, + while(!DAC->STATUS.bit.READY0); + while(!DAC->STATUS.bit.READY1); + */ + while(DAC->SYNCBUSY.bit.ENABLE || DAC->SYNCBUSY.bit.SWRST); + DAC->CTRLA.bit.ENABLE = 0; + while(DAC->SYNCBUSY.bit.ENABLE || DAC->SYNCBUSY.bit.SWRST); + DAC->DACCTRL[0].bit.ENABLE = 1; + DAC->DACCTRL[1].bit.ENABLE = 1; + while(DAC->SYNCBUSY.bit.ENABLE || DAC->SYNCBUSY.bit.SWRST); + DAC->CTRLA.bit.ENABLE = 1; + while(!DAC->STATUS.bit.READY0); + while(!DAC->STATUS.bit.READY1); +} + +// 0 - 4095 +void DACs::writeDac0(uint16_t val){ + //analogWrite(A0, val); + while(DAC->SYNCBUSY.bit.DATA0); + DAC->DATA[0].reg = val;//DAC_DATA_DATA(val); + currentVal0 = val; +} + +void DACs::writeDac1(uint16_t val){ + //analogWrite(A1, val); + while(DAC->SYNCBUSY.bit.DATA1); + DAC->DATA[1].reg = val;//DAC_DATA_DATA(val); + currentVal1 = val; +} + +void DACs::refresh(void){ + writeDac0(currentVal0); + writeDac1(currentVal1); + uint32_t now = micros(); + if(now > lastRefresh + 1000){ + lastRefresh = now; + } +} diff --git a/firmware/osape-smoothieroll-drop-stepper/src/drivers/dacs.h b/firmware/osape-smoothieroll-drop-stepper/src/drivers/dacs.h new file mode 100644 index 0000000000000000000000000000000000000000..6d095390582c08027f0bd3777136fed0e05edbf4 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/drivers/dacs.h @@ -0,0 +1,55 @@ +/* +osap/drivers/dacs.h + +dacs on the d51 + +Jake Read at the Center for Bits and Atoms +(c) Massachusetts Institute of Technology 2019 + +This work may be reproduced, modified, distributed, performed, and +displayed for any purpose, but must acknowledge the squidworks and ponyo +projects. Copyright is retained and must be preserved. The work is provided as +is; no warranty is provided, and users accept all liability. +*/ + +#ifndef DACS_H_ +#define DACS_H_ + +#include <arduino.h> + +#include "indicators.h" +#include "utils/syserror.h" + +// scrape https://github.com/adafruit/ArduinoCore-samd/blob/master/cores/arduino/wiring_analog.c +// scrape https://github.com/adafruit/ArduinoCore-samd/blob/master/cores/arduino/startup.c (clock) +// scrape https://github.com/adafruit/ArduinoCore-samd/blob/master/cores/arduino/wiring.c (peripheral clock) +// DAC0 is on PA02 +// DAC1 is on PA05 + +// NOTE: the DAC must be refreshed manually to maintain voltage. +// there does appear to be a refresh register in DACCTRL band, +// but it does *not* seem to work... + +#define GENERIC_CLOCK_GENERATOR_12M (4u) +#define GENERIC_CLOCK_GENERATOR_12M_SYNC GCLK_SYNCBUSY_GENCTRL4 + +class DACs { + private: + // is driver, is singleton, + static DACs* instance; + volatile uint16_t currentVal0 = 0; + volatile uint16_t currentVal1 = 0; + volatile uint32_t lastRefresh = 0; + + public: + DACs(); + static DACs* getInstance(void); + void init(void); + void writeDac0(uint16_t val); + void writeDac1(uint16_t val); + void refresh(void); +}; + +extern DACs* dacs; + +#endif \ No newline at end of file diff --git a/firmware/osape-smoothieroll-drop-stepper/src/drivers/dip_ucbus_config.cpp b/firmware/osape-smoothieroll-drop-stepper/src/drivers/dip_ucbus_config.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e0316b389b72c91a640a75883c4f426b9e5c07cf --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/drivers/dip_ucbus_config.cpp @@ -0,0 +1,55 @@ +// DIPs + +#include "dip_ucbus_config.h" + +void dip_init(void){ + // set direction in, + DIP_PORT.DIRCLR.reg = D_BM(D0_PIN) | D_BM(D1_PIN) | D_BM(D2_PIN) | D_BM(D3_PIN) | D_BM(D4_PIN) | D_BM(D5_PIN) | D_BM(D6_PIN) | D_BM(D7_PIN); + // enable in, + DIP_PORT.PINCFG[D0_PIN].bit.INEN = 1; + DIP_PORT.PINCFG[D1_PIN].bit.INEN = 1; + DIP_PORT.PINCFG[D2_PIN].bit.INEN = 1; + DIP_PORT.PINCFG[D3_PIN].bit.INEN = 1; + DIP_PORT.PINCFG[D4_PIN].bit.INEN = 1; + DIP_PORT.PINCFG[D5_PIN].bit.INEN = 1; + DIP_PORT.PINCFG[D6_PIN].bit.INEN = 1; + DIP_PORT.PINCFG[D7_PIN].bit.INEN = 1; + // enable pull, + DIP_PORT.PINCFG[D0_PIN].bit.PULLEN = 1; + DIP_PORT.PINCFG[D1_PIN].bit.PULLEN = 1; + DIP_PORT.PINCFG[D2_PIN].bit.PULLEN = 1; + DIP_PORT.PINCFG[D3_PIN].bit.PULLEN = 1; + DIP_PORT.PINCFG[D4_PIN].bit.PULLEN = 1; + DIP_PORT.PINCFG[D5_PIN].bit.PULLEN = 1; + DIP_PORT.PINCFG[D6_PIN].bit.PULLEN = 1; + DIP_PORT.PINCFG[D7_PIN].bit.PULLEN = 1; + // 'pull' references the value set in the 'out' register, so to pulldown: + DIP_PORT.OUTCLR.reg = D_BM(D0_PIN) | D_BM(D1_PIN) | D_BM(D2_PIN) | D_BM(D3_PIN) | D_BM(D4_PIN) | D_BM(D5_PIN) | D_BM(D6_PIN) | D_BM(D7_PIN); +} + +uint8_t dip_read_lower_five(void){ + uint32_t bits[5] = {0,0,0,0,0}; + if(DIP_PORT.IN.reg & D_BM(D7_PIN)) { bits[0] = 1; } + if(DIP_PORT.IN.reg & D_BM(D6_PIN)) { bits[1] = 1; } + if(DIP_PORT.IN.reg & D_BM(D5_PIN)) { bits[2] = 1; } + if(DIP_PORT.IN.reg & D_BM(D4_PIN)) { bits[3] = 1; } + if(DIP_PORT.IN.reg & D_BM(D3_PIN)) { bits[4] = 1; } + /* + bits[0] = (DIP_PORT.IN.reg & D_BM(D7_PIN)) >> D7_PIN; + bits[1] = (DIP_PORT.IN.reg & D_BM(D6_PIN)) >> D6_PIN; + bits[2] = (DIP_PORT.IN.reg & D_BM(D5_PIN)) >> D5_PIN; + bits[3] = (DIP_PORT.IN.reg & D_BM(D4_PIN)) >> D4_PIN; + bits[4] = (DIP_PORT.IN.reg & D_BM(D3_PIN)) >> D3_PIN; + */ + uint32_t word = 0; + word = word | (bits[4] << 4) | (bits[3] << 3) | (bits[2] << 2) | (bits[1] << 1) | (bits[0] << 0); + return (uint8_t)word; +} + +boolean dip_read_pin_0(void){ + return DIP_PORT.IN.reg & D_BM(D0_PIN); +} + +boolean dip_read_pin_1(void){ + return DIP_PORT.IN.reg & D_BM(D1_PIN); +} \ No newline at end of file diff --git a/firmware/osape-smoothieroll-drop-stepper/src/drivers/dip_ucbus_config.h b/firmware/osape-smoothieroll-drop-stepper/src/drivers/dip_ucbus_config.h new file mode 100644 index 0000000000000000000000000000000000000000..44eaa00d1fb1fd0f30118308ba436cd136cc344a --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/drivers/dip_ucbus_config.h @@ -0,0 +1,24 @@ +// DIP switch HAL macros +// pardon the mis-labeling: on board, and in the schem, these are 1-8, +// here they will be 0-7 + +// note: these are 'on' hi by default, from the factory. +// to set low, need to turn the internal pulldown on + +#include <Arduino.h> + +#define D0_PIN 5 +#define D1_PIN 4 +#define D2_PIN 3 +#define D3_PIN 2 +#define D4_PIN 1 +#define D5_PIN 0 +#define D6_PIN 31 +#define D7_PIN 30 +#define DIP_PORT PORT->Group[1] +#define D_BM(val) ((uint32_t)(1 << val)) + +void dip_init(void); +uint8_t dip_read_lower_five(void); +boolean dip_read_pin_0(void); // bus-head (hi) or bus-drop (lo) +boolean dip_read_pin_1(void); // if bus-drop, te-enable (hi) or no (lo) \ No newline at end of file diff --git a/firmware/osape-smoothieroll-drop-stepper/src/drivers/indicators.h b/firmware/osape-smoothieroll-drop-stepper/src/drivers/indicators.h new file mode 100644 index 0000000000000000000000000000000000000000..2dcb3ac7823931c3a60fdd534ab9cf10f10f176a --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/drivers/indicators.h @@ -0,0 +1,52 @@ +// for the new one! with the DIP switch! +#define CLKLIGHT_PIN 27 +#define CLKLIGHT_PORT PORT->Group[0] +#define ERRLIGHT_PIN 8 +#define ERRLIGHT_PORT PORT->Group[1] + +#define DEBUG1PIN_PIN 13 +#define DEBUG1PIN_PORT PORT->Group[1] +#define DEBUG2PIN_PIN 12 +#define DEBUG2PIN_PORT PORT->Group[1] +#define DEBUG3PIN_PIN 13 +#define DEBUG3PIN_PORT PORT->Group[1] +#define DEBUG4PIN_PIN 14 +#define DEBUG4PIN_PORT PORT->Group[1] + +// PA27 +#define CLKLIGHT_BM (uint32_t)(1 << CLKLIGHT_PIN) +#define CLKLIGHT_ON CLKLIGHT_PORT.OUTCLR.reg = CLKLIGHT_BM +#define CLKLIGHT_OFF CLKLIGHT_PORT.OUTSET.reg = CLKLIGHT_BM +#define CLKLIGHT_TOGGLE CLKLIGHT_PORT.OUTTGL.reg = CLKLIGHT_BM +#define CLKLIGHT_SETUP CLKLIGHT_PORT.DIRSET.reg = CLKLIGHT_BM; CLKLIGHT_OFF + +// PB08 +#define ERRLIGHT_BM (uint32_t)(1 << ERRLIGHT_PIN) +#define ERRLIGHT_ON ERRLIGHT_PORT.OUTCLR.reg = ERRLIGHT_BM +#define ERRLIGHT_OFF ERRLIGHT_PORT.OUTSET.reg = ERRLIGHT_BM +#define ERRLIGHT_TOGGLE ERRLIGHT_PORT.OUTTGL.reg = ERRLIGHT_BM +#define ERRLIGHT_SETUP ERRLIGHT_PORT.DIRSET.reg = ERRLIGHT_BM; ERRLIGHT_OFF + +#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-drop-stepper/src/drivers/peripheral_nums.h b/firmware/osape-smoothieroll-drop-stepper/src/drivers/peripheral_nums.h new file mode 100644 index 0000000000000000000000000000000000000000..eed9f188afacfb0da271d43603f833f61ec61191 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/drivers/peripheral_nums.h @@ -0,0 +1,18 @@ +#ifndef PERIPHERAL_NUMS_H_ +#define PERIPHERAL_NUMS_H_ + +#define PERIPHERAL_A 0 +#define PERIPHERAL_B 1 +#define PERIPHERAL_C 2 +#define PERIPHERAL_D 3 +#define PERIPHERAL_E 4 +#define PERIPHERAL_F 5 +#define PERIPHERAL_G 6 +#define PERIPHERAL_H 7 +#define PERIPHERAL_I 8 +#define PERIPHERAL_K 9 +#define PERIPHERAL_L 10 +#define PERIPHERAL_M 11 +#define PERIPHERAL_N 12 + +#endif \ No newline at end of file diff --git a/firmware/osape-smoothieroll-drop-stepper/src/drivers/step_a4950.cpp b/firmware/osape-smoothieroll-drop-stepper/src/drivers/step_a4950.cpp new file mode 100644 index 0000000000000000000000000000000000000000..361620b9d4e89ae076108b4ee997d98abe618d0f --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/drivers/step_a4950.cpp @@ -0,0 +1,184 @@ +/* +osap/drivers/step_a4950.cpp + +stepper code for two A4950s + +Jake Read at the Center for Bits and Atoms +(c) Massachusetts Institute of Technology 2019 + +This work may be reproduced, modified, distributed, performed, and +displayed for any purpose, but must acknowledge the squidworks and ponyo +projects. Copyright is retained and must be preserved. The work is provided as +is; no warranty is provided, and users accept all liability. +*/ + +#include "step_a4950.h" +//#include "ucbus_drop.h" + +// sine, 0-8190, 4095 center / 'zero', 256 steps +uint16_t LUT_8190[256] = { + 4095,4195,4296,4396,4496,4596,4696,4795,4894,4992, + 5090,5187,5284,5380,5475,5569,5662,5754,5846,5936, + 6025,6113,6200,6286,6370,6453,6534,6614,6693,6770, + 6845,6919,6991,7061,7129,7196,7260,7323,7384,7443, + 7500,7555,7607,7658,7706,7753,7797,7839,7878,7916, + 7951,7983,8014,8042,8067,8091,8111,8130,8146,8159, + 8170,8179,8185,8189,8190,8189,8185,8179,8170,8159, + 8146,8130,8111,8091,8067,8042,8014,7983,7951,7916, + 7878,7839,7797,7753,7706,7658,7607,7555,7500,7443, + 7384,7323,7260,7196,7129,7061,6991,6919,6845,6770, + 6693,6614,6534,6453,6370,6286,6200,6113,6025,5936, + 5846,5754,5662,5569,5475,5380,5284,5187,5090,4992, + 4894,4795,4696,4596,4496,4396,4296,4195,4095,3995, + 3894,3794,3694,3594,3494,3395,3296,3198,3100,3003, + 2906,2810,2715,2621,2528,2436,2344,2254,2165,2077, + 1990,1904,1820,1737,1656,1576,1497,1420,1345,1271, + 1199,1129,1061,994,930,867,806,747,690,635, + 583,532,484,437,393,351,312,274,239,207, + 176,148,123,99,79,60,44,31,20,11, + 5,1,0,1,5,11,20,31,44,60, + 79,99,123,148,176,207,239,274,312,351, + 393,437,484,532,583,635,690,747,806,867, + 930,994,1061,1129,1199,1271,1345,1420,1497,1576, + 1656,1737,1820,1904,1990,2077,2165,2254,2344,2436, + 2528,2621,2715,2810,2906,3003,3100,3198,3296,3395, + 3494,3594,3694,3794,3894,3995 +}; + +// sine, 0-1022 (511 center / 'zero'), 256 steps +uint16_t LUT_1022[256] = { + 511,524,536,549,561,574,586,598,611,623,635,647,659,671,683,695, + 707,718,729,741,752,763,774,784,795,805,815,825,835,845,854,863, + 872,881,890,898,906,914,921,929,936,943,949,956,962,967,973,978, + 983,988,992,996,1000,1003,1007,1010,1012,1014,1016,1018,1020,1021,1021,1022, + 1022,1022,1021,1021,1020,1018,1016,1014,1012,1010,1007,1003,1000,996,992,988, + 983,978,973,967,962,956,949,943,936,929,921,914,906,898,890,881, + 872,863,854,845,835,825,815,805,795,784,774,763,752,741,729,718, + 707,695,683,671,659,647,635,623,611,598,586,574,561,549,536,524, + 511,498,486,473,461,448,436,424,411,399,387,375,363,351,339,327, + 315,304,293,281,270,259,248,238,227,217,207,197,187,177,168,159, + 150,141,132,124,116,108,101,93,86,79,73,66,60,55,49,44, + 39,34,30,26,22,19,15,12,10,8,6,4,2,1,1,0, + 0,0,1,1,2,4,6,8,10,12,15,19,22,26,30,34, + 39,44,49,55,60,66,73,79,86,93,101,108,116,124,132,141, + 150,159,168,177,187,197,207,217,227,238,248,259,270,281,293,304, + 315,327,339,351,363,375,387,399,411,424,436,448,461,473,486,498, +}; + +uint16_t dacLUT[256]; + +STEP_A4950* STEP_A4950::instance = 0; + +STEP_A4950* STEP_A4950::getInstance(void){ + if(instance == 0){ + instance = new STEP_A4950(); + } + return instance; +} + +STEP_A4950* stepper_hw = STEP_A4950::getInstance(); + +STEP_A4950::STEP_A4950() {} + +void STEP_A4950::init(boolean invert, float cscale){ + // all of 'em, outputs + AIN1_PORT.DIRSET.reg = AIN1_BM; + AIN2_PORT.DIRSET.reg = AIN2_BM; + BIN1_PORT.DIRSET.reg = BIN1_BM; + BIN2_PORT.DIRSET.reg = BIN2_BM; + // floating cscale + if(cscale < 0){ + _cscale = 0; + } else if (cscale > 1){ + _cscale = 1; + } else { + _cscale = cscale; + } + // write a rectified LUT for writing to DACs + for(uint16_t i = 0; i < 256; i ++){ + if(LUT_8190[i] > 4095){ + dacLUT[i] = LUT_8190[i] - 4095; + } else if (LUT_8190[i] < 4095){ + dacLUT[i] = abs(4095 - LUT_8190[i]); + } else { + dacLUT[i] = 0; + } + } + // invert direction / not + _dir_invert = invert; + // start the DAAAC + dacs->init(); + // start condition, + step(); +} + +// sequence like +// S: 1 2 3 4 5 6 7 8 +// A: ^ ^ ^ x v v v x +// B: ^ x v v v x ^ ^ +void STEP_A4950::step(void){ + // increment: wrapping comes for free with uint8_t + if(_dir){ + if(_dir_invert){ + _aStep -= MICROSTEP_COUNT; + _bStep -= MICROSTEP_COUNT; + } else { + _aStep += MICROSTEP_COUNT; + _bStep += MICROSTEP_COUNT; + } + } else { + if(_dir_invert){ + _aStep += MICROSTEP_COUNT; + _bStep += MICROSTEP_COUNT; + } else { + _aStep -= MICROSTEP_COUNT; + _bStep -= MICROSTEP_COUNT; + } + } + // a phase, + if(LUT_8190[_aStep] > 4095){ + A_UP; + } else if (LUT_8190[_aStep] < 4095){ + A_DOWN; + } else { + A_OFF; + } + // a DAC + // so that we can easily rewrite currents on the fly. will extend to servoing, yeah + dacs->writeDac0(dacLUT[_aStep] * _cscale); + // b phase, + if(LUT_8190[_bStep] > 4095){ + B_UP; + } else if (LUT_8190[_bStep] < 4095){ + B_DOWN; + } else { + B_OFF; + } + // b DAC + dacs->writeDac1(dacLUT[_bStep] * _cscale); +} + +void STEP_A4950::dir(boolean val){ + _dir = val; +} + +boolean STEP_A4950::getDir(void){ + return _dir; +} + +void STEP_A4950::setCurrent(float cscale){ + if(cscale > 1){ + _cscale = 1; + } else if(cscale < 0){ + _cscale = 0; + } else { + _cscale = cscale; + } + // do DAC re-writes + dacs->writeDac0(dacLUT[_aStep] * _cscale); + dacs->writeDac1(dacLUT[_bStep] * _cscale); +} + +void STEP_A4950::dacRefresh(void){ + dacs->refresh(); +} diff --git a/firmware/osape-smoothieroll-drop-stepper/src/drivers/step_a4950.h b/firmware/osape-smoothieroll-drop-stepper/src/drivers/step_a4950.h new file mode 100644 index 0000000000000000000000000000000000000000..4b4286892b6795fba07de2a4e63d9dba53059b4f --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/drivers/step_a4950.h @@ -0,0 +1,102 @@ +/* +osap/drivers/step_a4950.h + +stepper code for two A4950s + +Jake Read at the Center for Bits and Atoms +(c) Massachusetts Institute of Technology 2019 + +This work may be reproduced, modified, distributed, performed, and +displayed for any purpose, but must acknowledge the squidworks and ponyo +projects. Copyright is retained and must be preserved. The work is provided as +is; no warranty is provided, and users accept all liability. +*/ + +#ifndef STEP_A4950_H_ +#define STEP_A4950_H_ + +#include <arduino.h> + +#include "dacs.h" +#include "indicators.h" +#include "utils/syserror.h" + +// C_SCALE +// 1: DACs go 0->512 (of 4096, peak current is 1.6A at 4096): 0.2A +// 2: DACs go 0->1024, +// ... +// 8: DACs go full width +//#define C_SCALE 8 // on init +// MICROSTEP_COUNT +// 1: do 1 tick of 256 table, for full resolution +// 2: 128 steps +// 4: 64 steps +// 8: 32 steps +// 16: 16 steps +// 32: 8 steps +#define MICROSTEP_COUNT 1 + +// AIN1 PB06 +// AIN2 PA04 +// BIN1 PA07 +// BIN2 PA06 +#define AIN1_PIN 6 +#define AIN1_PORT PORT->Group[1] +#define AIN1_BM (uint32_t)(1 << AIN1_PIN) +#define AIN2_PIN 4 +#define AIN2_PORT PORT->Group[0] +#define AIN2_BM (uint32_t)(1 << AIN2_PIN) +#define BIN1_PIN 7 +#define BIN1_PORT PORT->Group[0] +#define BIN1_BM (uint32_t)(1 << BIN1_PIN) +#define BIN2_PIN 6 +#define BIN2_PORT PORT->Group[0] +#define BIN2_BM (uint32_t)(1 << BIN2_PIN) + +// handles +#define AIN1_HI AIN1_PORT.OUTSET.reg = AIN1_BM +#define AIN1_LO AIN1_PORT.OUTCLR.reg = AIN1_BM +#define AIN2_HI AIN2_PORT.OUTSET.reg = AIN2_BM +#define AIN2_LO AIN2_PORT.OUTCLR.reg = AIN2_BM +#define BIN1_HI BIN1_PORT.OUTSET.reg = BIN1_BM +#define BIN1_LO BIN1_PORT.OUTCLR.reg = BIN1_BM +#define BIN2_HI BIN2_PORT.OUTSET.reg = BIN2_BM +#define BIN2_LO BIN2_PORT.OUTCLR.reg = BIN2_BM + +// set a phase up or down direction +// transition low first, avoid brake condition for however many ns +#define A_UP AIN2_LO; AIN1_HI +#define A_OFF AIN2_LO; AIN1_LO +#define A_DOWN AIN1_LO; AIN2_HI +#define B_UP BIN2_LO; BIN1_HI +#define B_OFF BIN2_LO; BIN1_LO +#define B_DOWN BIN1_LO; BIN2_HI + +class STEP_A4950 { + private: + // is driver, is singleton, + static STEP_A4950* instance; + volatile uint8_t _aStep = 0; // 0 of 256 micros, + volatile uint8_t _bStep = 63; // of the same table, startup 90' out of phase + volatile boolean _dir = false; + boolean _dir_invert = false; + // try single scalar + float _cscale = 0.25; + + public: + STEP_A4950(); + static STEP_A4950* getInstance(void); + // do like + void init(boolean invert, float cscale); + void step(void); + void dir(boolean val); + boolean getDir(void); + // current settings + void setCurrent(float cscale); + // for the dacs + void dacRefresh(void); +}; + +extern STEP_A4950* stepper_hw; + +#endif \ No newline at end of file diff --git a/firmware/osape-smoothieroll-drop-stepper/src/drivers/ucbus_drop.cpp b/firmware/osape-smoothieroll-drop-stepper/src/drivers/ucbus_drop.cpp new file mode 100644 index 0000000000000000000000000000000000000000..657919b0a40ca4d5f2ec6a653ff6e94a9b0cf7ae --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/drivers/ucbus_drop.cpp @@ -0,0 +1,281 @@ +/* +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_drop.h" + +UCBus_Drop* UCBus_Drop::instance = 0; + +UCBus_Drop* UCBus_Drop::getInstance(void){ + if(instance == 0){ + instance = new UCBus_Drop(); + } + return instance; +} + +// making this instance globally available, when built, +// recall extern declaration in .h +UCBus_Drop* ucBusDrop = UCBus_Drop::getInstance(); + +UCBus_Drop::UCBus_Drop(void) {} + +// so, first thing, I've some confusion about what might be the best way (again) to implement +// multiple similar drivers. I did this before w/ the cobserialport, I might again want to +// do it, but by writing more static hardware drivers that another parent class (the bus) forwards to +// so the init codes / etc could all be verbatim with hardware registers, instead of this infinite list of defines + +// yeah, this kind of stuff is morning work: focus, tracking little details. go to bed. + +void UCBus_Drop::init(boolean useDipPick, uint8_t ID) { + dip_init(); + if(useDipPick){ + // set our id, + id = dip_read_lower_five(); + } else { + id = ID; + } + if(id > 14){ + id = 14; + } + // set driver output LO to start: tri-state + UBD_DE_PORT.DIRSET.reg = UBD_DE_BM; + UBD_DRIVER_DISABLE; + // set receiver output on, forever: LO to set on + UBD_RE_PORT.DIRSET.reg = UBD_RE_BM; + UBD_RE_PORT.OUTCLR.reg = UBD_RE_BM; + // termination resistor should be set only on one drop, + // or none and physically with a 'tail' cable, or something? + UBD_TE_PORT.DIRSET.reg = UBD_TE_BM; + if(dip_read_pin_1()){ + UBD_TE_PORT.OUTCLR.reg = UBD_TE_BM; + } else { + UBD_TE_PORT.OUTSET.reg = UBD_TE_BM; + } + // rx pin setup + UBD_COMPORT.DIRCLR.reg = UBD_RXBM; + UBD_COMPORT.PINCFG[UBD_RXPIN].bit.PMUXEN = 1; + if(UBD_RXPIN % 2){ + UBD_COMPORT.PMUX[UBD_RXPIN >> 1].reg |= PORT_PMUX_PMUXO(UBD_RXPERIPHERAL); + } else { + UBD_COMPORT.PMUX[UBD_RXPIN >> 1].reg |= PORT_PMUX_PMUXE(UBD_RXPERIPHERAL); + } + // tx + UBD_COMPORT.DIRCLR.reg = UBD_TXBM; + UBD_COMPORT.PINCFG[UBD_TXPIN].bit.PMUXEN = 1; + if(UBD_TXPIN % 2){ + UBD_COMPORT.PMUX[UBD_TXPIN >> 1].reg |= PORT_PMUX_PMUXO(UBD_TXPERIPHERAL); + } else { + UBD_COMPORT.PMUX[UBD_TXPIN >> 1].reg |= PORT_PMUX_PMUXE(UBD_TXPERIPHERAL); + } + // ok, clocks, first line au manuel + // unmask clocks + MCLK->APBAMASK.bit.SERCOM1_ = 1; + GCLK->GENCTRL[UBD_GCLKNUM_PICK].reg = GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL) | GCLK_GENCTRL_GENEN; + while(GCLK->SYNCBUSY.reg & GCLK_SYNCBUSY_GENCTRL(UBD_GCLKNUM_PICK)); + GCLK->PCHCTRL[UBD_SERCOM_CLK].reg = GCLK_PCHCTRL_CHEN | GCLK_PCHCTRL_GEN(UBD_GCLKNUM_PICK); + // then, sercom + while(UBD_SER_USART.SYNCBUSY.bit.ENABLE); + UBD_SER_USART.CTRLA.bit.ENABLE = 0; + while(UBD_SER_USART.SYNCBUSY.bit.SWRST); + UBD_SER_USART.CTRLA.bit.SWRST = 1; + while(UBD_SER_USART.SYNCBUSY.bit.SWRST); + while(UBD_SER_USART.SYNCBUSY.bit.SWRST || UBD_SER_USART.SYNCBUSY.bit.ENABLE); + // ok, well + UBD_SER_USART.CTRLA.reg = SERCOM_USART_CTRLA_MODE(1) | SERCOM_USART_CTRLA_DORD; + UBD_SER_USART.CTRLA.reg |= SERCOM_USART_CTRLA_RXPO(UBD_RXPO) | SERCOM_USART_CTRLA_TXPO(0); + UBD_SER_USART.CTRLA.reg |= SERCOM_USART_CTRLA_FORM(1); // enable even parity + while(UBD_SER_USART.SYNCBUSY.bit.CTRLB); + UBD_SER_USART.CTRLB.reg = SERCOM_USART_CTRLB_RXEN | SERCOM_USART_CTRLB_TXEN | SERCOM_USART_CTRLB_CHSIZE(0); + // enable interrupts + NVIC_EnableIRQ(SERCOM1_2_IRQn); // rx, + NVIC_EnableIRQ(SERCOM1_1_IRQn); // txc ? + NVIC_EnableIRQ(SERCOM1_0_IRQn); // tx, + // set baud + UBD_SER_USART.BAUD.reg = UBD_BAUD_VAL; + // and finally, a kickoff + while(UBD_SER_USART.SYNCBUSY.bit.ENABLE); + UBD_SER_USART.CTRLA.bit.ENABLE = 1; + // enable rx interrupt + UBD_SER_USART.INTENSET.bit.RXC = 1; +} + +void SERCOM1_2_Handler(void){ + //ERRLIGHT_TOGGLE; + ucBusDrop->rxISR(); +} + +void UCBus_Drop::rxISR(void){ + // check parity bit, + uint16_t perr = UBD_SER_USART.STATUS.bit.PERR; + if(perr){ + //DEBUGPIN_ON; + uint8_t clear = UBD_SER_USART.DATA.reg; + UBD_SER_USART.STATUS.bit.PERR = 1; // clear parity error + return; + } + // cleared by reading out, but we are blind feed forward atm + uint8_t data = UBD_SER_USART.DATA.reg; + // for now, just running the clk, on every 0th bit + if((data >> 7) == 0){ // timer bit, on every 0th bit, and beginning of word + inWord[0] = data; + timeTick ++; + timeBlink ++; + if(timeBlink >= blinkTime){ + CLKLIGHT_TOGGLE; + timeBlink = 0; + } + onRxISR(); // on start of each word + } else { // end of word on every 0th bit + inWord[1] = data; + // now decouple, + inHeader = ((inWord[0] >> 1) & 0b00111000) | ((inWord[1] >> 4) & 0b00000111); + inByte = ((inWord[0] << 4) & 0b11110000) | (inWord[1] & 0b00001111); + // now check incoming data, + if((inHeader & 0b00110000) == 0b00100000){ // has-token, CHA + lastWordAHadToken = true; + if(inBufferALen != 0){ // in this case, we didn't read-out of the buffer in time, + inBufferALen = 0; // so we reset it, missing the last packet ! + inBufferARp = 0; + } + inBufferA[inBufferARp] = inByte; + inBufferARp ++; + } else if ((inHeader & 0b00110000) == 0b00000000) { // no-token, CHA + if(lastWordAHadToken){ + inBufferALen = inBufferARp - 1; + onPacketARx(); + } + lastWordAHadToken = false; + } else if ((inHeader & 0b00110000) == 0b00110000) { // has-token, CHB + //DEBUG1PIN_ON; + lastWordBHadToken = true; + if(inBufferBLen != 0){ + inBufferBLen = 0; + inBufferBRp = 0; + } + inBufferB[inBufferBRp] = inByte; + inBufferBRp ++; + } else if ((inHeader & 0b00110000) == 0b00010000) { // no-token, CHB + if(lastWordBHadToken){ + inBufferBLen = inBufferARp - 1; + //onPacketBRx(); // b-channel handled in loop, yah + } + lastWordBHadToken = false; + } + // now check if outgoing tick is ours, + if((inHeader & dropIdMask) == id){ + // our transmit + if(outBufferLen > 0){ + outByte = outBuffer[outBufferRp]; + outHeader = headerMask & (tokenWord | (id & 0b00011111)); + } else { + outByte = 0; + outHeader = headerMask & (noTokenWord | (id & 0b00011111)); + } + outWord[0] = 0b00000000 | ((outHeader << 1) & 0b01110000) | (outByte >> 4); + outWord[1] = 0b10000000 | ((outHeader << 4) & 0b01110000) | (outByte & 0b00001111); + // now transmit, + UBD_DRIVER_ENABLE; + UBD_SER_USART.DATA.reg = outWord[0]; + UBD_SER_USART.INTENSET.bit.DRE = 1; + // now do this, + if(outBufferLen > 0){ + outBufferRp ++; + if(outBufferRp >= outBufferLen){ + outBufferRp = 0; + outBufferLen = 0; + } + } + } else if ((inHeader & dropIdMask) == UBD_CLOCKRESET){ + // reset + timeTick = 0; + } + } // end 1th bit case, + // do every-tick stuff +} + +void SERCOM1_0_Handler(void){ + ucBusDrop->dreISR(); +} + +void UCBus_Drop::dreISR(void){ + UBD_SER_USART.DATA.reg = outWord[1]; // tx the next word, + UBD_SER_USART.INTENCLR.reg = SERCOM_USART_INTENCLR_DRE; // clear this interrupt + UBD_SER_USART.INTENSET.reg = SERCOM_USART_INTENSET_TXC; // now watch transmit complete +} + +void SERCOM1_1_Handler(void){ + ucBusDrop->txcISR(); +} + +void UCBus_Drop::txcISR(void){ + UBD_SER_USART.INTFLAG.bit.TXC = 1; // clear the flag by writing 1 + UBD_SER_USART.INTENCLR.reg = SERCOM_USART_INTENCLR_TXC; // turn off the interrupt + UBD_DRIVER_DISABLE; // turn off the driver, + //DEBUGPIN_TOGGLE; +} + +// -------------------------------------------------------- ASYNC API + +boolean UCBus_Drop::ctr_a(void){ + if(inBufferALen > 0){ + return true; + } else { + return false; + } +} + +boolean UCBus_Drop::ctr_b(void){ + if(inBufferBLen > 0){ + return true; + } else { + return false; + } +} + +size_t UCBus_Drop::read_a(uint8_t *dest){ + if(!ctr_a()) return 0; + NVIC_DisableIRQ(SERCOM1_2_IRQn); + // copy out, irq disabled, TODO safety on missing an interrupt in this case?? clock jitter? + memcpy(dest, inBufferA, inBufferALen); + size_t len = inBufferALen; + inBufferALen = 0; + inBufferARp = 0; + NVIC_EnableIRQ(SERCOM1_2_IRQn); + return len; +} + +size_t UCBus_Drop::read_b(uint8_t *dest){ + if(!ctr_b()) return 0; + NVIC_DisableIRQ(SERCOM1_2_IRQn); + memcpy(dest, inBufferB, inBufferBLen); + size_t len = inBufferBLen; + inBufferBLen = 0; + inBufferBRp = 0; + NVIC_EnableIRQ(SERCOM1_2_IRQn); + return len; +} + +boolean UCBus_Drop::cts(void){ + if(outBufferLen > 0){ + return false; + } else { + return true; + } +} + +void UCBus_Drop::transmit(uint8_t *data, uint16_t len){ + if(!cts()) return; + size_t encLen = cobsEncode(data, len, outBuffer); + outBufferLen = encLen; + outBufferRp = 0; +} \ No newline at end of file diff --git a/firmware/osape-smoothieroll-drop-stepper/src/drivers/ucbus_drop.h b/firmware/osape-smoothieroll-drop-stepper/src/drivers/ucbus_drop.h new file mode 100644 index 0000000000000000000000000000000000000000..de1d55eae2f771880b5f140450cf6033fa0d001c --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/drivers/ucbus_drop.h @@ -0,0 +1,123 @@ +/* +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_DROP_H_ +#define UCBUS_DROP_H_ + +#include <arduino.h> + +#include "indicators.h" +#include "dip_ucbus_config.h" +#include "peripheral_nums.h" +#include "utils/syserror.h" +#include "utils/cobs.h" + +#define UBD_SER_USART SERCOM1->USART +#define UBD_SERCOM_CLK SERCOM1_GCLK_ID_CORE +#define UBD_GCLKNUM_PICK 7 +#define UBD_COMPORT PORT->Group[0] +#define UBD_TXPIN 16 // x-0 +#define UBD_TXBM (uint32_t)(1 << UBD_TXPIN) +#define UBD_RXPIN 18 // x-2 +#define UBD_RXBM (uint32_t)(1 << UBD_RXPIN) +#define UBD_RXPO 2 +#define UBD_TXPERIPHERAL PERIPHERAL_C +#define UBD_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 UBD_BAUD_VAL 0 + +#define UBD_DE_PIN 16 // driver output enable: set HI to enable, LO to tri-state the driver +#define UBD_DE_BM (uint32_t)(1 << UBD_DE_PIN) +#define UBD_DE_PORT PORT->Group[1] +#define UBD_DRIVER_ENABLE UBD_DE_PORT.OUTSET.reg = UBD_DE_BM +#define UBD_DRIVER_DISABLE UBD_DE_PORT.OUTCLR.reg = UBD_DE_BM +#define UBD_RE_PIN 19 // receiver output enable, set LO to enable the RO, set HI to tri-state RO +#define UBD_RE_BM (uint32_t)(1 << UBD_RE_PIN) +#define UBD_RE_PORT PORT->Group[0] +#define UBD_TE_PIN 17 // termination enable, drive LO to enable to internal termination resistor, HI to disable +#define UBD_TE_BM (uint32_t)(1 << UBD_TE_PIN) +#define UBD_TE_PORT PORT->Group[0] + +#define UBD_BUFSIZE 1024 + +#define UBD_CLOCKRESET 15 + +#define UB_AK_GOTOPOS 91 +#define UB_AK_SETPOS 92 +#define UB_AK_SETRPM 93 + +class UCBus_Drop { + private: + // singleton-ness + static UCBus_Drop* instance; + // timing, + volatile uint64_t timeBlink = 0; + uint16_t blinkTime = 10000; + // input buffer & word + volatile uint8_t inWord[2]; + volatile uint8_t inHeader; + volatile uint8_t inByte; + volatile boolean lastWordAHadToken = false; + uint8_t inBufferA[UBD_BUFSIZE]; + volatile uint16_t inBufferARp = 0; + volatile uint16_t inBufferALen = 0; // writes at terminal zero, + volatile boolean lastWordBHadToken = false; + uint8_t inBufferB[UBD_BUFSIZE]; + volatile uint16_t inBufferBRp = 0; + volatile uint16_t inBufferBLen = 0; + // output, + volatile uint8_t outWord[2]; + volatile uint8_t outHeader; + volatile uint8_t outByte; + uint8_t outBuffer[UBD_BUFSIZE]; + volatile uint16_t outBufferRp = 0; + volatile uint16_t outBufferLen = 0; + const uint8_t headerMask = 0b00111111; + const uint8_t dropIdMask = 0b00001111; + const uint8_t tokenWord = 0b00100000; + const uint8_t noTokenWord = 0b00000000; + public: + UCBus_Drop(); + static UCBus_Drop* getInstance(void); + // available time count, + volatile uint16_t timeTick = 0; + // isrs + void rxISR(void); + void dreISR(void); + void txcISR(void); + // handlers + void onRxISR(void); + void onPacketARx(void); + //void onPacketBRx(void); + // our physical bus address, + volatile uint8_t id = 0; + // the api, eh + void init(boolean useDipPick, uint8_t ID); + boolean ctr_a(void); // return true if RX complete / buffer ready + boolean ctr_b(void); + size_t read_a(uint8_t *dest); // ship les bytos + size_t read_b(uint8_t *dest); + boolean cts(void); // true if tx buffer empty, + void transmit(uint8_t *data, uint16_t len); +}; + +extern UCBus_Drop* ucBusDrop; + +#endif \ No newline at end of file diff --git a/firmware/osape-smoothieroll-drop-stepper/src/main.cpp b/firmware/osape-smoothieroll-drop-stepper/src/main.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a340975f910ef8f4adfcc78be7239ce3f82eccb5 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/main.cpp @@ -0,0 +1,193 @@ +#include <Arduino.h> + +#include "drivers/indicators.h" +#include "utils/cobs.h" +#include "osap/osap.h" + +OSAP* osap = new OSAP("stepper motor drop"); + +#include "drivers/ucbus_drop.h" +#include "drivers/step_a4950.h" + +union chunk_float32 { + uint8_t bytes[4]; + float f; +}; + +union chunk_float64 { + uint8_t bytes[8]; + double f; +}; + +// C_SCALE +// 1: DACs go 0->512 (of 4096, peak current is 1.6A at 4096): 0.2A +// 2: DACs go 0->1024, +// ... +// 8: DACs go full width + +#define BUS_DROP 1 // Z: 1, YL: 2, X: 3, YR: 4 +#define AXIS_PICK 2 // Z: 2, Y: 1, X: 0 +#define AXIS_INVERT false // Z: false, YL: true, YR: false, X: false +#define SPU 3200.0F // always posiive! Z: 3200, XY: 400 +#define C_SCALE 0.2F // 0-1, floating: initial holding current to motor, 0-2.5A +#define TICKS_PER_PACKET 20.0F // always 20.0F + +void setup() { + ERRLIGHT_SETUP; + CLKLIGHT_SETUP; + DEBUG1PIN_SETUP; + DEBUG2PIN_SETUP; + DEBUG3PIN_SETUP; + DEBUG4PIN_SETUP; + // osap + osap->description = "remote stepper drop"; + // bus + ucBusDrop->init(false, BUS_DROP); + // stepper + stepper_hw->init(AXIS_INVERT, C_SCALE); +} + +uint8_t bChPck[64]; + +volatile float current_floating_pos = 0.0F; +volatile int32_t current_step_pos = 0; +volatile uint32_t delta_steps = 0; + +volatile float vel = 0.0F; +volatile float move_counter = 0.0F; + +volatile boolean setBlock = false; + +void loop() { + DEBUG2PIN_TOGGLE; + osap->loop(); + stepper_hw->dacRefresh(); + // do packet B grab / handle + if(ucBusDrop->ctr_b()){ + uint16_t len = ucBusDrop->read_b(bChPck); + uint16_t ptr = 0; + switch(bChPck[ptr]){ + case AK_SETCURRENT: { + // get currents + ptr ++; + chunk_float32 currentChunks[3]; + currentChunks[0] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } }; + currentChunks[1] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } }; + currentChunks[2] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } }; + // set current + stepper_hw->setCurrent(currentChunks[AXIS_PICK].f); + break; + } + case AK_SETPOS: { + // get posns, + ptr ++; + chunk_float32 posChunks[3]; + posChunks[0] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } }; + posChunks[1] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } }; + posChunks[2] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } }; + float target_current_pos = posChunks[AXIS_PICK].f; + int32_t target_current_steps = lroundf(target_current_pos * SPU); + setBlock = true; // don't do step work while these are modified + current_floating_pos = target_current_pos; + current_step_pos = target_current_steps; + setBlock = false; + } + default: + // noop, + break; + } + } +} // end loop + +void UCBus_Drop::onRxISR(void){ + if(setBlock) return; + //DEBUG2PIN_TOGGLE; + move_counter += vel; + boolean move_check = (move_counter > 1.0F); + //DEBUG2PIN_TOGGLE; + if(move_check){ + move_counter -= 1.0F; + if(delta_steps == 0){ + // nothing + } else { + //DEBUG1PIN_TOGGLE; + stepper_hw->step(); + delta_steps --; + if(stepper_hw->getDir()){ + current_step_pos ++; + } else { + current_step_pos --; + } + } + } +} + +void UCBus_Drop::onPacketARx(void){ + if(setBlock) return; + //DEBUG2PIN_TOGGLE; + // last move is done, convert back steps -> float, + current_floating_pos = current_step_pos / SPU; + vel = 0.0F; // reset zero in case packet is not move + uint8_t bptr = 0; + // switch bus packet types + switch(inBufferA[0]){ + case UB_AK_GOTOPOS: + { + bptr = AXIS_PICK * 4 + 1; + chunk_float32 target = { + .bytes = { inBufferA[bptr], inBufferA[bptr + 1], inBufferA[bptr + 2], inBufferA[bptr + 3] } + }; + /* + chunk_float64 target = { + .bytes = { inBuffer[bptr], inBuffer[bptr + 1], inBuffer[bptr + 2], inBuffer[bptr + 3], + inBuffer[bptr + 4], inBuffer[bptr + 5], inBuffer[bptr + 6], inBuffer[bptr + 7] }}; + */ + float delta = target.f - current_floating_pos; + // update, + //move_counter = 0.0F; // shouldn't need this ? try deleting + // direction swop, + if(delta > 0){ + stepper_hw->dir(true); + } else { + stepper_hw->dir(false); + } + // how many steps, + delta_steps = lroundf(abs(delta * SPU)); + // what speed + vel = abs(delta * SPU) / TICKS_PER_PACKET; + // for str8 r8 + /* + if(delta_steps == 0){ + vel = 0.0F; + } else { + vel = abs(delta * SPU) / TICKS_PER_PACKET; + } + */ + break; // end gotopos + } + case UB_AK_SETPOS: + { + // reqest is to set position, not go to it... + bptr = AXIS_PICK * 4 + 1; + chunk_float32 target = { + .bytes = { inBufferA[bptr], inBufferA[bptr + 1], inBufferA[bptr + 2], inBufferA[bptr + 3] } + }; + float target_current_pos = target.f; + int32_t target_current_steps = lroundf(target_current_pos * SPU); + setBlock = true; // don't do step work while these are modified + current_floating_pos = target_current_pos; + current_step_pos = target_current_steps; + vel = 0; + delta_steps = 0; + setBlock = false; + break; + } + default: + break; + } + //DEBUG2PIN_OFF; +} + +void OSAP::handleAppPacket(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t segsize, VPort* vp, uint16_t vpi, uint8_t pwp){ + vp->clearPacket(pwp); +} diff --git a/firmware/osape-smoothieroll-drop-stepper/src/osap/osap.cpp b/firmware/osape-smoothieroll-drop-stepper/src/osap/osap.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4b4499c5c2cd3fbd93ca95f0f43608b0f613aa2c --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/osap/osap.cpp @@ -0,0 +1,487 @@ +/* +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-drop-stepper/src/osap/osap.h b/firmware/osape-smoothieroll-drop-stepper/src/osap/osap.h new file mode 100644 index 0000000000000000000000000000000000000000..5242ba0ae6cdc77538ef57f119a6691fb46188fa --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/osap/osap.h @@ -0,0 +1,60 @@ +/* +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-drop-stepper/src/osap/ts.cpp b/firmware/osape-smoothieroll-drop-stepper/src/osap/ts.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1a399002651a3f22be9cb8c51e373af13d25163d --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/osap/ts.cpp @@ -0,0 +1,64 @@ +/* +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]; +} + +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-drop-stepper/src/osap/ts.h b/firmware/osape-smoothieroll-drop-stepper/src/osap/ts.h new file mode 100644 index 0000000000000000000000000000000000000000..1029171322097efa534c83d8db9c2ee835024ffc --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/osap/ts.h @@ -0,0 +1,100 @@ +/* +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_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_writeString(String val, unsigned char *buf, uint16_t *ptr); diff --git a/firmware/osape-smoothieroll-drop-stepper/src/osap/vport.cpp b/firmware/osape-smoothieroll-drop-stepper/src/osap/vport.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e064991454d718657dbc30027b4d6b0f9c8b7d8f --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/osap/vport.cpp @@ -0,0 +1,40 @@ +/* +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-drop-stepper/src/osap/vport.h b/firmware/osape-smoothieroll-drop-stepper/src/osap/vport.h new file mode 100644 index 0000000000000000000000000000000000000000..b705e91a150ff544b2efa0ceba17f4f6b3b515eb --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/osap/vport.h @@ -0,0 +1,52 @@ +/* +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-drop-stepper/src/osap/vport_usbserial.cpp b/firmware/osape-smoothieroll-drop-stepper/src/osap/vport_usbserial.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ddbc9fd308439d946194f2a75364b4613bb1f2a4 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/osap/vport_usbserial.cpp @@ -0,0 +1,141 @@ +/* +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-drop-stepper/src/osap/vport_usbserial.h b/firmware/osape-smoothieroll-drop-stepper/src/osap/vport_usbserial.h new file mode 100644 index 0000000000000000000000000000000000000000..4a72d1ce9462ca8bf4ee76992d56c62a8a54a0cf --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/osap/vport_usbserial.h @@ -0,0 +1,57 @@ +/* +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-drop-stepper/src/utils/clocks_d51_module.cpp b/firmware/osape-smoothieroll-drop-stepper/src/utils/clocks_d51_module.cpp new file mode 100644 index 0000000000000000000000000000000000000000..317c40390d35e1e947eab6ff27c05cb466124db8 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/utils/clocks_d51_module.cpp @@ -0,0 +1,115 @@ +/* +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-drop-stepper/src/utils/clocks_d51_module.h b/firmware/osape-smoothieroll-drop-stepper/src/utils/clocks_d51_module.h new file mode 100644 index 0000000000000000000000000000000000000000..084141e09bd4ca38bc56eb3505c31075358c8fb5 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/utils/clocks_d51_module.h @@ -0,0 +1,43 @@ +/* +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-drop-stepper/src/utils/cobs.cpp b/firmware/osape-smoothieroll-drop-stepper/src/utils/cobs.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a2d73787ba6f2626405e92aa1f72dc6da8ea43d5 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/utils/cobs.cpp @@ -0,0 +1,60 @@ +/* +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-drop-stepper/src/utils/cobs.h b/firmware/osape-smoothieroll-drop-stepper/src/utils/cobs.h new file mode 100644 index 0000000000000000000000000000000000000000..a9e587463dc4cfc6f8175d0ff48b53174970133c --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/utils/cobs.h @@ -0,0 +1,24 @@ +/* +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-drop-stepper/src/utils/syserror.cpp b/firmware/osape-smoothieroll-drop-stepper/src/utils/syserror.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a2473fe296a6744d233b39d217bfef523aa6f776 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/utils/syserror.cpp @@ -0,0 +1,38 @@ +#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-drop-stepper/src/utils/syserror.h b/firmware/osape-smoothieroll-drop-stepper/src/utils/syserror.h new file mode 100644 index 0000000000000000000000000000000000000000..b421cc2a08eb1d482a9f298ed9dbefaa90531f9d --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/src/utils/syserror.h @@ -0,0 +1,11 @@ +#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-drop-stepper/test/README b/firmware/osape-smoothieroll-drop-stepper/test/README new file mode 100644 index 0000000000000000000000000000000000000000..df5066e64d90bf4773299cb0037271e1c34d48d5 --- /dev/null +++ b/firmware/osape-smoothieroll-drop-stepper/test/README @@ -0,0 +1,11 @@ + +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 diff --git a/firmware/osape-smoothieroll-head/.gitignore b/firmware/osape-smoothieroll-head/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..89cc49cbd652508924b868ea609fa8f6b758ec56 --- /dev/null +++ b/firmware/osape-smoothieroll-head/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/firmware/osape-smoothieroll-head/.travis.yml b/firmware/osape-smoothieroll-head/.travis.yml new file mode 100644 index 0000000000000000000000000000000000000000..7c486f183c3cefc46ffae9d5768b04f61ab068b7 --- /dev/null +++ b/firmware/osape-smoothieroll-head/.travis.yml @@ -0,0 +1,67 @@ +# 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 new file mode 100644 index 0000000000000000000000000000000000000000..e80666bfb11e75aafd3600701dc99c10acce341c --- /dev/null +++ b/firmware/osape-smoothieroll-head/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ] +} diff --git a/firmware/osape-smoothieroll-head/.vscode/settings.json b/firmware/osape-smoothieroll-head/.vscode/settings.json new file mode 100644 index 0000000000000000000000000000000000000000..adb1e49f93dad1aafd82900e8f947a5169d8bf29 --- /dev/null +++ b/firmware/osape-smoothieroll-head/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "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 new file mode 100644 index 0000000000000000000000000000000000000000..194dcd43252dcbeb2044ee38510415041a0e7b47 --- /dev/null +++ b/firmware/osape-smoothieroll-head/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/firmware/osape-smoothieroll-head/lib/README b/firmware/osape-smoothieroll-head/lib/README new file mode 100644 index 0000000000000000000000000000000000000000..6debab1e8b4c3faa0d06f4ff44bce343ce2cdcbf --- /dev/null +++ b/firmware/osape-smoothieroll-head/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include <Foo.h> +#include <Bar.h> + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/firmware/osape-smoothieroll-head/platformio.ini b/firmware/osape-smoothieroll-head/platformio.ini new file mode 100644 index 0000000000000000000000000000000000000000..50208f0cadb2e924ce3a661e45b5c88422dc71af --- /dev/null +++ b/firmware/osape-smoothieroll-head/platformio.ini @@ -0,0 +1,14 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:adafruit_feather_m4] +platform = atmelsam +board = adafruit_feather_m4 +framework = arduino diff --git a/firmware/osape-smoothieroll-head/src/drivers/indicators.h b/firmware/osape-smoothieroll-head/src/drivers/indicators.h new file mode 100644 index 0000000000000000000000000000000000000000..5a2356af41aba61410ee375490ff3d6f458b1052 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/drivers/indicators.h @@ -0,0 +1,57 @@ +// 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 new file mode 100644 index 0000000000000000000000000000000000000000..eed9f188afacfb0da271d43603f833f61ec61191 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/drivers/peripheral_nums.h @@ -0,0 +1,18 @@ +#ifndef PERIPHERAL_NUMS_H_ +#define PERIPHERAL_NUMS_H_ + +#define PERIPHERAL_A 0 +#define PERIPHERAL_B 1 +#define PERIPHERAL_C 2 +#define PERIPHERAL_D 3 +#define PERIPHERAL_E 4 +#define PERIPHERAL_F 5 +#define PERIPHERAL_G 6 +#define PERIPHERAL_H 7 +#define PERIPHERAL_I 8 +#define PERIPHERAL_K 9 +#define PERIPHERAL_L 10 +#define PERIPHERAL_M 11 +#define PERIPHERAL_N 12 + +#endif \ No newline at end of file diff --git a/firmware/osape-smoothieroll-head/src/drivers/ucbus_head.cpp b/firmware/osape-smoothieroll-head/src/drivers/ucbus_head.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8b6126d291820a3ce14225882a676f1fb6d11a82 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/drivers/ucbus_head.cpp @@ -0,0 +1,261 @@ +/* +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 new file mode 100644 index 0000000000000000000000000000000000000000..e171d1cff9c3df0201f7d050a5d6c78def310f79 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/drivers/ucbus_head.h @@ -0,0 +1,122 @@ +/* +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 new file mode 100644 index 0000000000000000000000000000000000000000..c7fe2c576858564171e9d3a4620c6073d6b8c767 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/main.cpp @@ -0,0 +1,404 @@ +#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 new file mode 100644 index 0000000000000000000000000000000000000000..4b4499c5c2cd3fbd93ca95f0f43608b0f613aa2c --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/osap/osap.cpp @@ -0,0 +1,487 @@ +/* +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 new file mode 100644 index 0000000000000000000000000000000000000000..5242ba0ae6cdc77538ef57f119a6691fb46188fa --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/osap/osap.h @@ -0,0 +1,60 @@ +/* +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 new file mode 100644 index 0000000000000000000000000000000000000000..6fb3474c9a59a16fc5c1b50983f5e0a81ca424da --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/osap/ts.cpp @@ -0,0 +1,82 @@ +/* +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 new file mode 100644 index 0000000000000000000000000000000000000000..7cf3f01f0d9fecc27e0f6369d037fd5286d05743 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/osap/ts.h @@ -0,0 +1,103 @@ +/* +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 new file mode 100644 index 0000000000000000000000000000000000000000..e064991454d718657dbc30027b4d6b0f9c8b7d8f --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/osap/vport.cpp @@ -0,0 +1,40 @@ +/* +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 new file mode 100644 index 0000000000000000000000000000000000000000..b705e91a150ff544b2efa0ceba17f4f6b3b515eb --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/osap/vport.h @@ -0,0 +1,52 @@ +/* +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 new file mode 100644 index 0000000000000000000000000000000000000000..ddbc9fd308439d946194f2a75364b4613bb1f2a4 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/osap/vport_usbserial.cpp @@ -0,0 +1,141 @@ +/* +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 new file mode 100644 index 0000000000000000000000000000000000000000..4a72d1ce9462ca8bf4ee76992d56c62a8a54a0cf --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/osap/vport_usbserial.h @@ -0,0 +1,57 @@ +/* +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 new file mode 100644 index 0000000000000000000000000000000000000000..41ec8ee80c9dffc0934d9a2bf1e97cf2af360083 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/smoothie/SmoothieRoll.cpp @@ -0,0 +1,45 @@ +/* +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 new file mode 100644 index 0000000000000000000000000000000000000000..c9145816c4298b5f2feb1679c5f3f0b0973a09d7 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/smoothie/SmoothieRoll.h @@ -0,0 +1,43 @@ +/* +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 new file mode 100644 index 0000000000000000000000000000000000000000..fbc395520142ab401a617eaf39c2d3ad099df078 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/smoothie/libs/HeapRing.cpp @@ -0,0 +1,255 @@ +#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 new file mode 100644 index 0000000000000000000000000000000000000000..cd20af6115beb293e6fca29149623f4905c51931 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/smoothie/libs/HeapRing.h @@ -0,0 +1,86 @@ +#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 new file mode 100644 index 0000000000000000000000000000000000000000..463bfc0c86baff09d19962eeb3a6c31e4eafcca1 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/smoothie/libs/StepTicker.cpp @@ -0,0 +1,222 @@ +/* + 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 new file mode 100644 index 0000000000000000000000000000000000000000..fedb81242d355481b8a82c3b9c13e2b8d4d0aae0 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/smoothie/libs/StepTicker.h @@ -0,0 +1,76 @@ +/* + 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 new file mode 100644 index 0000000000000000000000000000000000000000..135dd048840e7b4d0bb15389cef7038254fb2707 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/ActuatorCoordinates.h @@ -0,0 +1,16 @@ +/* + 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 new file mode 100644 index 0000000000000000000000000000000000000000..460e584fb1f040213a26e818993c3875f3d28f1b --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Block.cpp @@ -0,0 +1,350 @@ +/* + 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 new file mode 100644 index 0000000000000000000000000000000000000000..76884db202ab6f6f4626a496ffb1184ed84817fa --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Block.h @@ -0,0 +1,79 @@ +/* + 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 new file mode 100644 index 0000000000000000000000000000000000000000..c0e187659e128f9ce38847082e8611e60c09914c --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Conveyor.cpp @@ -0,0 +1,318 @@ +/* + 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 new file mode 100644 index 0000000000000000000000000000000000000000..096e3ab7a6b1a9c0345c6b1be43d1082ad9f8375 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Conveyor.h @@ -0,0 +1,73 @@ +/* + 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 new file mode 100644 index 0000000000000000000000000000000000000000..4c41bbd0944e627de7dfeaaccfd57f9ee176ebc2 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Planner.cpp @@ -0,0 +1,351 @@ +/* + 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 new file mode 100644 index 0000000000000000000000000000000000000000..c0ed704c5258e8f6fb7ed8785aeab00a26147ab6 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/Planner.h @@ -0,0 +1,49 @@ +/* + 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 new file mode 100644 index 0000000000000000000000000000000000000000..7cf29a0ca96513e773066f428ee425c6326fd261 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/StepInterface.cpp @@ -0,0 +1,61 @@ +/* + +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 new file mode 100644 index 0000000000000000000000000000000000000000..b9d2f6dc213fff52a2dccbfc6f682ae4b7b902fe --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/smoothie/modules/robot/StepInterface.h @@ -0,0 +1,44 @@ +/* + +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 new file mode 100644 index 0000000000000000000000000000000000000000..af2c83c1f9ce4f7548ca12d257aebf88a612dd5b --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/utils/clocks_d51_module.cpp @@ -0,0 +1,115 @@ +/* +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 new file mode 100644 index 0000000000000000000000000000000000000000..084141e09bd4ca38bc56eb3505c31075358c8fb5 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/utils/clocks_d51_module.h @@ -0,0 +1,43 @@ +/* +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 new file mode 100644 index 0000000000000000000000000000000000000000..a2d73787ba6f2626405e92aa1f72dc6da8ea43d5 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/utils/cobs.cpp @@ -0,0 +1,60 @@ +/* +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 new file mode 100644 index 0000000000000000000000000000000000000000..a9e587463dc4cfc6f8175d0ff48b53174970133c --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/utils/cobs.h @@ -0,0 +1,24 @@ +/* +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 new file mode 100644 index 0000000000000000000000000000000000000000..a2473fe296a6744d233b39d217bfef523aa6f776 --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/utils/syserror.cpp @@ -0,0 +1,38 @@ +#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 new file mode 100644 index 0000000000000000000000000000000000000000..b421cc2a08eb1d482a9f298ed9dbefaa90531f9d --- /dev/null +++ b/firmware/osape-smoothieroll-head/src/utils/syserror.h @@ -0,0 +1,11 @@ +#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 new file mode 100644 index 0000000000000000000000000000000000000000..df5066e64d90bf4773299cb0037271e1c34d48d5 --- /dev/null +++ b/firmware/osape-smoothieroll-head/test/README @@ -0,0 +1,11 @@ + +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