├── .github └── workflows │ └── build.yaml ├── .gitignore ├── LICENSE ├── README.md ├── img └── photon-packet.png ├── photon-firmware.png └── photon ├── .gitignore ├── platformio.ini ├── src ├── FeederFloor.cpp ├── FeederFloor.h ├── PhotonFeeder.cpp ├── PhotonFeeder.h ├── PhotonFeederProtocol.cpp ├── PhotonFeederProtocol.h ├── PhotonNetworkLayer.cpp ├── PhotonNetworkLayer.h ├── PhotonPacketHandler.h ├── define.h ├── main.cpp ├── util ├── util.cpp ├── util.h └── versions.h └── test ├── test_desktop ├── test_index_feeder_protocol.cpp ├── test_index_network_layer.cpp └── test_main.cpp └── test_embedded └── test_main.cpp /.github/workflows/build.yaml: -------------------------------------------------------------------------------- 1 | name: Build Photon 2 | 3 | on: 4 | workflow_dispatch: 5 | release: 6 | types: [ published ] 7 | 8 | jobs: 9 | build: 10 | name: Build and Upload 11 | runs-on: ubuntu-22.04 12 | steps: 13 | - name: Check out the PR 14 | uses: actions/checkout@v4 15 | 16 | - name: Select Python 3.7 17 | uses: actions/setup-python@v3 18 | with: 19 | python-version: '3.7' 20 | architecture: 'x64' 21 | 22 | - name: Generate Short SHA Environment Variable 23 | run: echo "SHORT_SHA=`echo ${GITHUB_SHA} | cut -c1-8`" >> $GITHUB_ENV 24 | 25 | - name: Install PlatformIO 26 | run: | 27 | pip install -U platformio 28 | pio upgrade --dev 29 | pio pkg update --global 30 | 31 | - name: Compile Artifacts for Release 32 | if: github.event_name == 'release' 33 | run: | 34 | export VERSION_STRING=${{ github.event.release.tag_name }} 35 | cd photon 36 | pio run 37 | cp .pio/build/photon-bmp/firmware.bin ../photon-lumenpnp-${{ github.event.release.tag_name }}.bin 38 | cp .pio/build/photon-bmp/firmware.elf ../photon-lumenpnp-${{ github.event.release.tag_name }}.elf 39 | 40 | 41 | - name: Upload Artifacts to Release 42 | uses: softprops/action-gh-release@v2 43 | if: github.event_name == 'release' 44 | with: 45 | files: | 46 | photon-lumenpnp-${{ github.event.release.tag_name }}.bin 47 | photon-lumenpnp-${{ github.event.release.tag_name }}.elf 48 | 49 | - name: Compile Artifacts for Workflow Dispatch 50 | if: github.event_name != 'release' 51 | run: | 52 | export VERSION_STRING=debug-${{ env.SHORT_SHA }} 53 | cd photon && pio run 54 | cp .pio/build/photon-bmp/firmware.bin ../photon-lumenpnp-${{ env.SHORT_SHA }}.bin 55 | cp .pio/build/photon-bmp/firmware.elf ../photon-lumenpnp-${{ env.SHORT_SHA }}.elf 56 | 57 | - name: Upload Artifacts to Workflow Dispatch 58 | if: github.event_name != 'release' 59 | uses: actions/upload-artifact@v4 60 | with: 61 | name: photon-lumenpnp-${{ env.SHORT_SHA }}.bin 62 | path: photon-lumenpnp-${{ env.SHORT_SHA }}.elf 63 | 64 | 65 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | .DS_Store 35 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | # Opulo LumenPnP Feeder License and Copyright Notices 2 | 3 | LumenPnP is (c) by Opulo 4 | 5 | This notice must be included in any distributions of this project or derivative works. 6 | 7 | 1. The software in this repository is available under Mozilla MPL v2.0. Full text is available at https://www.mozilla.org/en-US/MPL/2.0/. 8 | 9 | 2. Opulo's logo, branding, and other media is used throughout this project. This is Copyright (c) Opulo and all rights are reserved. You may not distribute derivative works or products bearing the Opulo logo, icon, or other relevant mark. Derivative works should remove Opulo branding. 10 | 11 | 3. The name `Opulo` and `LumenPnP` are trademarked, and only to be used by Opulo. Any derivative works should remove both marks. 12 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![Photon Firmware](photon-firmware.png) 2 | 3 | Photon is Open-Source firmware for pick and place feeders. 4 | 5 | A feeder is a small machine that moves component tape very small, precise increments in order to automatically serve up new components to a pick and place machine. 6 | 7 | Photon was originally developed as part of the LumenPnP project, but it is designed to support many types of hardware. 8 | 9 | ## Picking a Release 10 | 11 | If you're looking to update to the latest stable release, use the release tagged `Latest`. Do NOT use a `Pre-Release` firmware, or one with `rc` in the name. We release lots of Release Candidate (denoted with an `rc` in the version name) firmware builds that are still being tested. If you're interested in testing the latest build, then the `rc` fimware is for you! 12 | 13 | ## Building and uploading 14 | 15 | We use [platformio] for building, testing, and uploading this firmware. Refer to their docs for basic instructions on using platfomio. 16 | 17 | The default `pio` environment is intended for use with a [Black Magic Probe][bmp] for uploading a debugging: 18 | 19 | ```sh 20 | pio run 21 | ``` 22 | 23 | However, if you just want to program the feeder over UART, such as with the FTDI USB UART bridge included with the LumenPnP, you can use: 24 | 25 | ```sh 26 | pio run -e photon-serial -t upload --upload-port /dev/tty... 27 | ``` 28 | 29 | 30 | [platformio]: http://platformio.org 31 | [bmp]: https://black-magic.org/index.html 32 | -------------------------------------------------------------------------------- /img/photon-packet.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/photonfirmware/photon/fd45e3b81123b62ba42b663bc7cdc090431e465d/img/photon-packet.png -------------------------------------------------------------------------------- /photon-firmware.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/photonfirmware/photon/fd45e3b81123b62ba42b663bc7cdc090431e465d/photon-firmware.png -------------------------------------------------------------------------------- /photon/.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .vscode/.browse.c_cpp.db* 3 | .vscode/c_cpp_properties.json 4 | .vscode/launch.json 5 | .vscode/ipch 6 | .travis.yml 7 | .vscode 8 | include 9 | lib 10 | -------------------------------------------------------------------------------- /photon/platformio.ini: -------------------------------------------------------------------------------- 1 | ; PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | 11 | [platformio] 12 | default_envs = photon-bmp 13 | 14 | [env] 15 | platform = ststm32 16 | framework = arduino 17 | board = nucleo_f031k6 18 | lib_deps = 19 | ricaun/ArduinoUniqueID@^1.1.0 20 | mathertel/RotaryEncoder@^1.5.3 21 | mike-matera/FastPID@^1.3.1 22 | paulstoffregen/OneWire@^2.3.7 23 | jnesselr/RS485@^0.0.9 24 | build_flags = -Os -ggdb2 25 | debug_build_flags = ${build_flags} 26 | test_ignore = test_desktop 27 | check_tool = cppcheck 28 | check_flags = cppcheck: -j 4 29 | check_src_filters = src/* 30 | check_skip_packages = yes 31 | 32 | [env:stm32f031k6t6] 33 | board_build.mcu = stm32f031k6t6 34 | 35 | [env:photon-bmp] 36 | ; For programming and debugging via a BlackMagicProbe 37 | extends = env:stm32f031k6t6 38 | monitor_speed = 115200 39 | upload_protocol = blackmagic 40 | debug_tool = blackmagic 41 | build_flags = 42 | -D VERSION_STRING=\"${sysenv.VERSION_STRING}\" 43 | 44 | [env:photon-serial] 45 | ; For programming feeders via the FTDI USB -> Serial tool included in shipped LumenPnPs. 46 | extends = env:stm32f031k6t6 47 | upload_protocol = serial 48 | upload_speed = 9600 49 | -------------------------------------------------------------------------------- /photon/src/FeederFloor.cpp: -------------------------------------------------------------------------------- 1 | #include "FeederFloor.h" 2 | 3 | FeederFloor::FeederFloor(OneWire* oneWire) : _oneWire(oneWire) { 4 | 5 | } 6 | 7 | uint8_t FeederFloor::read_floor_address() { 8 | // reset the 1-wire line, and return false if no chip detected 9 | if(!_oneWire->reset()) { 10 | return 0xFF; 11 | } 12 | 13 | // Send 0x3C to indicate skipping the ROM selection step; there'll only ever be one ROM on the bus 14 | _oneWire->skip(); 15 | 16 | // array with the commands to initiate a read, DS28E07 device expect 3 bytes to start a read: command,LSB&MSB adresses 17 | uint8_t leemem[3] = { 18 | 0xF0, 19 | 0x00, 20 | 0x00 21 | }; 22 | 23 | // sending those three bytes 24 | _oneWire->write_bytes(leemem, sizeof(leemem), 1); 25 | 26 | uint8_t addr = _oneWire->read(); // Start by reading our address byte 27 | 28 | // Read the next 31 bytes, discarding their value. Each page is 32 bytes so we need 32 read commands 29 | for (uint8_t i = 0; i < 31; i++) { 30 | _oneWire->read(); 31 | } 32 | 33 | // return the first byte from returned data 34 | return addr; 35 | } 36 | 37 | bool FeederFloor::write_floor_address(uint8_t address) { 38 | /* 39 | wriıte_floor_address() 40 | success returns programmed address byte 41 | failure returns 0xFF 42 | 43 | This function takes a byte as in input, and flashes it to address 0x0000 in the eeprom (where the floor ID is stored). 44 | The DS28E07 requires a million and one steps to make this happen. Reference the datasheet for details: 45 | https://datasheets.maximintegrated.com/en/ds/DS28E07.pdf 46 | */ 47 | 48 | 49 | byte i; // This is for the for loops 50 | //----- 51 | // Write To Scratchpad 52 | //----- 53 | 54 | byte data[8] = {address, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; 55 | 56 | // reset the 1-wire line, and return false if no chip detected 57 | if(!_oneWire->reset()){ 58 | return false; 59 | } 60 | 61 | // Send 0x3C to indicate skipping the ROM selection step; there'll only ever be one ROM on the bus 62 | _oneWire->skip(); 63 | 64 | // array with the commands to initiate a write to the scratchpad, DS28E07 device expect 3 bytes to start a read: command,LSB&MSB adresses 65 | byte leemem[3] = { 66 | 0x0F, 67 | 0x00, 68 | 0x00 69 | }; 70 | 71 | // sending those three bytes 72 | _oneWire->write_bytes(leemem, sizeof(leemem), 1); 73 | 74 | // Now it's time to actually write the data to the scratchpad 75 | for ( i = 0; i < 8; i++) { 76 | _oneWire->write(data[i], 1); 77 | } 78 | 79 | // read back the CRC 80 | //byte ccrc = _oneWire->read(); 81 | 82 | //----- 83 | // Read Scratchpad 84 | //----- 85 | 86 | // reset the 1-wire line, and return failure if no chip detected 87 | if(!_oneWire->reset()){ 88 | return false; 89 | } 90 | 91 | // Send 0x3C to indicate skipping the ROM selection step; there'll only ever be one ROM on the bus 92 | _oneWire->skip(); 93 | 94 | // send read scratchpad command 95 | _oneWire->write(0xAA, 1); 96 | 97 | // array for the data we'll read back 98 | byte read_data[11]; 99 | 100 | //read in TA1, TA2, and E/S bytes, then the 8 bytes of data 101 | for ( i = 0; i < sizeof(read_data); i++) { 102 | read_data[i] = _oneWire->read(); 103 | } 104 | 105 | #if 0 // TODO we should probably be checking the CRC 106 | // byte for the ccrc the eeprom will send us 107 | byte scratchpad_ccrc = _oneWire->read(); 108 | 109 | byte ccrc_calc = OneWire::crc8(read_data, sizeof(read_data)); 110 | 111 | // TODO need to be checking CCRC. never returns true, even when data is identical. 112 | // if(scratchpad_ccrc != ccrc_calc){ 113 | // // do nothing 114 | // } 115 | #endif 116 | 117 | //----- 118 | // Copy Scratchpad to Memory 119 | //----- 120 | 121 | // reset the 1-wire line, and return false if no chip detected 122 | if(!_oneWire->reset()){ 123 | return false; 124 | } 125 | 126 | // Send 0x3C to indicate skipping the ROM selection step; there'll only ever be one ROM on the bus 127 | _oneWire->skip(); 128 | 129 | // copy scratchpad command 130 | _oneWire->write(0x55, 1); 131 | 132 | // sending auth bytes from scratchpad read, which is the first 3 bytes 133 | _oneWire->write_bytes(read_data, 3, 1); 134 | 135 | // wait for programming, we'll get alternating 1s and 0s when done 136 | uint32_t timer = millis(); 137 | while(true){ 138 | if(_oneWire->read() == 0xAA){ 139 | break; 140 | } 141 | if( (millis() - timer) > 20 ){ // datasheet says it should only ever take 12ms at most to program 142 | break; 143 | } 144 | } 145 | 146 | // send reset 147 | if(!_oneWire->reset()){ 148 | return false; 149 | } 150 | 151 | // check the floor address by reading 152 | byte written_address = this->read_floor_address(); 153 | 154 | if(written_address == address) { 155 | //return new address 156 | return true; 157 | } 158 | 159 | return false; 160 | } -------------------------------------------------------------------------------- /photon/src/FeederFloor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | class FeederFloor { 6 | public: 7 | explicit FeederFloor(OneWire* oneWire); 8 | 9 | uint8_t read_floor_address(); 10 | bool write_floor_address(uint8_t address); 11 | 12 | private: 13 | OneWire* _oneWire; 14 | }; -------------------------------------------------------------------------------- /photon/src/PhotonFeeder.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "PhotonFeeder.h" 3 | #include "define.h" 4 | #include "versions.h" 5 | #include 6 | 7 | // -------------------- 8 | 9 | // Calculating Ticks per Tenth MM 10 | // gearbox has ratio of 1:1030 11 | // encoder has 14 ticks per revolution (7 per channel) 12 | // one full rotation of the output shaft is 14*1030 = 14420 ticks 13 | // divided by 32 teeth is 450.625 ticks per tooth 14 | // divided by 40 tenths of a mm per tooth (4mm) is 11.265625 ticks per tenth mm 15 | // 8.8 microns per tick 16 | 17 | // In reality, the gearbox has a slight deviation from the perfect 1:1030 gearing 18 | // Emperical testing brings our TICKS_PER_TENTH_MM to 11.273 19 | #define TICKS_PER_TENTH_MM 11.273 20 | #define THOUSANDTHS_TICKS_PER_TENTH_MM ((uint32_t)(TICKS_PER_TENTH_MM * 1000)) 21 | #define TENTH_MM_PER_PIP 40 22 | 23 | // ----------- 24 | // Thresholds 25 | // ----------- 26 | 27 | // number of ticks within requested tick position we should begin halting 28 | #define DRIVE_APPROACH_FINAL_TICKS 100 29 | // when moving backwards, how far further backwards past requested position to approach from the back 30 | #define BACKLASH_COMP_TENTH_MM 10 31 | 32 | // -------- 33 | // Timing 34 | // -------- 35 | 36 | // how long the peel motor will peel film per tenth mm of tape requested to be driven 37 | #define PEEL_TIME_PER_TENTH_MM 18 38 | // how long the peel motor will peel film during backwards movements per tenth mm of tape requested to be driven 39 | #define BACKWARDS_PEEL_TIME_PER_TENTH_MM 30 40 | // short amount of time peel motor moves backwards to reduce tension on film after peeling 41 | #define PEEL_BACKOFF_TIME 30 42 | // amount of time we allow for each tenth mm before timeout (in ms) 43 | #define TIMEOUT_TIME_PER_TENTH_MM 40 44 | // after driving backwards, how long do we peel to take up any potential slack in the film 45 | #define BACKWARDS_FEED_FILM_SLACK_REMOVAL_TIME 350 46 | 47 | // Unit Tests Fail Because This Isn't Defined In ArduinoFake for some reason 48 | #ifndef INPUT_ANALOG 49 | #define INPUT_ANALOG 0x04 50 | #endif 51 | 52 | PhotonFeeder::PhotonFeeder( 53 | uint8_t drive1_pin, 54 | uint8_t drive2_pin, 55 | uint8_t peel1_pin, 56 | uint8_t peel2_pin, 57 | uint8_t led_red, 58 | uint8_t led_green, 59 | uint8_t led_blue, 60 | RotaryEncoder* encoder 61 | ) : 62 | _drive1_pin(drive1_pin), 63 | _drive2_pin(drive2_pin), 64 | _peel1_pin(peel1_pin), 65 | _peel2_pin(peel2_pin), 66 | _led_red(led_red), 67 | _led_green(led_green), 68 | _led_blue(led_blue), 69 | _position(0), 70 | _encoder(encoder) { 71 | 72 | pinMode(_drive1_pin, OUTPUT); 73 | pinMode(_drive2_pin, OUTPUT); 74 | pinMode(_peel1_pin, OUTPUT); 75 | pinMode(_peel2_pin, OUTPUT); 76 | 77 | pinMode(_led_red, OUTPUT); 78 | pinMode(_led_green, OUTPUT); 79 | pinMode(_led_blue, OUTPUT); 80 | 81 | if(_version == ""){ 82 | _version = "debug"; 83 | } 84 | 85 | } 86 | 87 | //----------- 88 | // 89 | // TOOLS 90 | // 91 | //----------- 92 | 93 | PhotonFeeder::FeedResult PhotonFeeder::getMoveResult(){ 94 | return _lastFeedStatus; 95 | } 96 | 97 | uint16_t PhotonFeeder::calculateExpectedFeedTime(uint8_t distance, bool forward){ 98 | 99 | // This command is ONLY for generating a time that we send back to the host 100 | // calculating timeouts actually determining if we've failed a feed is separate. 101 | if(forward){ 102 | // we're calculating expected feed time of an _optimal_ forward feed command. this includes: 103 | // - peel forward time 104 | // - peel backoff time 105 | // - expected time to drive forward assuming one attempt 106 | if(_first_feed_since_load){ 107 | return (distance * PEEL_TIME_PER_TENTH_MM) + PEEL_BACKOFF_TIME + (distance * TIMEOUT_TIME_PER_TENTH_MM) + 1000; 108 | } 109 | else{ 110 | return (distance * PEEL_TIME_PER_TENTH_MM) + PEEL_BACKOFF_TIME + (distance * TIMEOUT_TIME_PER_TENTH_MM) + 200; 111 | } 112 | 113 | } 114 | else { 115 | // we're calculating expected feed time of an _optimal_ backward feed command. this includes: 116 | // - unpeeling film time to prep for backwards movement 117 | // - backwards movement including backlash distance 118 | // - remaining film slack takeup 119 | return (distance * BACKWARDS_PEEL_TIME_PER_TENTH_MM) + ((distance + (BACKLASH_COMP_TENTH_MM*2)) * TIMEOUT_TIME_PER_TENTH_MM) + BACKWARDS_FEED_FILM_SLACK_REMOVAL_TIME + 50; 120 | } 121 | } 122 | 123 | void PhotonFeeder::identify() { 124 | for(int i = 0;i<3;i++){ 125 | set_rgb(true, true, true); 126 | delay(300); 127 | set_rgb(false, false, false); 128 | delay(300); 129 | } 130 | 131 | } 132 | 133 | void PhotonFeeder::showVersion() { 134 | delay(500); 135 | 136 | if(_version.indexOf("debug") >= 0){ // flashing red, beta 137 | set_rgb(true, false, false); 138 | delay(250); 139 | set_rgb(false, false, false); 140 | delay(250); 141 | } 142 | 143 | else { // flashing green, v1.X.X 144 | 145 | set_rgb(false, true, false); 146 | delay(250); 147 | set_rgb(false, false, false); 148 | delay(250); 149 | 150 | } 151 | } 152 | 153 | void PhotonFeeder::vendorSpecific(uint8_t options[VENDOR_SPECIFIC_OPTIONS_LENGTH], uint8_t* response) { 154 | uint8_t command = options[0]; 155 | 156 | int len = _version.length(); 157 | 158 | // Commands 0x00 - 0x0F get the firmware version string in chunks. The firmware version string may be smaller than 159 | // the allotted 7 chunks, but it is null-terminated as expected. 160 | if(command <= 0x0F) { 161 | const auto start_index = min(size_t(command) * VENDOR_SPECIFIC_OPTIONS_LENGTH, _version.length()); 162 | const auto chunk = _version.c_str() + start_index; 163 | 164 | // NOTE: Using strncpy here specifically because we want its behavior where it handles encountering a null byte 165 | // in the src string. 166 | strncpy((char*)(response), chunk, VENDOR_SPECIFIC_OPTIONS_LENGTH); 167 | } 168 | 169 | switch(command){ 170 | case 0x10: 171 | uint8_t ledMask = options[1]; 172 | 173 | int blue = ledMask & 1; 174 | int green = (ledMask >> 1) & 1; 175 | int red = (ledMask >> 2) & 1; 176 | int set = (ledMask >> 3) & 1; 177 | 178 | if(set){ 179 | set_rgb(red, green, blue); 180 | } 181 | break; 182 | 183 | } 184 | 185 | return; 186 | 187 | 188 | } 189 | 190 | bool PhotonFeeder::checkLoaded() { 191 | /* checkLoaded() 192 | The checkLoaded() function checks to see what's loaded in the feeder, and sets drive methodology appropriately. 193 | This gets run on first movement command after boot, and after tape is driven through buttons 194 | */ 195 | 196 | //takes up any backlash slack, ensures any forward movement is tape movement 197 | analogWrite(_drive1_pin, 0); 198 | analogWrite(_drive2_pin, 250); 199 | delay(2); 200 | 201 | analogWrite(_drive1_pin, 0); 202 | analogWrite(_drive2_pin, 20); 203 | delay(100); 204 | 205 | halt(); 206 | 207 | // find starting threshold of movement 208 | int errorThreshold = 3; 209 | int movedAt = 256; 210 | signed long startingTick, currentTick; 211 | 212 | startingTick = _encoder->getPosition(); 213 | 214 | for(int movementIndex = 20; movementIndex<255; movementIndex=movementIndex + 5){ 215 | 216 | analogWrite(_drive1_pin, 0); 217 | analogWrite(_drive2_pin, movementIndex); 218 | 219 | delay(75); 220 | 221 | currentTick = _encoder->getPosition(); 222 | 223 | if(abs(startingTick - currentTick) > errorThreshold){ 224 | movedAt = movementIndex; 225 | break; 226 | } 227 | } 228 | 229 | halt(); 230 | 231 | if(movedAt > 140){ 232 | _beefy_boi = true; 233 | //set_rgb(true, false, false); 234 | //delay(200); 235 | } 236 | else{ 237 | _beefy_boi = false; 238 | //set_rgb(false, false, true); 239 | //delay(200); 240 | } 241 | 242 | return true; 243 | 244 | } 245 | 246 | void PhotonFeeder::resetEncoderPosition(uint16_t position){ 247 | _encoder->setPosition(position); 248 | } 249 | 250 | void PhotonFeeder::setMmPosition(uint16_t position){ 251 | _position = position; 252 | } 253 | 254 | void PhotonFeeder::set_rgb(bool red, bool green, bool blue) { 255 | digitalWrite(LED_R, ! red); 256 | digitalWrite(LED_G, ! green); 257 | digitalWrite(LED_B, ! blue); 258 | } 259 | 260 | //----------- 261 | // 262 | // MANUAL MOTOR CONTROL 263 | // 264 | //----------- 265 | 266 | void PhotonFeeder::peel(bool forward) { 267 | if(forward){ 268 | analogWrite(_peel1_pin, 255); 269 | analogWrite(_peel2_pin, 0); 270 | } 271 | else{ 272 | analogWrite(_peel1_pin, 0); 273 | analogWrite(_peel2_pin, 255); 274 | } 275 | } 276 | 277 | void PhotonFeeder::drive(bool forward){ 278 | 279 | if(forward){ 280 | analogWrite(_drive1_pin, 0); 281 | analogWrite(_drive2_pin, 255); 282 | } 283 | else{ 284 | analogWrite(_drive1_pin, 255); 285 | analogWrite(_drive2_pin, 0); 286 | } 287 | } 288 | 289 | void PhotonFeeder::driveValue(bool forward, uint8_t value){ 290 | if(forward){ 291 | analogWrite(_drive1_pin, 0); 292 | analogWrite(_drive2_pin, value); 293 | } 294 | else{ 295 | analogWrite(_drive1_pin, value); 296 | analogWrite(_drive2_pin, 0); 297 | } 298 | } 299 | 300 | void PhotonFeeder::driveBrakeValue(bool forward, uint8_t value){ 301 | if(forward){ 302 | analogWrite(_drive1_pin, 255-value); 303 | analogWrite(_drive2_pin, 255); 304 | } 305 | else{ 306 | analogWrite(_drive1_pin, 255); 307 | analogWrite(_drive2_pin, 255-value); 308 | } 309 | } 310 | 311 | void PhotonFeeder::brakeDrive(){ 312 | //brings both drive pins high 313 | analogWrite(_drive1_pin, 255); 314 | analogWrite(_drive2_pin, 255); 315 | } 316 | 317 | void PhotonFeeder::brakePeel(){ 318 | //brings both drive pins high 319 | analogWrite(_peel1_pin, 255); 320 | analogWrite(_peel2_pin, 255); 321 | } 322 | 323 | void PhotonFeeder::halt(){ 324 | brakeDrive(); 325 | brakePeel(); 326 | } 327 | 328 | //----------- 329 | // 330 | // FEEDING 331 | // 332 | //----------- 333 | 334 | void PhotonFeeder::feedDistance(uint16_t tenths_mm, bool forward) { 335 | 336 | if (_first_feed_since_load){ 337 | checkLoaded(); 338 | _first_feed_since_load = false; 339 | } 340 | 341 | set_rgb(true, true, true); 342 | 343 | bool success = (forward) ? moveForward(tenths_mm) : moveBackward(tenths_mm); 344 | 345 | if(success){ 346 | set_rgb(false, false, false); 347 | } 348 | else{ 349 | set_rgb(true, false, false); 350 | } 351 | 352 | } 353 | 354 | bool PhotonFeeder::moveForward(uint16_t tenths_mm) { 355 | int retry_index = 0; 356 | // First, ensure everything is stopped 357 | halt(); 358 | 359 | // segment our total distance into pip chunks 360 | div_t result = div(tenths_mm, TENTH_MM_PER_PIP); 361 | 362 | // quot is the number of 1 pip loops we run 363 | for(int i = 0;i _retry_limit){ 375 | _lastFeedStatus = PhotonFeeder::FeedResult::COULDNT_REACH; 376 | return false; 377 | } 378 | drive(false); 379 | delay(50); 380 | halt(); 381 | if(moveForwardSequence(40, false)){ 382 | break; 383 | } 384 | } 385 | } 386 | } 387 | 388 | // rem is the value we drive in a last loop to move any remaining distance less than a pip 389 | if(result.rem > 0){ 390 | //move forward result.rem 391 | if(!moveForwardSequence(result.rem, true)){ // if it fails, try again with a fresh pulse of power after moving the motor back a bit. 392 | while(true){ 393 | retry_index++; 394 | 395 | // if we're on our second to last attempt, we should absolutely be considered a beefy boi 396 | if(retry_index + 1 == _retry_limit){ 397 | _beefy_boi = true; 398 | } 399 | 400 | if(retry_index > _retry_limit){ 401 | _lastFeedStatus = PhotonFeeder::FeedResult::COULDNT_REACH; 402 | return false; 403 | } 404 | drive(false); 405 | delay(50); 406 | halt(); 407 | if(moveForwardSequence(result.rem, false)){ 408 | break; 409 | } 410 | } 411 | } 412 | } 413 | 414 | _lastFeedStatus = PhotonFeeder::FeedResult::SUCCESS; 415 | return true; 416 | 417 | } 418 | 419 | bool PhotonFeeder::moveBackward(uint16_t tenths_mm) { 420 | // First, ensure everything is stopped 421 | halt(); 422 | 423 | // Next, unspool some film to give the tape slack 424 | signed long peel_time = (tenths_mm * BACKWARDS_PEEL_TIME_PER_TENTH_MM); 425 | peel(false); 426 | delay(peel_time); 427 | brakePeel(); 428 | 429 | // move tape backward 430 | // first we overshoot by the backlash distance, then approach from the forward direction 431 | if (moveBackwardSequence(false, tenths_mm + BACKLASH_COMP_TENTH_MM)){ 432 | 433 | // if this is successful, we peel film to take up any slack 434 | peel(true); 435 | delay(BACKWARDS_FEED_FILM_SLACK_REMOVAL_TIME); 436 | brakePeel(); 437 | 438 | //then drive the last little bit 439 | if(moveBackwardSequence(true, BACKLASH_COMP_TENTH_MM)){ 440 | //peel again to take up any slack 441 | 442 | _lastFeedStatus = PhotonFeeder::FeedResult::SUCCESS; 443 | return true; 444 | } 445 | else{ 446 | _lastFeedStatus = PhotonFeeder::FeedResult::COULDNT_REACH; 447 | return false; 448 | } 449 | } 450 | else{ 451 | _lastFeedStatus = PhotonFeeder::FeedResult::COULDNT_REACH; 452 | return false; 453 | } 454 | } 455 | 456 | bool PhotonFeeder::moveForwardSequence(uint16_t tenths_mm, bool first_attempt) { 457 | /* moveForwardSequence() 458 | * This function actually handles the translation of the mm movement to driving the motor to the right position based on encoder ticks. 459 | 460 | This function should only be called in increments of tenths_mm. It contains all peeling and driving sequencing needed to get accurate 4 mm movements. 461 | 462 | * We can't just calculate the number of ticks we need to move for the given mm movement requested, and increment our tick count by that much. 463 | * Because the tick count is only ever an approximation of the precise mm position, any rounding done from the mm->tick conversion will 464 | * result in a significant amount of accrued error. 465 | * 466 | * Instead, we need to use the _mm_ position as ground truth, and only ever use the ticks as only how we command the control loop. We do this by 467 | * first finding the new requested position, then converting this to ticks _based on the startup 0 tick position_. This is similar to 468 | * absolute positioning vs. relative positioning in Marlin. Every mm->tick calculation needs to be done based on the initial 0 tick position. 469 | 470 | returns true if we reach position within timeout, returns false if we timeout. does NOT set _lastFeedStatus. 471 | * 472 | */ 473 | 474 | signed long goal_mm, timeout, signed_mm, current_tick; 475 | 476 | timeout = tenths_mm * TIMEOUT_TIME_PER_TENTH_MM; 477 | 478 | goal_mm = _position + tenths_mm; 479 | 480 | // calculating goal_tick based on absolute, not relative position 481 | 482 | // float goal_tick_precise_f = goal_mm * TICKS_PER_TENTH_MM; 483 | // volatile int goal_tick_f = round(goal_tick_precise_f); 484 | 485 | signed long goal_tick_precise = goal_mm * THOUSANDTHS_TICKS_PER_TENTH_MM; 486 | int goal_tick = 0; 487 | 488 | if (goal_tick_precise >= 0) { 489 | goal_tick = (goal_tick_precise + 500) / 1000; // round a positive number 490 | } else { 491 | goal_tick = (goal_tick_precise - 500) / 1000; // round a negative integer 492 | } 493 | 494 | int peel_delay = PEEL_TIME_PER_TENTH_MM * tenths_mm; 495 | 496 | // peel film for calculated time 497 | peel(true); 498 | delay(peel_delay); 499 | peel(false); 500 | delay(PEEL_BACKOFF_TIME); 501 | brakePeel(); 502 | 503 | // drive forward with ease in 504 | for(int i=150;i<255;i=i+3){ 505 | driveValue(true, i); 506 | delay(1); 507 | } 508 | 509 | //prepping variables for stall detection 510 | int tick_history[20] = {1, 100, 1, 100, 1, 100, 1, 100, 1, 100, 1, 100, 1, 100, 1, 100, 1, 100, 1, 100}; 511 | int tick_history_index = 0; 512 | int delta = 5; 513 | uint32_t stallCooldownTime = millis(); 514 | bool stallCooldown = false; 515 | uint32_t last_stall_position_sample_time = millis(); 516 | 517 | // setting start time for measuring timeout 518 | uint32_t start_time = millis(); 519 | 520 | // if it's not our first attempt, or it's thick tape, we drive full tilt 521 | // otherwise, we drive slow to start and ease our way up to the tick 522 | int currentDriveValue; 523 | if(first_attempt == false || _beefy_boi == true){ 524 | currentDriveValue = 255; 525 | } 526 | else { 527 | currentDriveValue = 30; 528 | } 529 | 530 | //volatile int test = 0; 531 | 532 | //monitor loop 533 | while(millis() < start_time + timeout + 20){ 534 | 535 | //getting encoder position 536 | current_tick = _encoder->getPosition(); 537 | 538 | //calculating error 539 | int error = goal_tick - current_tick; 540 | 541 | // if we're close enough it's time to ease in to final position 542 | if(error < DRIVE_APPROACH_FINAL_TICKS){ 543 | 544 | // updating tick history, calculate tick delta in last 20ms 545 | if(millis() > last_stall_position_sample_time + 1){ 546 | 547 | last_stall_position_sample_time = millis(); 548 | tick_history[tick_history_index] = current_tick; 549 | 550 | if(tick_history_index > 18){ 551 | tick_history_index = 0; 552 | } 553 | else{ 554 | tick_history_index++; 555 | } 556 | 557 | int max = *std::max_element(tick_history, tick_history + 20); 558 | int min = *std::min_element(tick_history, tick_history + 20); 559 | delta = max - min; 560 | 561 | } 562 | 563 | // reset stallcooldown if it's been a minute, motor has had time to react to new drive value 564 | if(stallCooldownTime + 5 < millis()){ 565 | stallCooldown = false; 566 | } 567 | 568 | // if stall detected, and it's been a minute since we've adjusted currentDriveValue, increase currentDriveValue 569 | if(delta <= 5 && stallCooldown == false){ 570 | 571 | currentDriveValue = currentDriveValue + 3; 572 | if(currentDriveValue > 255){ 573 | currentDriveValue = 255; 574 | } 575 | 576 | stallCooldown = true; 577 | stallCooldownTime = millis(); 578 | 579 | } 580 | 581 | //driving calculated value 582 | driveValue(true, currentDriveValue); 583 | 584 | //we've reached the final position! 585 | if(error < 1){ 586 | 587 | //immediately stop 588 | brakeDrive(); 589 | 590 | // int brakeTick = _encoder->getPosition(); 591 | 592 | // capture time at ss settle start 593 | uint32_t ssStartTime = millis(); 594 | 595 | // sample ticks until we've hit steady state 596 | while (delta > 0 && ssStartTime + 200 > millis()){ 597 | 598 | //watching for steady state ticks 599 | if(millis() > last_stall_position_sample_time + 3){ 600 | 601 | //getting encoder position 602 | current_tick = _encoder->getPosition(); 603 | 604 | last_stall_position_sample_time = millis(); 605 | tick_history[tick_history_index] = current_tick; 606 | 607 | if(tick_history_index > 18){ 608 | tick_history_index = 0; 609 | } 610 | else{ 611 | tick_history_index++; 612 | } 613 | 614 | int max = *std::max_element(tick_history, tick_history + 20); 615 | int min = *std::min_element(tick_history, tick_history + 20); 616 | delta = max-min; 617 | 618 | } 619 | } 620 | 621 | // int ssTick = _encoder->getPosition(); 622 | // volatile int coast = ssTick - brakeTick; 623 | 624 | // updating internal position to the goal position because we reached it 625 | _position = goal_mm; 626 | 627 | // Resetting internal position count so we dont creep up into our 2,147,483,647 limit on the variable 628 | // We can only do this when the exact tick we move to is a whole number so we don't accrue any drift 629 | if(goal_tick_precise == goal_tick * 1000){ 630 | resetEncoderPosition(_encoder->getPosition() - goal_tick); 631 | setMmPosition(0); 632 | } 633 | 634 | return true; 635 | 636 | } 637 | } 638 | 639 | // still far from final, just drive full force 640 | else{ 641 | drive(true); 642 | } 643 | } 644 | // brake to kill any coast 645 | halt(); 646 | 647 | return false; 648 | } 649 | 650 | bool PhotonFeeder::moveBackwardSequence(bool forward, uint16_t tenths_mm) { 651 | /* moveInternal() 652 | * This function actually handles the translation of the mm movement to driving the motor to the right position based on encoder ticks. 653 | * We can't just calculate the number of ticks we need to move for the given mm movement requested, and increment our tick count by that much. 654 | * Because the tick count is only ever an approximation of the precise mm position, any rounding done from the mm->tick conversion will 655 | * result in a signficiant amount of accrued error. 656 | * 657 | * Instead, we need to use the _mm_ position as ground truth, and only ever use the ticks as only how we command the PID loop. We do this by 658 | * first finding the new requested position, then converting this to ticks _based on the startup 0 tick position_. This is similar to 659 | * absolute positioning vs. relative positioning in Marlin. Every mm->tick calculation needs to be done based on the initial 0 tick position. 660 | * 661 | */ 662 | 663 | signed long goal_mm, timeout, signed_mm, current_tick; 664 | 665 | timeout = tenths_mm * TIMEOUT_TIME_PER_TENTH_MM; 666 | 667 | // doing final position math 668 | if(forward){ 669 | goal_mm = _position + tenths_mm; 670 | } 671 | else{ 672 | goal_mm = _position - tenths_mm; 673 | } 674 | 675 | // calculating goal_tick based on absolute, not relative position 676 | // float goal_tick_precise = goal_mm * TICKS_PER_TENTH_MM; 677 | // int goal_tick = round(goal_tick_precise); 678 | 679 | long goal_tick_precise = goal_mm * THOUSANDTHS_TICKS_PER_TENTH_MM; 680 | int goal_tick = 0; 681 | 682 | if (goal_tick_precise > 0) { 683 | goal_tick = (goal_tick_precise + 500) / 1000; // round a positive number 684 | } else { 685 | goal_tick = (goal_tick_precise - 500) / 1000; // round a negative integer 686 | } 687 | 688 | uint32_t start_time = millis(); 689 | 690 | // in the direction of the goal with ease in 691 | for(size_t i=0; i<255; i=i+5){ 692 | driveValue(forward, i); 693 | delay(1); 694 | } 695 | drive(forward); 696 | 697 | while(millis() < start_time + timeout + 20){ 698 | 699 | current_tick = _encoder->getPosition(); 700 | int error; 701 | 702 | if(forward){ 703 | error = goal_tick - current_tick; 704 | } 705 | else{ 706 | error = current_tick - goal_tick; 707 | } 708 | if (error < 0){ 709 | brakeDrive(); 710 | _position = goal_mm; 711 | 712 | //delay to let position settle 713 | delay(50); 714 | 715 | // Resetting internal position count so we dont creep up into our 2,147,483,647 limit on the variable 716 | // We can only do this when the exact tick we move to is a whole number so we don't accrue any drift 717 | if(goal_tick_precise == goal_tick * 1000){ 718 | resetEncoderPosition(_encoder->getPosition() - goal_tick); 719 | setMmPosition(0); 720 | } 721 | 722 | return true; 723 | } 724 | 725 | } 726 | // brake to kill any coast 727 | brakeDrive(); 728 | 729 | return false; 730 | } 731 | -------------------------------------------------------------------------------- /photon/src/PhotonFeeder.h: -------------------------------------------------------------------------------- 1 | #ifndef _PHOTON_FEEDER_H 2 | #define _PHOTON_FEEDER_H 3 | 4 | #include 5 | 6 | #ifndef MOTOR_DEPS 7 | #define MOTOR_DEPS 8 | #include 9 | #endif 10 | 11 | 12 | #define VENDOR_SPECIFIC_OPTIONS_LENGTH 20 // Chosen by fair d20 roll 13 | 14 | 15 | class PhotonFeeder { 16 | public: 17 | enum FeedResult { 18 | SUCCESS, 19 | INVALID_LENGTH, 20 | COULDNT_REACH, 21 | UNKNOWN_ERROR 22 | }; 23 | 24 | PhotonFeeder( 25 | uint8_t drive1_pin, 26 | uint8_t drive2_pin, 27 | uint8_t peel1_pin, 28 | uint8_t peel2_pin, 29 | uint8_t led_red, 30 | uint8_t led_green, 31 | uint8_t led_blue, 32 | RotaryEncoder* encoder 33 | ); 34 | 35 | FeedResult getMoveResult(); 36 | 37 | // Async Functions 38 | void peel(bool forward); 39 | void drive(bool forward); 40 | void driveValue(bool forward, uint8_t value); 41 | void driveBrakeValue(bool forward, uint8_t value); 42 | void brakePeel(); 43 | void brakeDrive(); 44 | void halt(); 45 | void set_rgb(bool red, bool green, bool blue); 46 | uint16_t calculateExpectedFeedTime(uint8_t distance, bool forward); 47 | void setMmPosition(uint16_t position); 48 | void resetEncoderPosition(uint16_t position); 49 | 50 | // Blocking Functions 51 | void feedDistance(uint16_t tenths_mm, bool forward); 52 | bool checkLoaded(); 53 | 54 | // Vendor Specific Functions 55 | // Should probably be virtual with hardware overriding it as well as return status 56 | void vendorSpecific(uint8_t options[VENDOR_SPECIFIC_OPTIONS_LENGTH], uint8_t* response); 57 | void identify(); 58 | void showVersion(); 59 | 60 | bool _first_feed_since_load = true; 61 | 62 | private: 63 | uint8_t _drive1_pin; 64 | uint8_t _drive2_pin; 65 | 66 | uint8_t _peel1_pin; 67 | uint8_t _peel2_pin; 68 | 69 | uint8_t _led_red; 70 | uint8_t _led_green; 71 | uint8_t _led_blue; 72 | 73 | uint8_t _retry_limit = 3; 74 | 75 | String _version = VERSION_STRING; 76 | 77 | bool _beefy_boi = false; 78 | // flag for if we should just drive full tilt 79 | // set when thick tape is detected 80 | // reset when tape is driven fast through buttons (likely swapping tape) 81 | 82 | FeedResult _lastFeedStatus; 83 | 84 | signed long _position; 85 | 86 | RotaryEncoder* _encoder; 87 | 88 | uint8_t _firmware_version = 2; 89 | 90 | bool moveForward(uint16_t tenths_mm); 91 | bool moveBackward(uint16_t tenths_mm); 92 | bool moveForwardSequence(uint16_t tenths_mm, bool first_attempt); 93 | bool moveBackwardSequence(bool forward, uint16_t tenths_mm); 94 | }; 95 | 96 | #endif //_PHOTON_FEEDER_H -------------------------------------------------------------------------------- /photon/src/PhotonFeederProtocol.cpp: -------------------------------------------------------------------------------- 1 | #include "PhotonFeederProtocol.h" 2 | #include "PhotonNetworkLayer.h" 3 | 4 | #include "rs485/protocols/checksums/crc8_107.h" 5 | 6 | #define MAX_PROTOCOL_VERSION 1 7 | 8 | typedef enum { 9 | STATUS_OK = 0x00, 10 | STATUS_WRONG_FEEDER_ID = 0x01, 11 | STATUS_COULDNT_REACH = 0x02, 12 | STATUS_UNINITIALIZED_FEEDER = 0x03, 13 | STATUS_FEEDING_IN_PROGRESS = 0x04, 14 | STATUS_FAIL = 0x05, 15 | 16 | STATUS_TIMEOUT = 0xFE, 17 | STATUS_UNKNOWN_ERROR = 0xFF 18 | } FeederStatus; 19 | 20 | typedef enum { 21 | // Unicast Commands 22 | GET_FEEDER_ID = 0x01, 23 | INITIALIZE_FEEDER = 0x02, 24 | GET_VERSION = 0x03, 25 | MOVE_FEED_FORWARD = 0x04, 26 | MOVE_FEED_BACKWARD = 0x05, 27 | MOVE_FEED_STATUS = 0x06, 28 | 29 | VENDOR_OPTIONS = 0xbf, 30 | 31 | // Broadcast Commands 32 | GET_FEEDER_ADDRESS = 0xc0, 33 | IDENTIFY_FEEDER = 0xc1, 34 | PROGRAM_FEEDER_FLOOR = 0xc2, 35 | UNINITIALIZED_FEEDERS_RESPOND = 0xc3 36 | // EXTENDED_COMMAND = 0xff, Unused, reserved for future use 37 | } FeederCommand; 38 | 39 | PhotonFeederProtocol::PhotonFeederProtocol( 40 | PhotonFeeder *feeder, 41 | FeederFloor *feederFloor, 42 | PhotonNetworkLayer* network, 43 | const uint8_t *uuid, 44 | size_t uuid_length): 45 | _feeder(feeder), 46 | _feederFloor(feederFloor), 47 | _network(network), 48 | _initialized(false) { 49 | memset(_uuid, 0, UUID_LENGTH); 50 | memcpy(_uuid, uuid, (uuid_length < UUID_LENGTH) ? uuid_length : UUID_LENGTH); 51 | } 52 | 53 | void PhotonFeederProtocol::tick() { 54 | bool hasPacket = _network->getPacket(commandBuffer, sizeof(commandBuffer)); 55 | 56 | if(! hasPacket) { 57 | return; 58 | } 59 | 60 | switch(command.commandId) { 61 | //switch(packetBuffer.packet.payload.command.commandId) { 62 | case GET_FEEDER_ID: 63 | handleGetFeederId(); 64 | break; 65 | case INITIALIZE_FEEDER: 66 | handleInitializeFeeder(); 67 | break; 68 | case GET_VERSION: 69 | handleGetVersion(); 70 | break; 71 | case MOVE_FEED_FORWARD: 72 | handleMoveFeedForward(); 73 | break; 74 | case MOVE_FEED_BACKWARD: 75 | handleMoveFeedBackward(); 76 | break; 77 | case MOVE_FEED_STATUS: 78 | handleMoveFeedStatus(); 79 | break; 80 | case VENDOR_OPTIONS: 81 | handleVendorOptions(); 82 | break; 83 | case GET_FEEDER_ADDRESS: 84 | handleGetFeederAddress(); 85 | break; 86 | case IDENTIFY_FEEDER: 87 | handleIdentifyFeeder(); 88 | break; 89 | case PROGRAM_FEEDER_FLOOR: 90 | handleProgramFeederFloor(); 91 | break; 92 | case UNINITIALIZED_FEEDERS_RESPOND: 93 | handleUnitializedFeedersRespond(); 94 | default: 95 | // Something has gone wrong if execution ever gets here. 96 | break; 97 | } 98 | } 99 | 100 | bool PhotonFeederProtocol::isInitialized() { 101 | return _initialized; 102 | } 103 | 104 | bool PhotonFeederProtocol::guardInitialized() { 105 | // Checks if the feeder is initialized and returns an error if it hasn't been 106 | if (_initialized) { 107 | return true; 108 | } 109 | 110 | response = { 111 | .status = STATUS_UNINITIALIZED_FEEDER, 112 | }; 113 | memcpy(response.initializeFeeder.uuid, _uuid, UUID_LENGTH); 114 | 115 | transmitResponse(sizeof(InitializeFeederResponse)); 116 | 117 | return false; 118 | } 119 | 120 | 121 | void PhotonFeederProtocol::handleGetFeederId() { 122 | response = { 123 | .status = STATUS_OK 124 | }; 125 | memcpy(response.getFeederId.uuid, _uuid, UUID_LENGTH); 126 | 127 | transmitResponse(sizeof(GetFeederIdResponse)); 128 | } 129 | 130 | void PhotonFeederProtocol::handleInitializeFeeder() { 131 | // Check uuid is correct, if not return a Wrong Feeder UUID error 132 | bool requestedUUIDMatchesMine = memcmp(command.initializeFeeder.uuid, _uuid, UUID_LENGTH) == 0; 133 | if (! requestedUUIDMatchesMine) { 134 | response = { 135 | .status = STATUS_WRONG_FEEDER_ID, 136 | }; 137 | memcpy(response.initializeFeeder.uuid, _uuid, UUID_LENGTH); 138 | 139 | transmitResponse(sizeof(InitializeFeederResponse)); 140 | return; 141 | } 142 | 143 | // Mark this feeder as initialized 144 | _initialized = true; 145 | 146 | response = { 147 | .status = STATUS_OK, 148 | }; 149 | memcpy(response.initializeFeeder.uuid, _uuid, UUID_LENGTH); 150 | 151 | transmitResponse(sizeof(InitializeFeederResponse)); 152 | } 153 | 154 | void PhotonFeederProtocol::handleGetVersion() { 155 | response = { 156 | .status = STATUS_OK, 157 | .protocolVersion = { 158 | .version = MAX_PROTOCOL_VERSION, 159 | }, 160 | }; 161 | 162 | transmitResponse(sizeof(GetProtocolVersionResponse)); 163 | } 164 | 165 | void PhotonFeederProtocol::move(uint8_t distance, bool forward) { 166 | if (! guardInitialized()) { 167 | return; 168 | } 169 | 170 | uint16_t time = _feeder->calculateExpectedFeedTime(distance, forward); 171 | uint16_t timeMSB = (time >> 8) | (time << 8); 172 | 173 | response = { 174 | .status = STATUS_OK, 175 | .expectedTimeToFeed = { 176 | .expectedFeedTime = timeMSB, 177 | }, 178 | }; 179 | 180 | transmitResponse(sizeof(FeedDistanceResponse)); 181 | 182 | // perform a blocking movement 183 | _feeder->feedDistance(distance, forward); 184 | 185 | // clear the serial buffer to remove any packets that came in while we were blocking 186 | _network->clearPackets(); 187 | 188 | } 189 | 190 | void PhotonFeederProtocol::handleMoveFeedForward() { 191 | move(command.move.distance, true); 192 | 193 | } 194 | 195 | void PhotonFeederProtocol::handleMoveFeedBackward() { 196 | move(command.move.distance, false); 197 | 198 | } 199 | 200 | void PhotonFeederProtocol::handleMoveFeedStatus() { 201 | PhotonFeeder::FeedResult result = _feeder->getMoveResult(); 202 | uint8_t moveResponseStatus; 203 | 204 | switch (result) 205 | { 206 | case PhotonFeeder::FeedResult::SUCCESS: 207 | moveResponseStatus = STATUS_OK; 208 | break; 209 | 210 | case PhotonFeeder::FeedResult::INVALID_LENGTH: // For Now Handle Invalid Length & Motor Fault The Same 211 | case PhotonFeeder::FeedResult::COULDNT_REACH: 212 | moveResponseStatus = STATUS_COULDNT_REACH; 213 | break; 214 | 215 | case PhotonFeeder::FeedResult::UNKNOWN_ERROR: 216 | default: 217 | moveResponseStatus = STATUS_UNKNOWN_ERROR; 218 | break; 219 | } 220 | 221 | response = { 222 | .status = moveResponseStatus, 223 | }; 224 | 225 | transmitResponse(); 226 | } 227 | 228 | void PhotonFeederProtocol::handleGetFeederAddress() { 229 | // Check For Feeder Match 230 | bool requestedUUIDMatchesMine = memcmp(command.initializeFeeder.uuid, _uuid, UUID_LENGTH) == 0; 231 | if (! requestedUUIDMatchesMine) { 232 | return; 233 | } 234 | 235 | response = { 236 | .status = STATUS_OK, 237 | }; 238 | 239 | 240 | 241 | transmitResponse(); 242 | } 243 | 244 | void PhotonFeederProtocol::handleVendorOptions() { 245 | if (! guardInitialized()) { 246 | return; 247 | } 248 | response = { 249 | .status = STATUS_OK, 250 | .vendorOptions = VendorOptionsResponse(), 251 | }; 252 | 253 | _feeder->vendorSpecific(command.vendorOptions.options, response.vendorOptions.options); 254 | 255 | transmitResponse(sizeof(VendorOptionsResponse)); 256 | } 257 | 258 | void PhotonFeederProtocol::handleIdentifyFeeder() { 259 | 260 | // Check For Feeder Match 261 | bool requestedUUIDMatchesMine = memcmp(command.identifyFeeder.uuid, _uuid, UUID_LENGTH) == 0; 262 | if (! requestedUUIDMatchesMine) { 263 | return; 264 | } 265 | 266 | response = { 267 | .status = STATUS_OK, 268 | }; 269 | 270 | transmitResponse(); 271 | 272 | _feeder->identify(); 273 | 274 | } 275 | 276 | void PhotonFeederProtocol::handleProgramFeederFloor() { 277 | bool addressWritten = _feederFloor->write_floor_address(command.programFeederFloorAddress.address); 278 | 279 | if(addressWritten){ 280 | _network->setLocalAddress(command.programFeederFloorAddress.address); 281 | } 282 | 283 | uint8_t feederStatus = addressWritten ? STATUS_OK : STATUS_FAIL; 284 | response = { 285 | .status = feederStatus, 286 | }; 287 | 288 | transmitResponse(); 289 | } 290 | 291 | void PhotonFeederProtocol::handleUnitializedFeedersRespond() { 292 | if (_initialized) { 293 | return; // Don't respond to this command at all since we've already been initialized 294 | } 295 | 296 | response = { 297 | .status = STATUS_OK, 298 | }; 299 | 300 | memcpy(response.getFeederId.uuid, _uuid, UUID_LENGTH); 301 | 302 | transmitResponse(sizeof(GetFeederIdResponse)); 303 | } 304 | 305 | void PhotonFeederProtocol::transmitResponse(uint8_t responseSize) { 306 | // responseSize is only the anonymous uninion in the response, not any variables common to all responses 307 | // We handle common values here. 308 | responseSize++; // uint8_t status 309 | 310 | response.header = { 311 | .toAddress = PHOTON_NETWORK_CONTROLLER_ADDRESS, 312 | .fromAddress = _network->getLocalAddress(), 313 | .packetId = command.header.packetId, 314 | .payloadLength = responseSize, 315 | }; 316 | 317 | CRC8_107 crc; 318 | 319 | crc.add(response.header.toAddress); 320 | crc.add(response.header.fromAddress); 321 | crc.add(response.header.packetId); 322 | crc.add(response.header.payloadLength); 323 | 324 | for(uint8_t i = 0; itransmitPacket(responseBuffer, totalPacketLength); 332 | } -------------------------------------------------------------------------------- /photon/src/PhotonFeederProtocol.h: -------------------------------------------------------------------------------- 1 | #ifndef _PHOTON_FEEDER_PROTOCOL_H 2 | #define _PHOTON_FEEDER_PROTOCOL_H 3 | 4 | #include "FeederFloor.h" 5 | #include "PhotonFeeder.h" 6 | #include "PhotonNetworkLayer.h" 7 | 8 | #define UUID_LENGTH 12 9 | 10 | #define PHOTON_NETWORK_CONTROLLER_ADDRESS 0x00 11 | #define PHOTON_NETWORK_BROADCAST_ADDRESS 0xFF 12 | 13 | #define PACKED __attribute__ ((packed)) 14 | 15 | struct PACKED PhotonPacketHeader { 16 | uint8_t toAddress; 17 | uint8_t fromAddress; 18 | uint8_t packetId; 19 | uint8_t payloadLength; 20 | uint8_t crc; 21 | }; 22 | 23 | struct PACKED MoveCommand { 24 | uint8_t distance; 25 | }; 26 | 27 | struct PACKED GetFeederAddressCommand { 28 | uint8_t uuid[UUID_LENGTH]; 29 | }; 30 | 31 | struct PACKED InitializeFeederCommand { 32 | uint8_t uuid[UUID_LENGTH]; 33 | }; 34 | 35 | struct PACKED VendorOptionsCommand { 36 | uint8_t options[VENDOR_SPECIFIC_OPTIONS_LENGTH]; 37 | }; 38 | 39 | struct PACKED ProgramFeederFloorAddressCommand { 40 | uint8_t uuid[UUID_LENGTH]; 41 | uint8_t address; 42 | }; 43 | 44 | struct PACKED IdentifyFeederCommand { 45 | uint8_t uuid[UUID_LENGTH]; 46 | }; 47 | 48 | struct PACKED PhotonCommand { 49 | PhotonPacketHeader header; 50 | uint8_t commandId; 51 | union { 52 | MoveCommand move; 53 | GetFeederAddressCommand getFeederAddress; 54 | InitializeFeederCommand initializeFeeder; 55 | VendorOptionsCommand vendorOptions; 56 | ProgramFeederFloorAddressCommand programFeederFloorAddress; 57 | IdentifyFeederCommand identifyFeeder; 58 | }; 59 | }; 60 | 61 | struct PACKED GetFeederIdResponse { 62 | uint8_t uuid[UUID_LENGTH]; 63 | }; 64 | 65 | struct PACKED InitializeFeederResponse { 66 | uint8_t uuid[UUID_LENGTH]; 67 | }; 68 | 69 | struct PACKED GetProtocolVersionResponse { 70 | uint8_t version; 71 | }; 72 | 73 | struct PACKED FeedDistanceResponse { 74 | uint16_t expectedFeedTime; 75 | }; 76 | 77 | struct PACKED VendorOptionsResponse { 78 | uint8_t options[VENDOR_SPECIFIC_OPTIONS_LENGTH]; 79 | }; 80 | 81 | struct PACKED PhotonResponse { 82 | PhotonPacketHeader header; 83 | uint8_t status; 84 | union { 85 | GetFeederIdResponse getFeederId; 86 | InitializeFeederResponse initializeFeeder; 87 | GetProtocolVersionResponse protocolVersion; 88 | FeedDistanceResponse expectedTimeToFeed; 89 | VendorOptionsResponse vendorOptions; 90 | }; 91 | }; 92 | 93 | class PhotonFeederProtocol { 94 | 95 | public: 96 | PhotonFeederProtocol( 97 | PhotonFeeder *feeder, 98 | FeederFloor *feederFloor, 99 | PhotonNetworkLayer* network, 100 | const uint8_t *uuid, 101 | size_t uuid_length 102 | ); 103 | void tick(); 104 | 105 | private: 106 | PhotonFeeder* _feeder; 107 | FeederFloor* _feederFloor; 108 | PhotonNetworkLayer* _network; 109 | uint8_t _uuid[UUID_LENGTH]; 110 | bool _initialized; 111 | 112 | union { 113 | PhotonCommand command; 114 | uint8_t commandBuffer[sizeof(PhotonCommand)]; 115 | }; 116 | 117 | union { 118 | PhotonResponse response; 119 | uint8_t responseBuffer[sizeof(PhotonResponse)]; 120 | }; 121 | 122 | // Unicast 123 | bool guardInitialized(); 124 | void handleGetFeederId(); 125 | void handleInitializeFeeder(); 126 | void handleGetVersion(); 127 | void handleMoveFeedForward(); 128 | void handleMoveFeedBackward(); 129 | void handleMoveFeedStatus(); 130 | void handleVendorOptions(); 131 | 132 | // Broadcast 133 | void handleGetFeederAddress(); 134 | void handleIdentifyFeeder(); 135 | void handleProgramFeederFloor(); 136 | void handleUnitializedFeedersRespond(); 137 | 138 | void move(uint8_t distance, bool forward); 139 | bool isInitialized(); 140 | 141 | void transmitResponse(uint8_t responseSize = 0); 142 | }; 143 | 144 | #endif //_PHOTON_FEEDER_PROTOCOL_H -------------------------------------------------------------------------------- /photon/src/PhotonNetworkLayer.cpp: -------------------------------------------------------------------------------- 1 | #include "define.h" 2 | 3 | #include "PhotonNetworkLayer.h" 4 | #include "FeederFloor.h" 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | #ifndef NATIVE 11 | #include "util.h" 12 | #endif 13 | 14 | #define PHOTON_PROTOCOL_DEFAULT_TIMEOUT_MS 100 15 | #define PHOTON_INCOMING_BUFFER_SIZE 16 16 | #define RS485_CONTROL_DELAY 10 17 | 18 | PhotonNetworkLayer::PhotonNetworkLayer( 19 | RS485Bus* bus, 20 | Packetizer* packetizer, 21 | FilterByValue* addressFilter, 22 | FeederFloor* feederFloor) : 23 | _bus(bus), 24 | _packetizer(packetizer), 25 | _addressFilter(addressFilter), 26 | _feederFloor(feederFloor), 27 | _local_address(0xFF) { 28 | 29 | _local_address = _feederFloor->read_floor_address(); 30 | _packetizer->setFilter(*_addressFilter); 31 | _packetizer->setFalsePacketVerificationTimeout(10000); 32 | _packetizer->setMaxReadTimeout(50000); 33 | _addressFilter->preValues.allowAll(); 34 | _addressFilter->postValues.allow(0xFF); 35 | 36 | // If floor address isn't set, _local_address is 0xFF and this function does nothing 37 | _addressFilter->postValues.allow(_local_address); 38 | } 39 | 40 | void PhotonNetworkLayer::setLocalAddress(uint8_t address) { 41 | if(_local_address != 0xff) { 42 | // The old address is no longer valid 43 | _addressFilter->postValues.reject(_local_address); 44 | } 45 | 46 | _local_address = address; 47 | _addressFilter->postValues.allow(_local_address); 48 | } 49 | 50 | uint8_t PhotonNetworkLayer::getLocalAddress() { 51 | return _local_address; 52 | } 53 | 54 | bool PhotonNetworkLayer::getPacket(uint8_t* buffer, size_t maxBufferLength) { 55 | // triggers if the packetizer detects that it has a packet 56 | if (! _packetizer->hasPacket()){ 57 | return false; 58 | } 59 | 60 | Packet packet = _packetizer->getPacket(); 61 | for(size_t i = packet.startIndex; i <= packet.endIndex; i++) { 62 | buffer[i] = (*_bus)[i]; 63 | } 64 | 65 | // clear the packet 66 | _packetizer->clearPacket(); 67 | 68 | return true; 69 | } 70 | 71 | void PhotonNetworkLayer::clearPackets() { 72 | while(_packetizer->hasPacket()){ 73 | _packetizer->clearPacket(); 74 | } 75 | } 76 | 77 | bool PhotonNetworkLayer::transmitPacket(const uint8_t *buffer, size_t buffer_length) { 78 | _packetizer->writePacket(buffer, buffer_length); 79 | 80 | // TODO Handle write packet status result 81 | 82 | return true; 83 | } -------------------------------------------------------------------------------- /photon/src/PhotonNetworkLayer.h: -------------------------------------------------------------------------------- 1 | #ifndef _PHOTON_NETWORK_LAYER_H 2 | #define _PHOTON_NETWORK_LAYER_H 3 | 4 | #include "define.h" 5 | 6 | #include "FeederFloor.h" 7 | #include 8 | #include 9 | #include "PhotonPacketHandler.h" 10 | #include "Stream.h" 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #define PHOTON_NETWORK_MAX_PDU 32 17 | #define PHOTON_PROTOCOL_CHECKSUM_LENGTH 2 18 | 19 | class PhotonNetworkLayer 20 | { 21 | public: 22 | PhotonNetworkLayer( 23 | RS485Bus* bus, 24 | Packetizer* packetizer, 25 | FilterByValue* addressFilter, 26 | FeederFloor* feederFloor); 27 | 28 | void setLocalAddress(uint8_t address); 29 | uint8_t getLocalAddress(); 30 | 31 | bool getPacket(uint8_t* buffer, size_t maxBufferLength); 32 | 33 | bool transmitPacket(const uint8_t *buffer, size_t buffer_length); 34 | 35 | void clearPackets(); 36 | 37 | private: 38 | RS485Bus* _bus; 39 | Packetizer* _packetizer; 40 | FilterByValue* _addressFilter; 41 | FeederFloor* _feederFloor; 42 | uint8_t _send_buffer[64]; 43 | 44 | uint8_t _local_address; 45 | }; 46 | 47 | #endif //_PHOTON_NETWORK_LAYER_H -------------------------------------------------------------------------------- /photon/src/PhotonPacketHandler.h: -------------------------------------------------------------------------------- 1 | #ifndef _PHOTON_PROTOCOL_HANDLER_H 2 | #define _PHOTON_PROTOCOL_HANDLER_H 3 | 4 | #include 5 | #include 6 | 7 | class PhotonNetworkLayer; 8 | 9 | class PhotonPacketHandler { 10 | public: 11 | virtual void handle(PhotonNetworkLayer *instance, uint8_t *buffer, size_t buffer_length) = 0; 12 | }; 13 | 14 | #endif //_PHOTON_PROTOCOL_HANDLER_H -------------------------------------------------------------------------------- /photon/src/define.h: -------------------------------------------------------------------------------- 1 | #define DE PA12 2 | #define _RE PA11 3 | 4 | #define LED_R PA7 5 | #define LED_G PA6 6 | #define LED_B PA4 7 | 8 | #define SW1 PB1 9 | #define SW2 PB0 10 | 11 | #define PEEL1 PB3 12 | #define PEEL2 PA15 13 | 14 | #define ONE_WIRE PA8 15 | #define MOTOR_ENABLE PA2 16 | 17 | #define LONG_PRESS_DELAY 500 18 | 19 | #define RS485_BUS_BUFFER_SIZE 64 20 | 21 | // #define PVT Motors (long shaft, long cable, manually flipping power wires) 22 | //#define PVT 23 | 24 | #ifdef PVT 25 | // PVT Motor 26 | #define DRIVE1 PB4 27 | #define DRIVE2 PB5 28 | #define DRIVE_ENC_A PB6 29 | #define DRIVE_ENC_B PB7 30 | #else 31 | // MP Motor 32 | #define DRIVE1 PB5 33 | #define DRIVE2 PB4 34 | #define DRIVE_ENC_A PB7 35 | #define DRIVE_ENC_B PB6 36 | #endif -------------------------------------------------------------------------------- /photon/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Photon Feeder Firmware 3 | Part of the LumenPnP Project 4 | MPL v2 5 | 2025 6 | */ 7 | 8 | #include "define.h" 9 | 10 | #ifdef UNIT_TEST 11 | #include 12 | #else 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #endif // UNIT_TEST 19 | 20 | #ifndef MOTOR_DEPS 21 | #define MOTOR_DEPS 22 | 23 | #include 24 | 25 | #endif 26 | 27 | #include "FeederFloor.h" 28 | #include "PhotonFeeder.h" 29 | #include "PhotonFeederProtocol.h" 30 | #include "PhotonNetworkLayer.h" 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #define BAUD_RATE 57600 39 | 40 | //----- 41 | // Global Variables 42 | //----- 43 | 44 | #ifdef UNIT_TEST 45 | StreamFake ser(); 46 | #else 47 | HardwareSerial ser(PA10, PA9); 48 | #endif // ARDUINO 49 | 50 | // EEPROM 51 | OneWire oneWire(ONE_WIRE); 52 | FeederFloor feederFloor(&oneWire); 53 | 54 | // RS485 55 | HardwareSerialBusIO busIO(&ser); 56 | RS485Bus bus(busIO, _RE, DE); 57 | PhotonProtocol photon_protocol; 58 | Packetizer packetizer(bus, photon_protocol); 59 | FilterByValue addressFilter(0); 60 | 61 | // Encoder 62 | RotaryEncoder encoder(DRIVE_ENC_A, DRIVE_ENC_B, RotaryEncoder::LatchMode::TWO03); 63 | 64 | // Flags 65 | bool drive_mode = false; 66 | bool driving = false; 67 | bool driving_direction = false; 68 | 69 | // Feeder Class Instances 70 | PhotonFeeder *feeder; 71 | PhotonFeederProtocol *protocol; 72 | PhotonNetworkLayer *network; 73 | 74 | //------- 75 | //FUNCTIONS 76 | //------- 77 | 78 | void checkPosition() 79 | { 80 | encoder.tick(); // just call tick() to check the state. 81 | } 82 | 83 | //------- 84 | //SETUP 85 | //------- 86 | 87 | void setup() { 88 | pinMode(LED_R, OUTPUT); 89 | pinMode(LED_G, OUTPUT); 90 | pinMode(LED_B, OUTPUT); 91 | feeder->set_rgb(false, false, false); 92 | 93 | pinMode(SW1, INPUT_PULLUP); 94 | pinMode(SW2, INPUT_PULLUP); 95 | pinMode(MOTOR_ENABLE, OUTPUT); 96 | digitalWrite(MOTOR_ENABLE, HIGH); 97 | 98 | // Setup Feeder 99 | feeder = new PhotonFeeder(DRIVE1, DRIVE2, PEEL1, PEEL2, LED_R, LED_G, LED_B, &encoder); 100 | network = new PhotonNetworkLayer(&bus, &packetizer, &addressFilter, &feederFloor); 101 | protocol = new PhotonFeederProtocol(feeder, &feederFloor, network, UniqueID, UniqueIDsize); 102 | 103 | byte addr = feederFloor.read_floor_address(); 104 | 105 | if(addr == 0xFF){ // not detected, turn red 106 | feeder->set_rgb(true, false, false); 107 | } 108 | else if (addr == 0x00){ //not programmed, turn blue 109 | feeder->set_rgb(false, false, true); 110 | } 111 | 112 | //Starting rs-485 serial 113 | ser.begin(BAUD_RATE); 114 | 115 | // attach interrupts for encoder pins 116 | attachInterrupt(digitalPinToInterrupt(DRIVE_ENC_A), checkPosition, CHANGE); 117 | attachInterrupt(digitalPinToInterrupt(DRIVE_ENC_B), checkPosition, CHANGE); 118 | 119 | feeder->resetEncoderPosition(0); 120 | feeder->setMmPosition(0); 121 | 122 | } 123 | 124 | void lifetime(){ 125 | // lifetime testing loop 126 | uint32_t counter = millis(); 127 | uint32_t interval = 3000; 128 | while(true){ 129 | if(millis() > counter + interval){ 130 | //reset counter to millis() 131 | counter = millis(); 132 | //move 133 | feeder->feedDistance(40, true); 134 | feeder->resetEncoderPosition(0); 135 | feeder->setMmPosition(0); 136 | } 137 | } 138 | } 139 | 140 | void showVersion(){ 141 | 142 | feeder->showVersion(); 143 | 144 | } 145 | 146 | void topShortPress(){ 147 | //turn led white for movement 148 | feeder->set_rgb(true, true, true); 149 | // move forward 2mm 150 | feeder->feedDistance(20, true); 151 | } 152 | 153 | void bottomShortPress(){ 154 | //turn led white for movement 155 | feeder->set_rgb(true, true, true); 156 | // move forward 2mm 157 | feeder->feedDistance(20, false); 158 | 159 | if (feeder->getMoveResult() == PhotonFeeder::FeedResult::SUCCESS){ 160 | feeder->set_rgb(false, false, false); 161 | } 162 | else{ 163 | feeder->set_rgb(true, false, false); 164 | } 165 | } 166 | 167 | void topLongPress(){ 168 | //we've got a long top press, lets drive forward, tape or film depending on drive_mode 169 | if(drive_mode){ 170 | feeder->peel(true); 171 | } 172 | else{ 173 | //resetting first feed, since we could now have a new tape type 174 | feeder->_first_feed_since_load = true; 175 | feeder->drive(true); 176 | } 177 | // set flag for concurrency to know driving state 178 | driving = true; 179 | driving_direction = true; 180 | } 181 | 182 | void bottomLongPress(){ 183 | // moving in reverse, motor selected by drive_mode 184 | if(drive_mode){ 185 | feeder->peel(false); 186 | } 187 | else{ 188 | //resetting first feed, since we could now have a new tape type 189 | feeder->_first_feed_since_load = true; 190 | feeder->drive(false); 191 | } 192 | // set flag for concurrency to know driving state 193 | driving = true; 194 | driving_direction = false; 195 | 196 | } 197 | 198 | void bothLongPress(){ 199 | //both are pressed, switching if we are driving tape or film 200 | 201 | if(drive_mode){ 202 | feeder->set_rgb(false, false, true); 203 | drive_mode = false; 204 | } 205 | else{ 206 | feeder->set_rgb(true, true, false); 207 | drive_mode = true; 208 | } 209 | 210 | //if both are held for a long time, we show current version id 211 | uint32_t timerStart = millis(); 212 | 213 | bool alreadyFlashed = false; 214 | 215 | while( (!digitalRead(SW1) || !digitalRead(SW2))){ 216 | //do nothing while waiting for debounce 217 | if((timerStart + 2000 < millis()) && !alreadyFlashed){ 218 | feeder->set_rgb(false, false, false); 219 | showVersion(); 220 | alreadyFlashed = true; 221 | } 222 | } 223 | 224 | //delay for debounce 225 | delay(50); 226 | feeder->set_rgb(false, false, false); 227 | } 228 | 229 | inline void checkButtons() { 230 | if(!driving){ 231 | // Checking bottom button 232 | if(!digitalRead(SW1)){ 233 | delay(LONG_PRESS_DELAY); 234 | // if bottom long press 235 | if(!digitalRead(SW1)){ 236 | // if both long press 237 | if(!digitalRead(SW2)){ 238 | bothLongPress(); 239 | } 240 | // if just bottom long press 241 | else{ 242 | bottomLongPress(); 243 | } 244 | } 245 | // if bottom short press 246 | else{ 247 | bottomShortPress(); 248 | } 249 | } 250 | // Checking top button 251 | if(!digitalRead(SW2)){ 252 | delay(LONG_PRESS_DELAY); 253 | // if top long press 254 | if(!digitalRead(SW2)){ 255 | // if both long press 256 | if(!digitalRead(SW1)){ 257 | bothLongPress(); 258 | } 259 | // if just top long press 260 | else{ 261 | topLongPress(); 262 | } 263 | } 264 | // if top short press 265 | else{ 266 | topShortPress(); 267 | } 268 | } 269 | } 270 | else{ 271 | if((driving_direction && digitalRead(SW2)) || (!driving_direction && digitalRead(SW1))){ 272 | //stop all motors 273 | feeder->halt(); 274 | //reset encoder and mm position 275 | feeder->resetEncoderPosition(0); 276 | feeder->setMmPosition(0); 277 | driving = false; 278 | delay(5); 279 | } 280 | } 281 | } 282 | 283 | inline void checkForRS485Packet() { 284 | protocol->tick(); 285 | } 286 | 287 | //------ 288 | // MAIN CONTROL LOOP 289 | //------ 290 | void loop() { 291 | checkButtons(); 292 | checkForRS485Packet(); 293 | } 294 | -------------------------------------------------------------------------------- /photon/src/util: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/photonfirmware/photon/fd45e3b81123b62ba42b663bc7cdc090431e465d/photon/src/util -------------------------------------------------------------------------------- /photon/src/util.cpp: -------------------------------------------------------------------------------- 1 | #include "util.h" 2 | 3 | #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ 4 | uint32_t htonl(uint32_t hostlong) { 5 | uint32_t tmp = (hostlong << 24) & 0xff000000; 6 | tmp |= (hostlong << 8) & 0x00ff0000; 7 | tmp |= (hostlong >> 8) & 0x0000ff00; 8 | tmp |= (hostlong >> 24) & 0x000000ff; 9 | return tmp; 10 | } 11 | #endif 12 | 13 | 14 | //#define htonl(x) 15 | //( (((uint8_t *)x)[0] << 24) | (((uint8_t *)x)[1] << 16) | (((uint8_t *)x)[2] << 8) | (((uint8_t *)x)[3]) ) 16 | //( (((uint32_t)(x << 24 )) & 0xff000000) | (((uint32_t)(x << 8)) & 0x00ff0000) | ((x >> 8) & 0x0000ff00) | ((x >> 24) & 0x000000ff) ) -------------------------------------------------------------------------------- /photon/src/util.h: -------------------------------------------------------------------------------- 1 | #ifndef _UTIL_H 2 | #define _UTIL_H 3 | 4 | #include 5 | 6 | #ifndef NATIVE 7 | #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ 8 | 9 | uint32_t htonl(uint32_t hostlong); 10 | #define htons(x) ( ((x << 8) & 0xff00) | ((x >> 8) & 0xff) ) 11 | #define ntohl(x) htonl(x) 12 | #define ntohs(x) htons(x) 13 | 14 | #elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__ 15 | 16 | #define htonl(x) (x) 17 | #define htons(x) (x) 18 | #define ntohl(x) (x) 19 | #define ntohs(x) (x) 20 | 21 | #else 22 | #error Unknown Endianness 23 | #endif 24 | #endif 25 | 26 | #endif //_UTIL_H -------------------------------------------------------------------------------- /photon/src/versions.h: -------------------------------------------------------------------------------- 1 | 2 | struct version { 3 | uint8_t major; 4 | uint8_t minor; 5 | uint8_t patch; 6 | }; 7 | 8 | struct version beta = {0, 0, 0}; 9 | 10 | // The structs listed below are official releases of Photon 11 | // When prepping for a formal release, create a new struct named the scematic version number of the release with . replaced with _ 12 | // The first element in the struct should equal the MAJOR revision number of the version. This indicates the color. 13 | // The second element indicates the number of flashes. Increment this from the previous MAJOR version's value, or if a new MAJOR version, start with 1. 14 | // Save, commit, and push this change before attempting to make a release. It will fail compilation if this step is not completed. 15 | 16 | struct version v1_0_3 = {1, 0, 3}; 17 | 18 | struct version v1_0_4 = {1, 0, 4}; 19 | -------------------------------------------------------------------------------- /photon/test/test_desktop/test_index_feeder_protocol.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace fakeit; 6 | 7 | #include 8 | #include 9 | 10 | typedef struct { 11 | uint8_t destination_address; 12 | uint8_t *data; 13 | size_t length; 14 | } transmit_arg_capture_t; 15 | 16 | static std::vector args; 17 | 18 | static void reset_transmit_arg_capture() { 19 | for (transmit_arg_capture_t arg : args) { 20 | if (arg.data != NULL) { 21 | free(arg.data); 22 | } 23 | } 24 | args.clear(); 25 | } 26 | 27 | #define GET_FEEDER_ID 0x01 28 | #define INITIALIZE_FEEDER_ID 0x02 29 | #define GET_VERSION_ID 0x03 30 | #define MOVE_FEED_FORWARD_ID 0x04 31 | #define MOVE_FEED_BACKWARD_ID 0x05 32 | #define GET_FEEDER_ADDRESS_ID 0xC0 33 | 34 | #define ERROR_SUCCESS 0x00 35 | #define ERROR_WRONG_FEEDER_UUID 0x01 36 | #define ERROR_MOTOR_FAULT 0x02 37 | #define ERROR_UNINITIALIZED_FEEDER 0x03 38 | 39 | std::function transmit_capture = [](uint8_t destination_address, const uint8_t *buffer, size_t buffer_length) { 40 | transmit_arg_capture_t arg_entry; 41 | arg_entry.destination_address = destination_address; 42 | arg_entry.length = buffer_length; 43 | 44 | if (buffer_length > 0 && buffer != NULL) { 45 | arg_entry.data = (uint8_t *)malloc(buffer_length); 46 | memcpy(arg_entry.data, buffer, buffer_length); 47 | } 48 | 49 | args.push_back(arg_entry); 50 | return true; 51 | }; 52 | 53 | static uint8_t feeder_address = 0x01; 54 | static uint8_t feeder_uuid[] = {0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xaa, 0xbb}; 55 | 56 | static Mock feeder_mock; 57 | static Feeder &feeder = feeder_mock.get(); 58 | 59 | static Mock network_mock; 60 | static IndexNetworkLayer &network = network_mock.get(); 61 | 62 | static void test_index_feeder_protocol_setup() { 63 | feeder_mock.Reset(); 64 | When(Method(feeder_mock, init)).AlwaysReturn(true); 65 | 66 | network_mock.Reset(); 67 | reset_transmit_arg_capture(); 68 | When(Method(network_mock, transmitPacket)).AlwaysDo(transmit_capture); 69 | When(Method(network_mock, getLocalAddress)).AlwaysReturn(feeder_address); 70 | } 71 | 72 | static void test_index_feeder_protocol_teardown() { 73 | reset_transmit_arg_capture(); 74 | } 75 | 76 | static void test_index_feeder_protocol_get_feeder_id() { 77 | IndexFeederProtocol protocol(&feeder, feeder_uuid, sizeof(feeder_uuid)); 78 | 79 | uint8_t get_feeder[] = {GET_FEEDER_ID}; 80 | protocol.handle(&network, get_feeder, sizeof(get_feeder)); 81 | 82 | Verify(Method(network_mock, transmitPacket)).Once(); 83 | uint8_t expected[2 + sizeof(feeder_uuid)]; 84 | expected[0] = feeder_address; 85 | expected[1] = ERROR_SUCCESS; 86 | memcpy(&expected[2], feeder_uuid, sizeof(feeder_uuid)); 87 | TEST_ASSERT_EQUAL_UINT8(0, args[0].destination_address); 88 | TEST_ASSERT_EQUAL(sizeof(expected), args[0].length); 89 | TEST_ASSERT_EQUAL_MEMORY(expected, args[0].data, sizeof(expected)); 90 | } 91 | 92 | static void test_initialize_feeder(uint8_t *uuid, size_t uuid_length, uint8_t expected_status) { 93 | 94 | if (uuid_length != 12) { 95 | TEST_FAIL_MESSAGE("Only 12 byte UUIDs are supported"); 96 | } 97 | 98 | IndexFeederProtocol protocol(&feeder, feeder_uuid, sizeof(feeder_uuid)); 99 | 100 | uint8_t init_feeder[1 + uuid_length]; 101 | init_feeder[0] = INITIALIZE_FEEDER_ID; 102 | memcpy(&init_feeder[1], uuid, uuid_length); 103 | protocol.handle(&network, init_feeder, sizeof(init_feeder)); 104 | 105 | TEST_ASSERT_EQUAL_UINT8(0, args[0].destination_address); 106 | Verify(Method(network_mock, transmitPacket)).Once(); 107 | 108 | switch (expected_status) 109 | { 110 | case ERROR_SUCCESS: // SUCCESS 111 | { 112 | uint8_t expected[] = { feeder_address, expected_status }; 113 | TEST_ASSERT_EQUAL(sizeof(expected), args[0].length); 114 | TEST_ASSERT_EQUAL_MEMORY(expected, args[0].data, sizeof(expected)); 115 | 116 | TEST_ASSERT_TRUE(protocol.isInitialized()); 117 | Verify(Method(feeder_mock, init)).Once(); 118 | } 119 | break; 120 | 121 | case ERROR_WRONG_FEEDER_UUID: // Wrong Feeder Id 122 | { 123 | uint8_t expected[2 + sizeof(feeder_uuid)]; 124 | expected[0] = feeder_address; 125 | expected[1] = ERROR_WRONG_FEEDER_UUID; // Wrong Feeder Id 126 | memcpy(&expected[2], feeder_uuid, sizeof(feeder_uuid)); 127 | TEST_ASSERT_EQUAL(sizeof(expected), args[0].length); 128 | TEST_ASSERT_EQUAL_MEMORY(expected, args[0].data, sizeof(expected)); 129 | 130 | TEST_ASSERT_FALSE(protocol.isInitialized()); 131 | Verify(Method(feeder_mock, init)).Never(); 132 | } 133 | break; 134 | 135 | default: 136 | TEST_FAIL_MESSAGE("Unsupported Return Code"); 137 | break; 138 | } 139 | 140 | } 141 | 142 | static void test_index_feeder_protocol_initialize_feeder_good_uuid() { 143 | test_initialize_feeder(feeder_uuid, sizeof(feeder_uuid), ERROR_SUCCESS); 144 | } 145 | 146 | static void test_index_feeder_protocol_initialize_feeder_zero_uuid() { 147 | uint8_t uuid[12]; 148 | memset(uuid, 0, sizeof(uuid)); 149 | test_initialize_feeder(uuid, sizeof(uuid), ERROR_SUCCESS); 150 | } 151 | 152 | static void test_index_feeder_protocol_initialize_feeder_invalid_uuid() { 153 | uint8_t uuid[12]; 154 | memset(uuid, 0x11, sizeof(uuid)); 155 | test_initialize_feeder(uuid, sizeof(uuid), ERROR_WRONG_FEEDER_UUID); 156 | } 157 | 158 | static void test_index_feeder_protocol_get_version() { 159 | IndexFeederProtocol protocol(&feeder, feeder_uuid, sizeof(feeder_uuid)); 160 | 161 | uint8_t get_version[] = {GET_VERSION_ID}; 162 | protocol.handle(&network, get_version, sizeof(get_version)); 163 | 164 | TEST_ASSERT_EQUAL_UINT8(0, args[0].destination_address); 165 | Verify(Method(network_mock, transmitPacket)).Once(); 166 | uint8_t expected[] = { feeder_address, ERROR_SUCCESS, 0x01}; 167 | TEST_ASSERT_EQUAL_UINT8(0, args[0].destination_address); 168 | TEST_ASSERT_EQUAL(sizeof(expected), args[0].length); 169 | TEST_ASSERT_EQUAL_MEMORY(expected, args[0].data, sizeof(expected)); 170 | } 171 | 172 | static void index_feeder_move_test(uint8_t distance, bool forward, bool initialized, Feeder::FeedResult feeder_status, uint8_t expected_status) { 173 | When(Method(feeder_mock, feedDistance)).AlwaysReturn(feeder_status); 174 | IndexFeederProtocol protocol(&feeder, feeder_uuid, sizeof(feeder_uuid)); 175 | 176 | // Initialize Feeder 177 | if (initialized) { 178 | uint8_t init_feeder[1 + sizeof(feeder_uuid)]; 179 | init_feeder[0] = INITIALIZE_FEEDER_ID; 180 | memcpy(&init_feeder[1], feeder_uuid, sizeof(feeder_uuid)); 181 | protocol.handle(&network, init_feeder, sizeof(init_feeder)); 182 | 183 | // Reset To Only Track The Move Commands 184 | reset_transmit_arg_capture(); 185 | feeder_mock.ClearInvocationHistory(); 186 | network_mock.ClearInvocationHistory(); 187 | } 188 | 189 | uint8_t move_feed[2]; 190 | move_feed[0] = (forward) ? MOVE_FEED_FORWARD_ID : MOVE_FEED_BACKWARD_ID; // Adjust command based on direction 191 | move_feed[1] = distance; 192 | protocol.handle(&network, move_feed, sizeof(move_feed)); 193 | 194 | TEST_ASSERT_EQUAL_UINT8(0, args[0].destination_address); 195 | Verify(Method(network_mock, transmitPacket)).Once(); 196 | 197 | switch (expected_status) { 198 | case ERROR_SUCCESS: // SUCCESS 199 | { 200 | uint8_t expected[] = { feeder_address, expected_status }; 201 | TEST_ASSERT_EQUAL(sizeof(expected), args[0].length); 202 | TEST_ASSERT_EQUAL_MEMORY(expected, args[0].data, sizeof(expected)); 203 | Verify(Method(feeder_mock, feedDistance).Using(distance, forward)).Once(); 204 | } 205 | break; 206 | case ERROR_MOTOR_FAULT: // Motor Fault 207 | { 208 | uint8_t expected[] = { feeder_address, ERROR_MOTOR_FAULT }; 209 | TEST_ASSERT_EQUAL(sizeof(expected), args[0].length); 210 | TEST_ASSERT_EQUAL_MEMORY(expected, args[0].data, sizeof(expected)); 211 | Verify(Method(feeder_mock, feedDistance).Using(distance, forward)).Once(); 212 | } 213 | break; 214 | case ERROR_UNINITIALIZED_FEEDER: // Uninitialized Feeder 215 | { 216 | uint8_t expected[2 + sizeof(feeder_uuid)]; 217 | expected[0] = feeder_address; 218 | expected[1] = ERROR_UNINITIALIZED_FEEDER; 219 | memcpy(&expected[2], feeder_uuid, sizeof(feeder_uuid)); 220 | TEST_ASSERT_EQUAL(sizeof(expected), args[0].length); 221 | TEST_ASSERT_EQUAL_MEMORY(expected, args[0].data, sizeof(expected)); 222 | Verify(Method(feeder_mock, feedDistance)).Never(); 223 | } 224 | break; 225 | } 226 | } 227 | 228 | static void test_index_feeder_protocol_move_feed_forward_ok() { 229 | index_feeder_move_test(40, true, true, Feeder::FeedResult::SUCCESS, ERROR_SUCCESS); 230 | } 231 | 232 | static void test_index_feeder_protocol_move_feed_forward_motor_error() { 233 | index_feeder_move_test(40, true, true, Feeder::FeedResult::MOTOR_FAULT, ERROR_MOTOR_FAULT); 234 | } 235 | 236 | static void test_index_feeder_protocol_move_feed_forward_invalid_distance() { 237 | index_feeder_move_test(39, true, true, Feeder::FeedResult::INVALID_LENGTH, ERROR_MOTOR_FAULT); 238 | } 239 | 240 | static void test_index_feeder_protocol_move_feed_forward_uninitialized_feeder() { 241 | index_feeder_move_test(40, true, false, Feeder::FeedResult::SUCCESS, ERROR_UNINITIALIZED_FEEDER); 242 | 243 | } 244 | 245 | static void test_index_feeder_protocol_move_feed_backward_ok() { 246 | index_feeder_move_test(40, false, true, Feeder::FeedResult::SUCCESS, ERROR_SUCCESS); 247 | } 248 | 249 | static void test_index_feeder_protocol_move_feed_backward_motor_error() { 250 | index_feeder_move_test(40, false, true, Feeder::FeedResult::MOTOR_FAULT, ERROR_MOTOR_FAULT); 251 | } 252 | 253 | static void test_index_feeder_protocol_move_feed_backward_invalid_distance() { 254 | index_feeder_move_test(39, false, true, Feeder::FeedResult::INVALID_LENGTH, ERROR_MOTOR_FAULT); 255 | } 256 | 257 | static void test_index_feeder_protocol_move_feed_backward_uninitialized_feeder() { 258 | index_feeder_move_test(40, false, false, Feeder::FeedResult::SUCCESS, ERROR_UNINITIALIZED_FEEDER); 259 | } 260 | 261 | static void test_index_feeder_protocol_get_feeder_address_match() { 262 | IndexFeederProtocol protocol(&feeder, feeder_uuid, sizeof(feeder_uuid)); 263 | 264 | uint8_t payload[1 + sizeof(feeder_uuid)]; 265 | payload[0] = GET_FEEDER_ADDRESS_ID; 266 | memcpy(&payload[1], feeder_uuid, sizeof(feeder_uuid)); 267 | protocol.handle(&network, payload, sizeof(payload)); 268 | 269 | TEST_ASSERT_EQUAL_UINT8(0, args[0].destination_address); 270 | Verify(Method(network_mock, transmitPacket)).Once(); 271 | 272 | uint8_t expected[2 + sizeof(feeder_uuid)]; 273 | expected[0] = feeder_address; 274 | expected[1] = ERROR_SUCCESS; 275 | memcpy(&expected[2], feeder_uuid, sizeof(feeder_uuid)); 276 | TEST_ASSERT_EQUAL(sizeof(expected), args[0].length); 277 | TEST_ASSERT_EQUAL_MEMORY(expected, args[0].data, sizeof(expected)); 278 | } 279 | 280 | static void test_index_feeder_protocol_get_feeder_address_no_match() { 281 | IndexFeederProtocol protocol(&feeder, feeder_uuid, sizeof(feeder_uuid)); 282 | 283 | uint8_t payload[1 + sizeof(feeder_uuid)]; 284 | payload[0] = GET_FEEDER_ADDRESS_ID; 285 | memset(&payload[1], 0, sizeof(feeder_uuid)); 286 | protocol.handle(&network, payload, sizeof(payload)); 287 | 288 | Verify(Method(network_mock, transmitPacket)).Never(); 289 | } 290 | 291 | #define RUN_FEEDER_TEST(func) { test_index_feeder_protocol_setup(); RUN_TEST(func); test_index_feeder_protocol_teardown(); } 292 | 293 | void index_feeder_protocol_tests() { 294 | RUN_FEEDER_TEST(test_index_feeder_protocol_get_feeder_id); 295 | RUN_FEEDER_TEST(test_index_feeder_protocol_initialize_feeder_good_uuid); 296 | RUN_FEEDER_TEST(test_index_feeder_protocol_initialize_feeder_zero_uuid); 297 | RUN_FEEDER_TEST(test_index_feeder_protocol_initialize_feeder_invalid_uuid); 298 | RUN_FEEDER_TEST(test_index_feeder_protocol_get_version); 299 | RUN_FEEDER_TEST(test_index_feeder_protocol_move_feed_forward_ok); 300 | RUN_FEEDER_TEST(test_index_feeder_protocol_move_feed_forward_motor_error); 301 | RUN_FEEDER_TEST(test_index_feeder_protocol_move_feed_forward_invalid_distance); 302 | RUN_FEEDER_TEST(test_index_feeder_protocol_move_feed_forward_uninitialized_feeder); 303 | RUN_FEEDER_TEST(test_index_feeder_protocol_move_feed_backward_ok); 304 | RUN_FEEDER_TEST(test_index_feeder_protocol_move_feed_backward_motor_error); 305 | RUN_FEEDER_TEST(test_index_feeder_protocol_move_feed_backward_invalid_distance); 306 | RUN_FEEDER_TEST(test_index_feeder_protocol_move_feed_backward_uninitialized_feeder); 307 | RUN_FEEDER_TEST(test_index_feeder_protocol_get_feeder_address_match); 308 | RUN_FEEDER_TEST(test_index_feeder_protocol_get_feeder_address_no_match); 309 | } -------------------------------------------------------------------------------- /photon/test/test_desktop/test_index_network_layer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace fakeit; 6 | 7 | #include 8 | 9 | static void test_index_network_single_message_good_crc() { 10 | 11 | // Validate A Good CRC 12 | uint8_t buf[] = {0x00, 0x01, 0x01, 0xb1, 0x90}; 13 | 14 | 15 | Mock handler_mock; 16 | When(Method(handler_mock, handle)).AlwaysReturn(); 17 | IndexPacketHandler &handler = handler_mock.get(); 18 | 19 | When(Method(ArduinoFake(), millis)).Return(1); 20 | 21 | Stream* stream = ArduinoFakeMock(Stream); 22 | IndexNetworkLayer network(stream, 0, &handler); 23 | 24 | When(Method(ArduinoFake(Stream), available)).AlwaysReturn(0); 25 | network.tick(); 26 | 27 | Verify(Method(handler_mock, handle)).Never(); 28 | 29 | When(Method(ArduinoFake(Stream), available)).Return(sizeof(buf), 0); 30 | 31 | std::function fn = [buf](char *buffer, size_t buffer_length) { 32 | if (buffer_length >= sizeof(buf)) { 33 | memcpy(buffer, buf, buffer_length); 34 | return buffer_length; 35 | } 36 | return (size_t)0; 37 | }; 38 | 39 | When(Method(ArduinoFake(Stream), readBytes)).Do(fn); 40 | 41 | network.tick(); 42 | 43 | Verify(Method(handler_mock, handle)).Once(); 44 | } 45 | 46 | static void test_index_network_multiple_message_good_crc() { 47 | // Validate Multiple Messages At Once 48 | uint8_t buf[] = { 49 | 0x00, 0x04, 0x01, 0x02, 0x03, 0x04, 0x50, 0xd4, 50 | 0x00, 0x02, 0x05, 0x06, 0x22, 0xb6 51 | }; 52 | 53 | Mock handler_mock; 54 | When(Method(handler_mock, handle)).AlwaysReturn(); 55 | IndexPacketHandler &handler = handler_mock.get(); 56 | 57 | When(Method(ArduinoFake(), millis)).Return(1); 58 | 59 | Stream* stream = ArduinoFakeMock(Stream); 60 | IndexNetworkLayer network(stream, 0, &handler); 61 | 62 | When(Method(ArduinoFake(Stream), available)).AlwaysReturn(0); 63 | When(Method(ArduinoFake(Stream), available)).Return(sizeof(buf), 0); 64 | 65 | std::function fn = [buf](char *buffer, size_t buffer_length) { 66 | if (buffer_length >= sizeof(buf)) { 67 | memcpy(buffer, buf, buffer_length); 68 | return buffer_length; 69 | } 70 | return (size_t)0; 71 | }; 72 | 73 | When(Method(ArduinoFake(Stream), readBytes)).Do(fn); 74 | 75 | network.tick(); 76 | 77 | Verify(Method(handler_mock, handle)).Exactly(2); 78 | } 79 | 80 | static void test_index_network_single_message_bad_crc() { 81 | // Validate A Bad CRC 82 | uint8_t buf[] = {0x00, 0x01, 0x01, 0xb1, 0x91}; 83 | 84 | Mock handler_mock; 85 | When(Method(handler_mock, handle)).AlwaysReturn(); 86 | IndexPacketHandler &handler = handler_mock.get(); 87 | 88 | When(Method(ArduinoFake(), millis)).Return(1); 89 | 90 | Stream* stream = ArduinoFakeMock(Stream); 91 | IndexNetworkLayer network(stream, 0, &handler); 92 | 93 | When(Method(ArduinoFake(Stream), available)).Return(sizeof(buf)).AlwaysReturn(0); 94 | 95 | std::function fn = [buf](char *buffer, size_t buffer_length) { 96 | if (buffer_length >= sizeof(buf)) { 97 | memcpy(buffer, buf, buffer_length); 98 | return buffer_length; 99 | } 100 | return (size_t)0; 101 | }; 102 | 103 | When(Method(ArduinoFake(Stream), readBytes)).Do(fn); 104 | 105 | network.tick(); 106 | 107 | Verify(Method(handler_mock, handle)).Never(); 108 | } 109 | 110 | static void test_index_network_bad_crc_followed_by_good_crc() { 111 | Mock handler_mock; 112 | When(Method(handler_mock, handle)).AlwaysReturn(); 113 | IndexPacketHandler &handler = handler_mock.get(); 114 | 115 | // Validate A Bad CRC followed by a good CRC 116 | uint8_t buf[] = { 117 | 0x00, 0x01, 0x01, 0xb1, 0x91, 118 | 0x00, 0x04, 0x01, 0x02, 0x03, 0x04, 0x50, 0xd4 119 | }; 120 | 121 | When(Method(ArduinoFake(), millis)).Return(1); 122 | 123 | Stream* stream = ArduinoFakeMock(Stream); 124 | IndexNetworkLayer network(stream, 0, &handler); 125 | 126 | When(Method(ArduinoFake(Stream), available)).Return(sizeof(buf)).AlwaysReturn(0); 127 | 128 | std::function fn = [buf](char *buffer, size_t buffer_length) { 129 | if (buffer_length >= sizeof(buf)) { 130 | memcpy(buffer, buf, buffer_length); 131 | return buffer_length; 132 | } 133 | return (size_t)0; 134 | }; 135 | 136 | When(Method(ArduinoFake(Stream), readBytes)).Do(fn); 137 | 138 | network.tick(); 139 | 140 | Verify(Method(handler_mock, handle)).Once(); 141 | } 142 | 143 | static void test_malformed_frame_with_interframe_time(unsigned long interframe_time, bool should_return) { 144 | Mock handler_mock; 145 | When(Method(handler_mock, handle)).AlwaysReturn(); 146 | IndexPacketHandler &handler = handler_mock.get(); 147 | 148 | // Message That Is 1 Octet To Short 149 | uint8_t buf[] = {0x00, 0x01, 0x01, 0xb1 }; 150 | // Message That Is Valid 151 | uint8_t buf2[] = { 0x00, 0x04, 0x01, 0x02, 0x03, 0x04, 0x50, 0xd4 }; 152 | 153 | When(Method(ArduinoFake(), millis)).Return(1); 154 | 155 | Stream* stream = ArduinoFakeMock(Stream); 156 | IndexNetworkLayer network(stream, 0, &handler); 157 | 158 | When(Method(ArduinoFake(Stream), available)).Return(sizeof(buf)).AlwaysReturn(0); 159 | 160 | std::function fn = [buf](char *buffer, size_t buffer_length) { 161 | if (buffer_length >= sizeof(buf)) { 162 | memcpy(buffer, buf, buffer_length); 163 | return buffer_length; 164 | } 165 | return (size_t)0; 166 | }; 167 | 168 | When(Method(ArduinoFake(Stream), readBytes)).Do(fn); 169 | 170 | network.tick(); 171 | 172 | // Check The Method Is Never Called Because We're In Mid Frame 173 | Verify(Method(handler_mock, handle)).Never(); 174 | 175 | // Call The Second Frame 200ms later which is after the timeout 176 | std::function fn2 = [buf2](char *buffer, size_t buffer_length) { 177 | if (buffer_length >= sizeof(buf2)) { 178 | memcpy(buffer, buf2, buffer_length); 179 | return buffer_length; 180 | } 181 | return (size_t)0; 182 | }; 183 | When(Method(ArduinoFake(Stream), available)).Return(sizeof(buf2)).AlwaysReturn(0); 184 | When(Method(ArduinoFake(Stream), readBytes)).Do(fn2); 185 | When(Method(ArduinoFake(), millis)).Return(interframe_time + 1); 186 | 187 | network.tick(); 188 | if (should_return) { 189 | Verify(Method(handler_mock, handle)).Once(); 190 | } else { 191 | Verify(Method(handler_mock, handle)).Never(); 192 | } 193 | 194 | } 195 | 196 | static void test_malformed_frame_timeout() { 197 | test_malformed_frame_with_interframe_time(200, true); 198 | } 199 | 200 | static void test_malformed_frame_just_past_timeout() { 201 | test_malformed_frame_with_interframe_time(101, true); 202 | } 203 | 204 | static void test_malformed_frame_at_timeout() { 205 | test_malformed_frame_with_interframe_time(100, false); 206 | } 207 | 208 | static void test_malformed_frame_without_timeout() { 209 | test_malformed_frame_with_interframe_time(10, false); 210 | } 211 | 212 | static void test_transmit_packet() { 213 | Stream* stream = ArduinoFakeMock(Stream); 214 | IndexNetworkLayer network(stream, 0, NULL); 215 | 216 | const uint8_t payload[] = {0x01, 0x00, 0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xaa, 0xbb, 0xcc}; 217 | 218 | When(OverloadedMethod(ArduinoFake(Stream), write, size_t(const uint8_t *, size_t)) ).Return(1, 1, sizeof(payload), 2); 219 | When(Method(ArduinoFake(Stream), flush)).Return(); 220 | 221 | bool result = network.transmitPacket(0x00, payload, sizeof(payload)); 222 | 223 | // Assert That It Transmitted 224 | TEST_ASSERT_TRUE(result); 225 | 226 | // Verify The Address 227 | auto length_is_one = [](const uint8_t *buffer, size_t buffer_length) { return (buffer_length == 1); }; 228 | auto payload_match = [&](const uint8_t *buffer, size_t buffer_length) { return ((buffer == payload) && (buffer_length == sizeof(payload))); }; 229 | auto length_is_two = [](const uint8_t *buffer, size_t buffer_length) { return (buffer_length == 2); }; 230 | 231 | //TODO: Figure Out How To Validate The CRC 232 | Verify( 233 | OverloadedMethod(ArduinoFake(Stream), write, size_t(const uint8_t *, size_t)).Matching(length_is_one) * 2 + 234 | OverloadedMethod(ArduinoFake(Stream), write, size_t(const uint8_t *, size_t)).Matching(payload_match) + 235 | OverloadedMethod(ArduinoFake(Stream), write, size_t(const uint8_t *, size_t)).Matching(length_is_two) 236 | ); 237 | 238 | } 239 | 240 | static void test_transmit_null() { 241 | Stream* stream = ArduinoFakeMock(Stream); 242 | IndexNetworkLayer network(stream, 0, NULL); 243 | bool result = network.transmitPacket(0x00, NULL, 0); 244 | TEST_ASSERT_FALSE(result); 245 | } 246 | 247 | static void test_transmit_too_long() { 248 | #define TEST_LENGTH (INDEX_NETWORK_MAX_PDU + 1) 249 | Stream* stream = ArduinoFakeMock(Stream); 250 | IndexNetworkLayer network(stream, 0, NULL); 251 | uint8_t payload[TEST_LENGTH]; 252 | memset(payload, 0, TEST_LENGTH); 253 | bool result = network.transmitPacket(0x00, payload, sizeof(payload)); 254 | TEST_ASSERT_FALSE(result); 255 | } 256 | 257 | void index_network_layer_tests() { 258 | RUN_TEST(test_index_network_single_message_good_crc); 259 | RUN_TEST(test_index_network_multiple_message_good_crc); 260 | RUN_TEST(test_index_network_single_message_bad_crc); 261 | RUN_TEST(test_index_network_bad_crc_followed_by_good_crc); 262 | RUN_TEST(test_malformed_frame_timeout); 263 | RUN_TEST(test_malformed_frame_just_past_timeout); 264 | RUN_TEST(test_malformed_frame_at_timeout); 265 | RUN_TEST(test_malformed_frame_without_timeout); 266 | RUN_TEST(test_transmit_packet); 267 | RUN_TEST(test_transmit_null); 268 | RUN_TEST(test_transmit_too_long); 269 | 270 | } -------------------------------------------------------------------------------- /photon/test/test_desktop/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | extern void index_network_layer_tests(); 5 | extern void index_feeder_protocol_tests(); 6 | 7 | void setUp(void) { 8 | ArduinoFakeReset(); 9 | } 10 | 11 | int process() { 12 | UNITY_BEGIN(); 13 | index_network_layer_tests(); 14 | index_feeder_protocol_tests(); 15 | return UNITY_END(); 16 | } 17 | 18 | int main(int argc, char **argv) { 19 | return process(); 20 | } -------------------------------------------------------------------------------- /photon/test/test_embedded/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int process() { 5 | UNITY_BEGIN(); 6 | //index_protocol_tests(); 7 | return UNITY_END(); 8 | } 9 | 10 | void setup() { 11 | // NOTE!!! Wait for >2 secs 12 | // if board doesn't support software reset via Serial.DTR/RTS 13 | 14 | delay(2000); 15 | process(); 16 | } 17 | 18 | void loop() { 19 | digitalWrite(13, HIGH); 20 | delay(100); 21 | digitalWrite(13, LOW); 22 | delay(500); 23 | } --------------------------------------------------------------------------------