code finished for testing, added serial monitor and debug output
This commit is contained in:
janik
@@ -0,0 +1,9 @@
|
||||
.pio
|
||||
.vscode/.browse.c_cpp.db*
|
||||
.vscode/c_cpp_properties.json
|
||||
.vscode/launch.json
|
||||
.vscode/ipch
|
||||
.travis.yml
|
||||
.vscode
|
||||
include
|
||||
lib
|
||||
@@ -0,0 +1,48 @@
|
||||
; 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
|
||||
|
||||
[platformio]
|
||||
default_envs = photon-bmp
|
||||
|
||||
[env]
|
||||
platform = ststm32
|
||||
framework = arduino
|
||||
board = nucleo_f031k6
|
||||
lib_deps =
|
||||
ricaun/ArduinoUniqueID@^1.1.0
|
||||
mathertel/RotaryEncoder@^1.5.3
|
||||
mike-matera/FastPID@^1.3.1
|
||||
paulstoffregen/OneWire@^2.3.7
|
||||
jnesselr/RS485@^0.0.9
|
||||
build_flags = -Os -ggdb2
|
||||
debug_build_flags = ${build_flags}
|
||||
test_ignore = test_desktop
|
||||
check_tool = cppcheck
|
||||
check_flags = cppcheck: -j 4
|
||||
check_src_filters = src/*
|
||||
check_skip_packages = yes
|
||||
|
||||
[env:stm32f031k6t6]
|
||||
board_build.mcu = stm32f031k6t6
|
||||
build_flags =
|
||||
-D VERSION_STRING=\"${sysenv.VERSION_STRING}\"
|
||||
|
||||
[env:photon-bmp]
|
||||
; For programming and debugging via a BlackMagicProbe
|
||||
extends = env:stm32f031k6t6
|
||||
monitor_speed = 115200
|
||||
upload_protocol = stlink
|
||||
debug_tool = stlink
|
||||
|
||||
[env:photon-serial]
|
||||
; For programming feeders via the FTDI USB -> Serial tool included in shipped LumenPnPs.
|
||||
extends = env:stm32f031k6t6
|
||||
upload_protocol = serial
|
||||
upload_speed = 9600
|
||||
@@ -0,0 +1,160 @@
|
||||
#include "FeederFloor.h"
|
||||
|
||||
FeederFloor::FeederFloor(OneWire* oneWire) : _oneWire(oneWire) {
|
||||
|
||||
}
|
||||
|
||||
uint8_t FeederFloor::read_floor_address() {
|
||||
// reset the 1-wire line, and return false if no chip detected
|
||||
if(!_oneWire->reset()) {
|
||||
return 0xFF;
|
||||
}
|
||||
|
||||
// Send 0x3C to indicate skipping the ROM selection step; there'll only ever be one ROM on the bus
|
||||
_oneWire->skip();
|
||||
|
||||
// array with the commands to initiate a read, DS28E07 device expect 3 bytes to start a read: command,LSB&MSB adresses
|
||||
uint8_t leemem[3] = {
|
||||
0xF0,
|
||||
0x00,
|
||||
0x00
|
||||
};
|
||||
|
||||
// sending those three bytes
|
||||
_oneWire->write_bytes(leemem, sizeof(leemem), 1);
|
||||
|
||||
uint8_t addr = _oneWire->read(); // Start by reading our address byte
|
||||
|
||||
// Read the next 31 bytes, discarding their value. Each page is 32 bytes so we need 32 read commands
|
||||
for (uint8_t i = 0; i < 31; i++) {
|
||||
_oneWire->read();
|
||||
}
|
||||
|
||||
// return the first byte from returned data
|
||||
return addr;
|
||||
}
|
||||
|
||||
bool FeederFloor::write_floor_address(uint8_t address) {
|
||||
/*
|
||||
wriıte_floor_address()
|
||||
success returns programmed address byte
|
||||
failure returns 0xFF
|
||||
|
||||
This function takes a byte as in input, and flashes it to address 0x0000 in the eeprom (where the floor ID is stored).
|
||||
The DS28E07 requires a million and one steps to make this happen. Reference the datasheet for details:
|
||||
https://datasheets.maximintegrated.com/en/ds/DS28E07.pdf
|
||||
*/
|
||||
|
||||
|
||||
byte i; // This is for the for loops
|
||||
//-----
|
||||
// Write To Scratchpad
|
||||
//-----
|
||||
|
||||
byte data[8] = {address, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
|
||||
|
||||
// reset the 1-wire line, and return false if no chip detected
|
||||
if(!_oneWire->reset()){
|
||||
return false;
|
||||
}
|
||||
|
||||
// Send 0x3C to indicate skipping the ROM selection step; there'll only ever be one ROM on the bus
|
||||
_oneWire->skip();
|
||||
|
||||
// array with the commands to initiate a write to the scratchpad, DS28E07 device expect 3 bytes to start a read: command,LSB&MSB adresses
|
||||
byte leemem[3] = {
|
||||
0x0F,
|
||||
0x00,
|
||||
0x00
|
||||
};
|
||||
|
||||
// sending those three bytes
|
||||
_oneWire->write_bytes(leemem, sizeof(leemem), 1);
|
||||
|
||||
// Now it's time to actually write the data to the scratchpad
|
||||
for ( i = 0; i < 8; i++) {
|
||||
_oneWire->write(data[i], 1);
|
||||
}
|
||||
|
||||
// read back the CRC
|
||||
//byte ccrc = _oneWire->read();
|
||||
|
||||
//-----
|
||||
// Read Scratchpad
|
||||
//-----
|
||||
|
||||
// reset the 1-wire line, and return failure if no chip detected
|
||||
if(!_oneWire->reset()){
|
||||
return false;
|
||||
}
|
||||
|
||||
// Send 0x3C to indicate skipping the ROM selection step; there'll only ever be one ROM on the bus
|
||||
_oneWire->skip();
|
||||
|
||||
// send read scratchpad command
|
||||
_oneWire->write(0xAA, 1);
|
||||
|
||||
// array for the data we'll read back
|
||||
byte read_data[11];
|
||||
|
||||
//read in TA1, TA2, and E/S bytes, then the 8 bytes of data
|
||||
for ( i = 0; i < sizeof(read_data); i++) {
|
||||
read_data[i] = _oneWire->read();
|
||||
}
|
||||
|
||||
#if 0 // TODO we should probably be checking the CRC
|
||||
// byte for the ccrc the eeprom will send us
|
||||
byte scratchpad_ccrc = _oneWire->read();
|
||||
|
||||
byte ccrc_calc = OneWire::crc8(read_data, sizeof(read_data));
|
||||
|
||||
// TODO need to be checking CCRC. never returns true, even when data is identical.
|
||||
// if(scratchpad_ccrc != ccrc_calc){
|
||||
// // do nothing
|
||||
// }
|
||||
#endif
|
||||
|
||||
//-----
|
||||
// Copy Scratchpad to Memory
|
||||
//-----
|
||||
|
||||
// reset the 1-wire line, and return false if no chip detected
|
||||
if(!_oneWire->reset()){
|
||||
return false;
|
||||
}
|
||||
|
||||
// Send 0x3C to indicate skipping the ROM selection step; there'll only ever be one ROM on the bus
|
||||
_oneWire->skip();
|
||||
|
||||
// copy scratchpad command
|
||||
_oneWire->write(0x55, 1);
|
||||
|
||||
// sending auth bytes from scratchpad read, which is the first 3 bytes
|
||||
_oneWire->write_bytes(read_data, 3, 1);
|
||||
|
||||
// wait for programming, we'll get alternating 1s and 0s when done
|
||||
uint32_t timer = millis();
|
||||
while(true){
|
||||
if(_oneWire->read() == 0xAA){
|
||||
break;
|
||||
}
|
||||
if( (millis() - timer) > 20 ){ // datasheet says it should only ever take 12ms at most to program
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// send reset
|
||||
if(!_oneWire->reset()){
|
||||
return false;
|
||||
}
|
||||
|
||||
// check the floor address by reading
|
||||
byte written_address = this->read_floor_address();
|
||||
|
||||
if(written_address == address) {
|
||||
//return new address
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
#pragma once
|
||||
|
||||
#include <OneWire.h>
|
||||
|
||||
class FeederFloor {
|
||||
public:
|
||||
explicit FeederFloor(OneWire* oneWire);
|
||||
|
||||
uint8_t read_floor_address();
|
||||
bool write_floor_address(uint8_t address);
|
||||
|
||||
private:
|
||||
OneWire* _oneWire;
|
||||
};
|
||||
@@ -0,0 +1,731 @@
|
||||
#include <Arduino.h>
|
||||
#include "PhotonFeeder.h"
|
||||
#include "define.h"
|
||||
#include "versions.h"
|
||||
#include <vector>
|
||||
|
||||
// --------------------
|
||||
|
||||
// Calculating Ticks per Tenth MM
|
||||
// gearbox has ratio of 1:1030
|
||||
// encoder has 14 ticks per revolution (7 per channel)
|
||||
// one full rotation of the output shaft is 14*1030 = 14420 ticks
|
||||
// divided by 32 teeth is 450.625 ticks per tooth
|
||||
// divided by 40 tenths of a mm per tooth (4mm) is 11.265625 ticks per tenth mm
|
||||
// 8.8 microns per tick
|
||||
|
||||
// In reality, the gearbox has a slight deviation from the perfect 1:1030 gearing
|
||||
// Emperical testing brings our TICKS_PER_TENTH_MM to 11.273
|
||||
//#define TICKS_PER_TENTH_MM 11.273
|
||||
#define TICKS_PER_TENTH_MM 11.762
|
||||
#define THOUSANDTHS_TICKS_PER_TENTH_MM ((uint32_t)(TICKS_PER_TENTH_MM * 1000))
|
||||
#define TENTH_MM_PER_PIP 40
|
||||
|
||||
// -----------
|
||||
// Thresholds
|
||||
// -----------
|
||||
|
||||
// number of ticks within requested tick position we should begin halting
|
||||
#define DRIVE_APPROACH_FINAL_TICKS 100
|
||||
// when moving backwards, how far further backwards past requested position to approach from the back
|
||||
#define BACKLASH_COMP_TENTH_MM 10
|
||||
|
||||
// --------
|
||||
// Timing
|
||||
// --------
|
||||
|
||||
// how long the peel motor will peel film per tenth mm of tape requested to be driven
|
||||
#define PEEL_TIME_PER_TENTH_MM 18
|
||||
// how long the peel motor will peel film during backwards movements per tenth mm of tape requested to be driven
|
||||
#define BACKWARDS_PEEL_TIME_PER_TENTH_MM 30
|
||||
// short amount of time peel motor moves backwards to reduce tension on film after peeling
|
||||
#define PEEL_BACKOFF_TIME 30
|
||||
// amount of time we allow for each tenth mm before timeout (in ms)
|
||||
#define TIMEOUT_TIME_PER_TENTH_MM 40
|
||||
// after driving backwards, how long do we peel to take up any potential slack in the film
|
||||
#define BACKWARDS_FEED_FILM_SLACK_REMOVAL_TIME 350
|
||||
|
||||
// Unit Tests Fail Because This Isn't Defined In ArduinoFake for some reason
|
||||
#ifndef INPUT_ANALOG
|
||||
#define INPUT_ANALOG 0x04
|
||||
#endif
|
||||
|
||||
PhotonFeeder::PhotonFeeder(
|
||||
uint8_t drive1_pin,
|
||||
uint8_t drive2_pin,
|
||||
uint8_t peel1_pin,
|
||||
uint8_t peel2_pin,
|
||||
uint8_t led_red,
|
||||
uint8_t led_green,
|
||||
uint8_t led_blue,
|
||||
RotaryEncoder* encoder
|
||||
) :
|
||||
_drive1_pin(drive1_pin),
|
||||
_drive2_pin(drive2_pin),
|
||||
_peel1_pin(peel1_pin),
|
||||
_peel2_pin(peel2_pin),
|
||||
_led_red(led_red),
|
||||
_led_green(led_green),
|
||||
_led_blue(led_blue),
|
||||
_position(0),
|
||||
_encoder(encoder) {
|
||||
|
||||
pinMode(_drive1_pin, OUTPUT);
|
||||
pinMode(_drive2_pin, OUTPUT);
|
||||
pinMode(_peel1_pin, OUTPUT);
|
||||
pinMode(_peel2_pin, OUTPUT);
|
||||
|
||||
pinMode(_led_red, OUTPUT);
|
||||
pinMode(_led_green, OUTPUT);
|
||||
pinMode(_led_blue, OUTPUT);
|
||||
|
||||
if(_version == ""){
|
||||
_version = "debug";
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//-----------
|
||||
//
|
||||
// TOOLS
|
||||
//
|
||||
//-----------
|
||||
|
||||
PhotonFeeder::FeedResult PhotonFeeder::getMoveResult(){
|
||||
return _lastFeedStatus;
|
||||
}
|
||||
|
||||
uint16_t PhotonFeeder::calculateExpectedFeedTime(uint8_t distance, bool forward){
|
||||
|
||||
// This command is ONLY for generating a time that we send back to the host
|
||||
// calculating timeouts actually determining if we've failed a feed is separate.
|
||||
if(forward){
|
||||
// we're calculating expected feed time of an _optimal_ forward feed command. this includes:
|
||||
// - peel forward time
|
||||
// - peel backoff time
|
||||
// - expected time to drive forward assuming one attempt
|
||||
if(_first_feed_since_load){
|
||||
return (distance * PEEL_TIME_PER_TENTH_MM) + PEEL_BACKOFF_TIME + (distance * TIMEOUT_TIME_PER_TENTH_MM) + 1000;
|
||||
}
|
||||
else{
|
||||
return (distance * PEEL_TIME_PER_TENTH_MM) + PEEL_BACKOFF_TIME + (distance * TIMEOUT_TIME_PER_TENTH_MM) + 200;
|
||||
}
|
||||
|
||||
}
|
||||
else {
|
||||
// we're calculating expected feed time of an _optimal_ backward feed command. this includes:
|
||||
// - unpeeling film time to prep for backwards movement
|
||||
// - backwards movement including backlash distance
|
||||
// - remaining film slack takeup
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
void PhotonFeeder::identify() {
|
||||
for(int i = 0;i<3;i++){
|
||||
set_rgb(true, true, true);
|
||||
delay(300);
|
||||
set_rgb(false, false, false);
|
||||
delay(300);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void PhotonFeeder::showVersion() {
|
||||
delay(500);
|
||||
|
||||
if(_version.indexOf("debug") >= 0){ // flashing red, beta
|
||||
set_rgb(true, false, false);
|
||||
delay(250);
|
||||
set_rgb(false, false, false);
|
||||
delay(250);
|
||||
}
|
||||
|
||||
else { // flashing green, v1.X.X
|
||||
|
||||
set_rgb(false, true, false);
|
||||
delay(250);
|
||||
set_rgb(false, false, false);
|
||||
delay(250);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void PhotonFeeder::vendorSpecific(uint8_t options[VENDOR_SPECIFIC_OPTIONS_LENGTH], uint8_t* response) {
|
||||
uint8_t command = options[0];
|
||||
|
||||
int len = _version.length();
|
||||
|
||||
// Commands 0x00 - 0x0F get the firmware version string in chunks. The firmware version string may be smaller than
|
||||
// the allotted 7 chunks, but it is null-terminated as expected.
|
||||
if(command <= 0x0F) {
|
||||
const auto start_index = min(size_t(command) * VENDOR_SPECIFIC_OPTIONS_LENGTH, _version.length());
|
||||
const auto chunk = _version.c_str() + start_index;
|
||||
|
||||
// NOTE: Using strncpy here specifically because we want its behavior where it handles encountering a null byte
|
||||
// in the src string.
|
||||
strncpy((char*)(response), chunk, VENDOR_SPECIFIC_OPTIONS_LENGTH);
|
||||
}
|
||||
|
||||
switch(command){
|
||||
case 0x10:
|
||||
uint8_t ledMask = options[1];
|
||||
|
||||
int blue = ledMask & 1;
|
||||
int green = (ledMask >> 1) & 1;
|
||||
int red = (ledMask >> 2) & 1;
|
||||
int set = (ledMask >> 3) & 1;
|
||||
|
||||
if(set){
|
||||
set_rgb(red, green, blue);
|
||||
}
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
return;
|
||||
|
||||
|
||||
}
|
||||
|
||||
bool PhotonFeeder::checkLoaded() {
|
||||
/* checkLoaded()
|
||||
The checkLoaded() function checks to see what's loaded in the feeder, and sets drive methodology appropriately.
|
||||
This gets run on first movement command after boot, and after tape is driven through buttons
|
||||
*/
|
||||
|
||||
//takes up any backlash slack, ensures any forward movement is tape movement
|
||||
analogWrite(_drive1_pin, 0);
|
||||
analogWrite(_drive2_pin, 250);
|
||||
delay(2);
|
||||
|
||||
analogWrite(_drive1_pin, 0);
|
||||
analogWrite(_drive2_pin, 20);
|
||||
delay(100);
|
||||
|
||||
halt();
|
||||
|
||||
// find starting threshold of movement
|
||||
int errorThreshold = 3;
|
||||
int movedAt = 256;
|
||||
signed long startingTick, currentTick;
|
||||
|
||||
startingTick = _encoder->getPosition();
|
||||
|
||||
for(int movementIndex = 20; movementIndex<255; movementIndex=movementIndex + 5){
|
||||
|
||||
analogWrite(_drive1_pin, 0);
|
||||
analogWrite(_drive2_pin, movementIndex);
|
||||
|
||||
delay(75);
|
||||
|
||||
currentTick = _encoder->getPosition();
|
||||
|
||||
if(abs(startingTick - currentTick) > errorThreshold){
|
||||
movedAt = movementIndex;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
halt();
|
||||
|
||||
if(movedAt > 140){
|
||||
_beefy_boi = true;
|
||||
//set_rgb(true, false, false);
|
||||
//delay(200);
|
||||
}
|
||||
else{
|
||||
_beefy_boi = false;
|
||||
//set_rgb(false, false, true);
|
||||
//delay(200);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
void PhotonFeeder::resetEncoderPosition(uint16_t position){
|
||||
_encoder->setPosition(position);
|
||||
}
|
||||
|
||||
void PhotonFeeder::setMmPosition(uint16_t position){
|
||||
_position = position;
|
||||
}
|
||||
|
||||
void PhotonFeeder::set_rgb(bool red, bool green, bool blue) {
|
||||
digitalWrite(LED_R, ! red);
|
||||
digitalWrite(LED_G, ! green);
|
||||
digitalWrite(LED_B, ! blue);
|
||||
}
|
||||
|
||||
//-----------
|
||||
//
|
||||
// MANUAL MOTOR CONTROL
|
||||
//
|
||||
//-----------
|
||||
|
||||
void PhotonFeeder::peel(bool forward) {
|
||||
if(forward){
|
||||
analogWrite(_peel1_pin, 255);
|
||||
analogWrite(_peel2_pin, 0);
|
||||
}
|
||||
else{
|
||||
analogWrite(_peel1_pin, 0);
|
||||
analogWrite(_peel2_pin, 255);
|
||||
}
|
||||
}
|
||||
|
||||
void PhotonFeeder::drive(bool forward){
|
||||
|
||||
if(forward){
|
||||
analogWrite(_drive1_pin, 0);
|
||||
analogWrite(_drive2_pin, 255);
|
||||
}
|
||||
else{
|
||||
analogWrite(_drive1_pin, 255);
|
||||
analogWrite(_drive2_pin, 0);
|
||||
}
|
||||
}
|
||||
|
||||
void PhotonFeeder::driveValue(bool forward, uint8_t value){
|
||||
if(forward){
|
||||
analogWrite(_drive1_pin, 0);
|
||||
analogWrite(_drive2_pin, value);
|
||||
}
|
||||
else{
|
||||
analogWrite(_drive1_pin, value);
|
||||
analogWrite(_drive2_pin, 0);
|
||||
}
|
||||
}
|
||||
|
||||
void PhotonFeeder::driveBrakeValue(bool forward, uint8_t value){
|
||||
if(forward){
|
||||
analogWrite(_drive1_pin, 255-value);
|
||||
analogWrite(_drive2_pin, 255);
|
||||
}
|
||||
else{
|
||||
analogWrite(_drive1_pin, 255);
|
||||
analogWrite(_drive2_pin, 255-value);
|
||||
}
|
||||
}
|
||||
|
||||
void PhotonFeeder::brakeDrive(){
|
||||
//brings both drive pins high
|
||||
analogWrite(_drive1_pin, 255);
|
||||
analogWrite(_drive2_pin, 255);
|
||||
}
|
||||
|
||||
void PhotonFeeder::brakePeel(){
|
||||
//brings both drive pins high
|
||||
analogWrite(_peel1_pin, 255);
|
||||
analogWrite(_peel2_pin, 255);
|
||||
}
|
||||
|
||||
void PhotonFeeder::halt(){
|
||||
brakeDrive();
|
||||
brakePeel();
|
||||
}
|
||||
|
||||
//-----------
|
||||
//
|
||||
// FEEDING
|
||||
//
|
||||
//-----------
|
||||
|
||||
void PhotonFeeder::feedDistance(uint16_t tenths_mm, bool forward) {
|
||||
|
||||
if (_first_feed_since_load){
|
||||
checkLoaded();
|
||||
_first_feed_since_load = false;
|
||||
}
|
||||
|
||||
set_rgb(true, true, true);
|
||||
|
||||
bool success = (forward) ? moveForward(tenths_mm) : moveBackward(tenths_mm);
|
||||
|
||||
if(success){
|
||||
set_rgb(false, false, false);
|
||||
}
|
||||
else{
|
||||
set_rgb(true, false, false);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
bool PhotonFeeder::moveForward(uint16_t tenths_mm) {
|
||||
int retry_index = 0;
|
||||
// First, ensure everything is stopped
|
||||
halt();
|
||||
|
||||
// segment our total distance into pip chunks
|
||||
div_t result = div(tenths_mm, TENTH_MM_PER_PIP);
|
||||
|
||||
// quot is the number of 1 pip loops we run
|
||||
for(int i = 0;i<result.quot;i++){
|
||||
// move forward 40 tenths
|
||||
if(!moveForwardSequence(40, true)){ // if it fails, try again with a fresh pulse of power after moving the motor back a bit.
|
||||
while(true){
|
||||
retry_index++;
|
||||
|
||||
// if we're on our second to last attempt, we should absolutely be considered a beefy boi
|
||||
if(retry_index + 1 == _retry_limit){
|
||||
_beefy_boi = true;
|
||||
}
|
||||
|
||||
if(retry_index > _retry_limit){
|
||||
_lastFeedStatus = PhotonFeeder::FeedResult::COULDNT_REACH;
|
||||
return false;
|
||||
}
|
||||
drive(false);
|
||||
delay(50);
|
||||
halt();
|
||||
if(moveForwardSequence(40, false)){
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// rem is the value we drive in a last loop to move any remaining distance less than a pip
|
||||
if(result.rem > 0){
|
||||
//move forward result.rem
|
||||
if(!moveForwardSequence(result.rem, true)){ // if it fails, try again with a fresh pulse of power after moving the motor back a bit.
|
||||
while(true){
|
||||
retry_index++;
|
||||
|
||||
// if we're on our second to last attempt, we should absolutely be considered a beefy boi
|
||||
if(retry_index + 1 == _retry_limit){
|
||||
_beefy_boi = true;
|
||||
}
|
||||
|
||||
if(retry_index > _retry_limit){
|
||||
_lastFeedStatus = PhotonFeeder::FeedResult::COULDNT_REACH;
|
||||
return false;
|
||||
}
|
||||
drive(false);
|
||||
delay(50);
|
||||
halt();
|
||||
if(moveForwardSequence(result.rem, false)){
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_lastFeedStatus = PhotonFeeder::FeedResult::SUCCESS;
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
bool PhotonFeeder::moveBackward(uint16_t tenths_mm) {
|
||||
// First, ensure everything is stopped
|
||||
halt();
|
||||
|
||||
// Next, unspool some film to give the tape slack
|
||||
signed long peel_time = (tenths_mm * BACKWARDS_PEEL_TIME_PER_TENTH_MM);
|
||||
peel(false);
|
||||
delay(peel_time);
|
||||
brakePeel();
|
||||
|
||||
// move tape backward
|
||||
// first we overshoot by the backlash distance, then approach from the forward direction
|
||||
if (moveBackwardSequence(false, tenths_mm + BACKLASH_COMP_TENTH_MM)){
|
||||
|
||||
// if this is successful, we peel film to take up any slack
|
||||
peel(true);
|
||||
delay(BACKWARDS_FEED_FILM_SLACK_REMOVAL_TIME);
|
||||
brakePeel();
|
||||
|
||||
//then drive the last little bit
|
||||
if(moveBackwardSequence(true, BACKLASH_COMP_TENTH_MM)){
|
||||
//peel again to take up any slack
|
||||
|
||||
_lastFeedStatus = PhotonFeeder::FeedResult::SUCCESS;
|
||||
return true;
|
||||
}
|
||||
else{
|
||||
_lastFeedStatus = PhotonFeeder::FeedResult::COULDNT_REACH;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else{
|
||||
_lastFeedStatus = PhotonFeeder::FeedResult::COULDNT_REACH;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool PhotonFeeder::moveForwardSequence(uint16_t tenths_mm, bool first_attempt) {
|
||||
/* moveForwardSequence()
|
||||
* This function actually handles the translation of the mm movement to driving the motor to the right position based on encoder ticks.
|
||||
|
||||
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.
|
||||
|
||||
* 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.
|
||||
* Because the tick count is only ever an approximation of the precise mm position, any rounding done from the mm->tick conversion will
|
||||
* result in a significant amount of accrued error.
|
||||
*
|
||||
* 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
|
||||
* first finding the new requested position, then converting this to ticks _based on the startup 0 tick position_. This is similar to
|
||||
* absolute positioning vs. relative positioning in Marlin. Every mm->tick calculation needs to be done based on the initial 0 tick position.
|
||||
|
||||
returns true if we reach position within timeout, returns false if we timeout. does NOT set _lastFeedStatus.
|
||||
*
|
||||
*/
|
||||
|
||||
signed long goal_mm, timeout, signed_mm, current_tick;
|
||||
|
||||
timeout = tenths_mm * TIMEOUT_TIME_PER_TENTH_MM;
|
||||
|
||||
goal_mm = _position + tenths_mm;
|
||||
|
||||
// calculating goal_tick based on absolute, not relative position
|
||||
|
||||
// float goal_tick_precise_f = goal_mm * TICKS_PER_TENTH_MM;
|
||||
// volatile int goal_tick_f = round(goal_tick_precise_f);
|
||||
|
||||
signed long goal_tick_precise = goal_mm * THOUSANDTHS_TICKS_PER_TENTH_MM;
|
||||
int goal_tick = 0;
|
||||
|
||||
if (goal_tick_precise >= 0) {
|
||||
goal_tick = (goal_tick_precise + 500) / 1000; // round a positive number
|
||||
} else {
|
||||
goal_tick = (goal_tick_precise - 500) / 1000; // round a negative integer
|
||||
}
|
||||
|
||||
int peel_delay = PEEL_TIME_PER_TENTH_MM * tenths_mm;
|
||||
|
||||
// peel film for calculated time
|
||||
peel(true);
|
||||
delay(peel_delay);
|
||||
peel(false);
|
||||
delay(PEEL_BACKOFF_TIME);
|
||||
brakePeel();
|
||||
|
||||
// drive forward with ease in
|
||||
for(int i=150;i<255;i=i+3){
|
||||
driveValue(true, i);
|
||||
delay(1);
|
||||
}
|
||||
|
||||
//prepping variables for stall detection
|
||||
int tick_history[20] = {1, 100, 1, 100, 1, 100, 1, 100, 1, 100, 1, 100, 1, 100, 1, 100, 1, 100, 1, 100};
|
||||
int tick_history_index = 0;
|
||||
int delta = 5;
|
||||
uint32_t stallCooldownTime = millis();
|
||||
bool stallCooldown = false;
|
||||
uint32_t last_stall_position_sample_time = millis();
|
||||
|
||||
// setting start time for measuring timeout
|
||||
uint32_t start_time = millis();
|
||||
|
||||
// if it's not our first attempt, or it's thick tape, we drive full tilt
|
||||
// otherwise, we drive slow to start and ease our way up to the tick
|
||||
int currentDriveValue;
|
||||
if(first_attempt == false || _beefy_boi == true){
|
||||
currentDriveValue = 255;
|
||||
}
|
||||
else {
|
||||
currentDriveValue = 30;
|
||||
}
|
||||
|
||||
//volatile int test = 0;
|
||||
|
||||
//monitor loop
|
||||
while(millis() < start_time + timeout + 20){
|
||||
|
||||
//getting encoder position
|
||||
current_tick = _encoder->getPosition();
|
||||
|
||||
//calculating error
|
||||
int error = goal_tick - current_tick;
|
||||
|
||||
// if we're close enough it's time to ease in to final position
|
||||
if(error < DRIVE_APPROACH_FINAL_TICKS){
|
||||
|
||||
// updating tick history, calculate tick delta in last 20ms
|
||||
if(millis() > last_stall_position_sample_time + 1){
|
||||
|
||||
last_stall_position_sample_time = millis();
|
||||
tick_history[tick_history_index] = current_tick;
|
||||
|
||||
if(tick_history_index > 18){
|
||||
tick_history_index = 0;
|
||||
}
|
||||
else{
|
||||
tick_history_index++;
|
||||
}
|
||||
|
||||
int max = *std::max_element(tick_history, tick_history + 20);
|
||||
int min = *std::min_element(tick_history, tick_history + 20);
|
||||
delta = max - min;
|
||||
|
||||
}
|
||||
|
||||
// reset stallcooldown if it's been a minute, motor has had time to react to new drive value
|
||||
if(stallCooldownTime + 5 < millis()){
|
||||
stallCooldown = false;
|
||||
}
|
||||
|
||||
// if stall detected, and it's been a minute since we've adjusted currentDriveValue, increase currentDriveValue
|
||||
if(delta <= 5 && stallCooldown == false){
|
||||
|
||||
currentDriveValue = currentDriveValue + 3;
|
||||
if(currentDriveValue > 255){
|
||||
currentDriveValue = 255;
|
||||
}
|
||||
|
||||
stallCooldown = true;
|
||||
stallCooldownTime = millis();
|
||||
|
||||
}
|
||||
|
||||
//driving calculated value
|
||||
driveValue(true, currentDriveValue);
|
||||
|
||||
//we've reached the final position!
|
||||
if(error < 1){
|
||||
|
||||
//immediately stop
|
||||
brakeDrive();
|
||||
|
||||
// int brakeTick = _encoder->getPosition();
|
||||
|
||||
// capture time at ss settle start
|
||||
uint32_t ssStartTime = millis();
|
||||
|
||||
// sample ticks until we've hit steady state
|
||||
while (delta > 0 && ssStartTime + 200 > millis()){
|
||||
|
||||
//watching for steady state ticks
|
||||
if(millis() > last_stall_position_sample_time + 3){
|
||||
|
||||
//getting encoder position
|
||||
current_tick = _encoder->getPosition();
|
||||
|
||||
last_stall_position_sample_time = millis();
|
||||
tick_history[tick_history_index] = current_tick;
|
||||
|
||||
if(tick_history_index > 18){
|
||||
tick_history_index = 0;
|
||||
}
|
||||
else{
|
||||
tick_history_index++;
|
||||
}
|
||||
|
||||
int max = *std::max_element(tick_history, tick_history + 20);
|
||||
int min = *std::min_element(tick_history, tick_history + 20);
|
||||
delta = max-min;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// int ssTick = _encoder->getPosition();
|
||||
// volatile int coast = ssTick - brakeTick;
|
||||
|
||||
// updating internal position to the goal position because we reached it
|
||||
_position = goal_mm;
|
||||
|
||||
// Resetting internal position count so we dont creep up into our 2,147,483,647 limit on the variable
|
||||
// We can only do this when the exact tick we move to is a whole number so we don't accrue any drift
|
||||
if(goal_tick_precise == goal_tick * 1000){
|
||||
resetEncoderPosition(_encoder->getPosition() - goal_tick);
|
||||
setMmPosition(0);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// still far from final, just drive full force
|
||||
else{
|
||||
drive(true);
|
||||
}
|
||||
}
|
||||
// brake to kill any coast
|
||||
halt();
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool PhotonFeeder::moveBackwardSequence(bool forward, uint16_t tenths_mm) {
|
||||
/* moveInternal()
|
||||
* This function actually handles the translation of the mm movement to driving the motor to the right position based on encoder ticks.
|
||||
* 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.
|
||||
* Because the tick count is only ever an approximation of the precise mm position, any rounding done from the mm->tick conversion will
|
||||
* result in a signficiant amount of accrued error.
|
||||
*
|
||||
* 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
|
||||
* first finding the new requested position, then converting this to ticks _based on the startup 0 tick position_. This is similar to
|
||||
* absolute positioning vs. relative positioning in Marlin. Every mm->tick calculation needs to be done based on the initial 0 tick position.
|
||||
*
|
||||
*/
|
||||
|
||||
signed long goal_mm, timeout, signed_mm, current_tick;
|
||||
|
||||
timeout = tenths_mm * TIMEOUT_TIME_PER_TENTH_MM;
|
||||
|
||||
// doing final position math
|
||||
if(forward){
|
||||
goal_mm = _position + tenths_mm;
|
||||
}
|
||||
else{
|
||||
goal_mm = _position - tenths_mm;
|
||||
}
|
||||
|
||||
// calculating goal_tick based on absolute, not relative position
|
||||
// float goal_tick_precise = goal_mm * TICKS_PER_TENTH_MM;
|
||||
// int goal_tick = round(goal_tick_precise);
|
||||
|
||||
long goal_tick_precise = goal_mm * THOUSANDTHS_TICKS_PER_TENTH_MM;
|
||||
int goal_tick = 0;
|
||||
|
||||
if (goal_tick_precise > 0) {
|
||||
goal_tick = (goal_tick_precise + 500) / 1000; // round a positive number
|
||||
} else {
|
||||
goal_tick = (goal_tick_precise - 500) / 1000; // round a negative integer
|
||||
}
|
||||
|
||||
uint32_t start_time = millis();
|
||||
|
||||
// in the direction of the goal with ease in
|
||||
for(size_t i=0; i<255; i=i+5){
|
||||
driveValue(forward, i);
|
||||
delay(1);
|
||||
}
|
||||
drive(forward);
|
||||
|
||||
while(millis() < start_time + timeout + 20){
|
||||
|
||||
current_tick = _encoder->getPosition();
|
||||
int error;
|
||||
|
||||
if(forward){
|
||||
error = goal_tick - current_tick;
|
||||
}
|
||||
else{
|
||||
error = current_tick - goal_tick;
|
||||
}
|
||||
if (error < 0){
|
||||
brakeDrive();
|
||||
_position = goal_mm;
|
||||
|
||||
//delay to let position settle
|
||||
delay(50);
|
||||
|
||||
// Resetting internal position count so we dont creep up into our 2,147,483,647 limit on the variable
|
||||
// We can only do this when the exact tick we move to is a whole number so we don't accrue any drift
|
||||
if(goal_tick_precise == goal_tick * 1000){
|
||||
resetEncoderPosition(_encoder->getPosition() - goal_tick);
|
||||
setMmPosition(0);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
}
|
||||
// brake to kill any coast
|
||||
brakeDrive();
|
||||
|
||||
return false;
|
||||
}
|
||||
@@ -0,0 +1,96 @@
|
||||
#ifndef _PHOTON_FEEDER_H
|
||||
#define _PHOTON_FEEDER_H
|
||||
|
||||
#include <functional>
|
||||
|
||||
#ifndef MOTOR_DEPS
|
||||
#define MOTOR_DEPS
|
||||
#include <RotaryEncoder.h>
|
||||
#endif
|
||||
|
||||
|
||||
#define VENDOR_SPECIFIC_OPTIONS_LENGTH 20 // Chosen by fair d20 roll
|
||||
|
||||
|
||||
class PhotonFeeder {
|
||||
public:
|
||||
enum FeedResult {
|
||||
SUCCESS,
|
||||
INVALID_LENGTH,
|
||||
COULDNT_REACH,
|
||||
UNKNOWN_ERROR
|
||||
};
|
||||
|
||||
PhotonFeeder(
|
||||
uint8_t drive1_pin,
|
||||
uint8_t drive2_pin,
|
||||
uint8_t peel1_pin,
|
||||
uint8_t peel2_pin,
|
||||
uint8_t led_red,
|
||||
uint8_t led_green,
|
||||
uint8_t led_blue,
|
||||
RotaryEncoder* encoder
|
||||
);
|
||||
|
||||
FeedResult getMoveResult();
|
||||
|
||||
// Async Functions
|
||||
void peel(bool forward);
|
||||
void drive(bool forward);
|
||||
void driveValue(bool forward, uint8_t value);
|
||||
void driveBrakeValue(bool forward, uint8_t value);
|
||||
void brakePeel();
|
||||
void brakeDrive();
|
||||
void halt();
|
||||
void set_rgb(bool red, bool green, bool blue);
|
||||
uint16_t calculateExpectedFeedTime(uint8_t distance, bool forward);
|
||||
void setMmPosition(uint16_t position);
|
||||
void resetEncoderPosition(uint16_t position);
|
||||
|
||||
// Blocking Functions
|
||||
void feedDistance(uint16_t tenths_mm, bool forward);
|
||||
bool checkLoaded();
|
||||
|
||||
// Vendor Specific Functions
|
||||
// Should probably be virtual with hardware overriding it as well as return status
|
||||
void vendorSpecific(uint8_t options[VENDOR_SPECIFIC_OPTIONS_LENGTH], uint8_t* response);
|
||||
void identify();
|
||||
void showVersion();
|
||||
|
||||
bool _first_feed_since_load = true;
|
||||
|
||||
private:
|
||||
uint8_t _drive1_pin;
|
||||
uint8_t _drive2_pin;
|
||||
|
||||
uint8_t _peel1_pin;
|
||||
uint8_t _peel2_pin;
|
||||
|
||||
uint8_t _led_red;
|
||||
uint8_t _led_green;
|
||||
uint8_t _led_blue;
|
||||
|
||||
uint8_t _retry_limit = 3;
|
||||
|
||||
String _version = VERSION_STRING;
|
||||
|
||||
bool _beefy_boi = false;
|
||||
// flag for if we should just drive full tilt
|
||||
// set when thick tape is detected
|
||||
// reset when tape is driven fast through buttons (likely swapping tape)
|
||||
|
||||
FeedResult _lastFeedStatus;
|
||||
|
||||
signed long _position;
|
||||
|
||||
RotaryEncoder* _encoder;
|
||||
|
||||
uint8_t _firmware_version = 2;
|
||||
|
||||
bool moveForward(uint16_t tenths_mm);
|
||||
bool moveBackward(uint16_t tenths_mm);
|
||||
bool moveForwardSequence(uint16_t tenths_mm, bool first_attempt);
|
||||
bool moveBackwardSequence(bool forward, uint16_t tenths_mm);
|
||||
};
|
||||
|
||||
#endif //_PHOTON_FEEDER_H
|
||||
@@ -0,0 +1,332 @@
|
||||
#include "PhotonFeederProtocol.h"
|
||||
#include "PhotonNetworkLayer.h"
|
||||
|
||||
#include "rs485/protocols/checksums/crc8_107.h"
|
||||
|
||||
#define MAX_PROTOCOL_VERSION 1
|
||||
|
||||
typedef enum {
|
||||
STATUS_OK = 0x00,
|
||||
STATUS_WRONG_FEEDER_ID = 0x01,
|
||||
STATUS_COULDNT_REACH = 0x02,
|
||||
STATUS_UNINITIALIZED_FEEDER = 0x03,
|
||||
STATUS_FEEDING_IN_PROGRESS = 0x04,
|
||||
STATUS_FAIL = 0x05,
|
||||
|
||||
STATUS_TIMEOUT = 0xFE,
|
||||
STATUS_UNKNOWN_ERROR = 0xFF
|
||||
} FeederStatus;
|
||||
|
||||
typedef enum {
|
||||
// Unicast Commands
|
||||
GET_FEEDER_ID = 0x01,
|
||||
INITIALIZE_FEEDER = 0x02,
|
||||
GET_VERSION = 0x03,
|
||||
MOVE_FEED_FORWARD = 0x04,
|
||||
MOVE_FEED_BACKWARD = 0x05,
|
||||
MOVE_FEED_STATUS = 0x06,
|
||||
|
||||
VENDOR_OPTIONS = 0xbf,
|
||||
|
||||
// Broadcast Commands
|
||||
GET_FEEDER_ADDRESS = 0xc0,
|
||||
IDENTIFY_FEEDER = 0xc1,
|
||||
PROGRAM_FEEDER_FLOOR = 0xc2,
|
||||
UNINITIALIZED_FEEDERS_RESPOND = 0xc3
|
||||
// EXTENDED_COMMAND = 0xff, Unused, reserved for future use
|
||||
} FeederCommand;
|
||||
|
||||
PhotonFeederProtocol::PhotonFeederProtocol(
|
||||
PhotonFeeder *feeder,
|
||||
FeederFloor *feederFloor,
|
||||
PhotonNetworkLayer* network,
|
||||
const uint8_t *uuid,
|
||||
size_t uuid_length):
|
||||
_feeder(feeder),
|
||||
_feederFloor(feederFloor),
|
||||
_network(network),
|
||||
_initialized(false) {
|
||||
memset(_uuid, 0, UUID_LENGTH);
|
||||
memcpy(_uuid, uuid, (uuid_length < UUID_LENGTH) ? uuid_length : UUID_LENGTH);
|
||||
}
|
||||
|
||||
void PhotonFeederProtocol::tick() {
|
||||
bool hasPacket = _network->getPacket(commandBuffer, sizeof(commandBuffer));
|
||||
|
||||
if(! hasPacket) {
|
||||
return;
|
||||
}
|
||||
|
||||
switch(command.commandId) {
|
||||
//switch(packetBuffer.packet.payload.command.commandId) {
|
||||
case GET_FEEDER_ID:
|
||||
handleGetFeederId();
|
||||
break;
|
||||
case INITIALIZE_FEEDER:
|
||||
handleInitializeFeeder();
|
||||
break;
|
||||
case GET_VERSION:
|
||||
handleGetVersion();
|
||||
break;
|
||||
case MOVE_FEED_FORWARD:
|
||||
handleMoveFeedForward();
|
||||
break;
|
||||
case MOVE_FEED_BACKWARD:
|
||||
handleMoveFeedBackward();
|
||||
break;
|
||||
case MOVE_FEED_STATUS:
|
||||
handleMoveFeedStatus();
|
||||
break;
|
||||
case VENDOR_OPTIONS:
|
||||
handleVendorOptions();
|
||||
break;
|
||||
case GET_FEEDER_ADDRESS:
|
||||
handleGetFeederAddress();
|
||||
break;
|
||||
case IDENTIFY_FEEDER:
|
||||
handleIdentifyFeeder();
|
||||
break;
|
||||
case PROGRAM_FEEDER_FLOOR:
|
||||
handleProgramFeederFloor();
|
||||
break;
|
||||
case UNINITIALIZED_FEEDERS_RESPOND:
|
||||
handleUnitializedFeedersRespond();
|
||||
default:
|
||||
// Something has gone wrong if execution ever gets here.
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
bool PhotonFeederProtocol::isInitialized() {
|
||||
return _initialized;
|
||||
}
|
||||
|
||||
bool PhotonFeederProtocol::guardInitialized() {
|
||||
// Checks if the feeder is initialized and returns an error if it hasn't been
|
||||
if (_initialized) {
|
||||
return true;
|
||||
}
|
||||
|
||||
response = {
|
||||
.status = STATUS_UNINITIALIZED_FEEDER,
|
||||
};
|
||||
memcpy(response.initializeFeeder.uuid, _uuid, UUID_LENGTH);
|
||||
|
||||
transmitResponse(sizeof(InitializeFeederResponse));
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
void PhotonFeederProtocol::handleGetFeederId() {
|
||||
response = {
|
||||
.status = STATUS_OK
|
||||
};
|
||||
memcpy(response.getFeederId.uuid, _uuid, UUID_LENGTH);
|
||||
|
||||
transmitResponse(sizeof(GetFeederIdResponse));
|
||||
}
|
||||
|
||||
void PhotonFeederProtocol::handleInitializeFeeder() {
|
||||
// Check uuid is correct, if not return a Wrong Feeder UUID error
|
||||
bool requestedUUIDMatchesMine = memcmp(command.initializeFeeder.uuid, _uuid, UUID_LENGTH) == 0;
|
||||
if (! requestedUUIDMatchesMine) {
|
||||
response = {
|
||||
.status = STATUS_WRONG_FEEDER_ID,
|
||||
};
|
||||
memcpy(response.initializeFeeder.uuid, _uuid, UUID_LENGTH);
|
||||
|
||||
transmitResponse(sizeof(InitializeFeederResponse));
|
||||
return;
|
||||
}
|
||||
|
||||
// Mark this feeder as initialized
|
||||
_initialized = true;
|
||||
|
||||
response = {
|
||||
.status = STATUS_OK,
|
||||
};
|
||||
memcpy(response.initializeFeeder.uuid, _uuid, UUID_LENGTH);
|
||||
|
||||
transmitResponse(sizeof(InitializeFeederResponse));
|
||||
}
|
||||
|
||||
void PhotonFeederProtocol::handleGetVersion() {
|
||||
response = {
|
||||
.status = STATUS_OK,
|
||||
.protocolVersion = {
|
||||
.version = MAX_PROTOCOL_VERSION,
|
||||
},
|
||||
};
|
||||
|
||||
transmitResponse(sizeof(GetProtocolVersionResponse));
|
||||
}
|
||||
|
||||
void PhotonFeederProtocol::move(uint8_t distance, bool forward) {
|
||||
if (! guardInitialized()) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint16_t time = _feeder->calculateExpectedFeedTime(distance, forward);
|
||||
uint16_t timeMSB = (time >> 8) | (time << 8);
|
||||
|
||||
response = {
|
||||
.status = STATUS_OK,
|
||||
.expectedTimeToFeed = {
|
||||
.expectedFeedTime = timeMSB,
|
||||
},
|
||||
};
|
||||
|
||||
transmitResponse(sizeof(FeedDistanceResponse));
|
||||
|
||||
// perform a blocking movement
|
||||
_feeder->feedDistance(distance, forward);
|
||||
|
||||
// clear the serial buffer to remove any packets that came in while we were blocking
|
||||
_network->clearPackets();
|
||||
|
||||
}
|
||||
|
||||
void PhotonFeederProtocol::handleMoveFeedForward() {
|
||||
move(command.move.distance, true);
|
||||
|
||||
}
|
||||
|
||||
void PhotonFeederProtocol::handleMoveFeedBackward() {
|
||||
move(command.move.distance, false);
|
||||
|
||||
}
|
||||
|
||||
void PhotonFeederProtocol::handleMoveFeedStatus() {
|
||||
PhotonFeeder::FeedResult result = _feeder->getMoveResult();
|
||||
uint8_t moveResponseStatus;
|
||||
|
||||
switch (result)
|
||||
{
|
||||
case PhotonFeeder::FeedResult::SUCCESS:
|
||||
moveResponseStatus = STATUS_OK;
|
||||
break;
|
||||
|
||||
case PhotonFeeder::FeedResult::INVALID_LENGTH: // For Now Handle Invalid Length & Motor Fault The Same
|
||||
case PhotonFeeder::FeedResult::COULDNT_REACH:
|
||||
moveResponseStatus = STATUS_COULDNT_REACH;
|
||||
break;
|
||||
|
||||
case PhotonFeeder::FeedResult::UNKNOWN_ERROR:
|
||||
default:
|
||||
moveResponseStatus = STATUS_UNKNOWN_ERROR;
|
||||
break;
|
||||
}
|
||||
|
||||
response = {
|
||||
.status = moveResponseStatus,
|
||||
};
|
||||
|
||||
transmitResponse();
|
||||
}
|
||||
|
||||
void PhotonFeederProtocol::handleGetFeederAddress() {
|
||||
// Check For Feeder Match
|
||||
bool requestedUUIDMatchesMine = memcmp(command.initializeFeeder.uuid, _uuid, UUID_LENGTH) == 0;
|
||||
if (! requestedUUIDMatchesMine) {
|
||||
return;
|
||||
}
|
||||
|
||||
response = {
|
||||
.status = STATUS_OK,
|
||||
};
|
||||
|
||||
|
||||
|
||||
transmitResponse();
|
||||
}
|
||||
|
||||
void PhotonFeederProtocol::handleVendorOptions() {
|
||||
if (! guardInitialized()) {
|
||||
return;
|
||||
}
|
||||
response = {
|
||||
.status = STATUS_OK,
|
||||
.vendorOptions = VendorOptionsResponse(),
|
||||
};
|
||||
|
||||
_feeder->vendorSpecific(command.vendorOptions.options, response.vendorOptions.options);
|
||||
|
||||
transmitResponse(sizeof(VendorOptionsResponse));
|
||||
}
|
||||
|
||||
void PhotonFeederProtocol::handleIdentifyFeeder() {
|
||||
|
||||
// Check For Feeder Match
|
||||
bool requestedUUIDMatchesMine = memcmp(command.identifyFeeder.uuid, _uuid, UUID_LENGTH) == 0;
|
||||
if (! requestedUUIDMatchesMine) {
|
||||
return;
|
||||
}
|
||||
|
||||
response = {
|
||||
.status = STATUS_OK,
|
||||
};
|
||||
|
||||
transmitResponse();
|
||||
|
||||
_feeder->identify();
|
||||
|
||||
}
|
||||
|
||||
void PhotonFeederProtocol::handleProgramFeederFloor() {
|
||||
bool addressWritten = _feederFloor->write_floor_address(command.programFeederFloorAddress.address);
|
||||
|
||||
if(addressWritten){
|
||||
_network->setLocalAddress(command.programFeederFloorAddress.address);
|
||||
}
|
||||
|
||||
uint8_t feederStatus = addressWritten ? STATUS_OK : STATUS_FAIL;
|
||||
response = {
|
||||
.status = feederStatus,
|
||||
};
|
||||
|
||||
transmitResponse();
|
||||
}
|
||||
|
||||
void PhotonFeederProtocol::handleUnitializedFeedersRespond() {
|
||||
if (_initialized) {
|
||||
return; // Don't respond to this command at all since we've already been initialized
|
||||
}
|
||||
|
||||
response = {
|
||||
.status = STATUS_OK,
|
||||
};
|
||||
|
||||
memcpy(response.getFeederId.uuid, _uuid, UUID_LENGTH);
|
||||
|
||||
transmitResponse(sizeof(GetFeederIdResponse));
|
||||
}
|
||||
|
||||
void PhotonFeederProtocol::transmitResponse(uint8_t responseSize) {
|
||||
// responseSize is only the anonymous uninion in the response, not any variables common to all responses
|
||||
// We handle common values here.
|
||||
responseSize++; // uint8_t status
|
||||
|
||||
response.header = {
|
||||
.toAddress = PHOTON_NETWORK_CONTROLLER_ADDRESS,
|
||||
.fromAddress = _network->getLocalAddress(),
|
||||
.packetId = command.header.packetId,
|
||||
.payloadLength = responseSize,
|
||||
};
|
||||
|
||||
CRC8_107 crc;
|
||||
|
||||
crc.add(response.header.toAddress);
|
||||
crc.add(response.header.fromAddress);
|
||||
crc.add(response.header.packetId);
|
||||
crc.add(response.header.payloadLength);
|
||||
|
||||
for(uint8_t i = 0; i<response.header.payloadLength; i++){
|
||||
crc.add(responseBuffer[sizeof(PhotonPacketHeader) + i]);
|
||||
}
|
||||
|
||||
response.header.crc = crc;
|
||||
|
||||
size_t totalPacketLength = sizeof(PhotonPacketHeader) + response.header.payloadLength;
|
||||
_network->transmitPacket(responseBuffer, totalPacketLength);
|
||||
}
|
||||
@@ -0,0 +1,144 @@
|
||||
#ifndef _PHOTON_FEEDER_PROTOCOL_H
|
||||
#define _PHOTON_FEEDER_PROTOCOL_H
|
||||
|
||||
#include "FeederFloor.h"
|
||||
#include "PhotonFeeder.h"
|
||||
#include "PhotonNetworkLayer.h"
|
||||
|
||||
#define UUID_LENGTH 12
|
||||
|
||||
#define PHOTON_NETWORK_CONTROLLER_ADDRESS 0x00
|
||||
#define PHOTON_NETWORK_BROADCAST_ADDRESS 0xFF
|
||||
|
||||
#define PACKED __attribute__ ((packed))
|
||||
|
||||
struct PACKED PhotonPacketHeader {
|
||||
uint8_t toAddress;
|
||||
uint8_t fromAddress;
|
||||
uint8_t packetId;
|
||||
uint8_t payloadLength;
|
||||
uint8_t crc;
|
||||
};
|
||||
|
||||
struct PACKED MoveCommand {
|
||||
uint8_t distance;
|
||||
};
|
||||
|
||||
struct PACKED GetFeederAddressCommand {
|
||||
uint8_t uuid[UUID_LENGTH];
|
||||
};
|
||||
|
||||
struct PACKED InitializeFeederCommand {
|
||||
uint8_t uuid[UUID_LENGTH];
|
||||
};
|
||||
|
||||
struct PACKED VendorOptionsCommand {
|
||||
uint8_t options[VENDOR_SPECIFIC_OPTIONS_LENGTH];
|
||||
};
|
||||
|
||||
struct PACKED ProgramFeederFloorAddressCommand {
|
||||
uint8_t uuid[UUID_LENGTH];
|
||||
uint8_t address;
|
||||
};
|
||||
|
||||
struct PACKED IdentifyFeederCommand {
|
||||
uint8_t uuid[UUID_LENGTH];
|
||||
};
|
||||
|
||||
struct PACKED PhotonCommand {
|
||||
PhotonPacketHeader header;
|
||||
uint8_t commandId;
|
||||
union {
|
||||
MoveCommand move;
|
||||
GetFeederAddressCommand getFeederAddress;
|
||||
InitializeFeederCommand initializeFeeder;
|
||||
VendorOptionsCommand vendorOptions;
|
||||
ProgramFeederFloorAddressCommand programFeederFloorAddress;
|
||||
IdentifyFeederCommand identifyFeeder;
|
||||
};
|
||||
};
|
||||
|
||||
struct PACKED GetFeederIdResponse {
|
||||
uint8_t uuid[UUID_LENGTH];
|
||||
};
|
||||
|
||||
struct PACKED InitializeFeederResponse {
|
||||
uint8_t uuid[UUID_LENGTH];
|
||||
};
|
||||
|
||||
struct PACKED GetProtocolVersionResponse {
|
||||
uint8_t version;
|
||||
};
|
||||
|
||||
struct PACKED FeedDistanceResponse {
|
||||
uint16_t expectedFeedTime;
|
||||
};
|
||||
|
||||
struct PACKED VendorOptionsResponse {
|
||||
uint8_t options[VENDOR_SPECIFIC_OPTIONS_LENGTH];
|
||||
};
|
||||
|
||||
struct PACKED PhotonResponse {
|
||||
PhotonPacketHeader header;
|
||||
uint8_t status;
|
||||
union {
|
||||
GetFeederIdResponse getFeederId;
|
||||
InitializeFeederResponse initializeFeeder;
|
||||
GetProtocolVersionResponse protocolVersion;
|
||||
FeedDistanceResponse expectedTimeToFeed;
|
||||
VendorOptionsResponse vendorOptions;
|
||||
};
|
||||
};
|
||||
|
||||
class PhotonFeederProtocol {
|
||||
|
||||
public:
|
||||
PhotonFeederProtocol(
|
||||
PhotonFeeder *feeder,
|
||||
FeederFloor *feederFloor,
|
||||
PhotonNetworkLayer* network,
|
||||
const uint8_t *uuid,
|
||||
size_t uuid_length
|
||||
);
|
||||
void tick();
|
||||
|
||||
private:
|
||||
PhotonFeeder* _feeder;
|
||||
FeederFloor* _feederFloor;
|
||||
PhotonNetworkLayer* _network;
|
||||
uint8_t _uuid[UUID_LENGTH];
|
||||
bool _initialized;
|
||||
|
||||
union {
|
||||
PhotonCommand command;
|
||||
uint8_t commandBuffer[sizeof(PhotonCommand)];
|
||||
};
|
||||
|
||||
union {
|
||||
PhotonResponse response;
|
||||
uint8_t responseBuffer[sizeof(PhotonResponse)];
|
||||
};
|
||||
|
||||
// Unicast
|
||||
bool guardInitialized();
|
||||
void handleGetFeederId();
|
||||
void handleInitializeFeeder();
|
||||
void handleGetVersion();
|
||||
void handleMoveFeedForward();
|
||||
void handleMoveFeedBackward();
|
||||
void handleMoveFeedStatus();
|
||||
void handleVendorOptions();
|
||||
|
||||
// Broadcast
|
||||
void handleGetFeederAddress();
|
||||
void handleIdentifyFeeder();
|
||||
void handleProgramFeederFloor();
|
||||
void handleUnitializedFeedersRespond();
|
||||
|
||||
void move(uint8_t distance, bool forward);
|
||||
bool isInitialized();
|
||||
|
||||
void transmitResponse(uint8_t responseSize = 0);
|
||||
};
|
||||
|
||||
#endif //_PHOTON_FEEDER_PROTOCOL_H
|
||||
@@ -0,0 +1,83 @@
|
||||
#include "define.h"
|
||||
|
||||
#include "PhotonNetworkLayer.h"
|
||||
#include "FeederFloor.h"
|
||||
#include <Arduino.h>
|
||||
#include <cstring>
|
||||
#include <cstdio>
|
||||
|
||||
|
||||
#ifndef NATIVE
|
||||
#include "util.h"
|
||||
#endif
|
||||
|
||||
#define PHOTON_PROTOCOL_DEFAULT_TIMEOUT_MS 100
|
||||
#define PHOTON_INCOMING_BUFFER_SIZE 16
|
||||
#define RS485_CONTROL_DELAY 10
|
||||
|
||||
PhotonNetworkLayer::PhotonNetworkLayer(
|
||||
RS485Bus<RS485_BUS_BUFFER_SIZE>* bus,
|
||||
Packetizer* packetizer,
|
||||
FilterByValue* addressFilter,
|
||||
FeederFloor* feederFloor) :
|
||||
_bus(bus),
|
||||
_packetizer(packetizer),
|
||||
_addressFilter(addressFilter),
|
||||
_feederFloor(feederFloor),
|
||||
_local_address(0xFF) {
|
||||
|
||||
_local_address = _feederFloor->read_floor_address();
|
||||
_packetizer->setFilter(*_addressFilter);
|
||||
_packetizer->setFalsePacketVerificationTimeout(10000);
|
||||
_packetizer->setMaxReadTimeout(50000);
|
||||
_addressFilter->preValues.allowAll();
|
||||
_addressFilter->postValues.allow(0xFF);
|
||||
|
||||
// If floor address isn't set, _local_address is 0xFF and this function does nothing
|
||||
_addressFilter->postValues.allow(_local_address);
|
||||
}
|
||||
|
||||
void PhotonNetworkLayer::setLocalAddress(uint8_t address) {
|
||||
if(_local_address != 0xff) {
|
||||
// The old address is no longer valid
|
||||
_addressFilter->postValues.reject(_local_address);
|
||||
}
|
||||
|
||||
_local_address = address;
|
||||
_addressFilter->postValues.allow(_local_address);
|
||||
}
|
||||
|
||||
uint8_t PhotonNetworkLayer::getLocalAddress() {
|
||||
return _local_address;
|
||||
}
|
||||
|
||||
bool PhotonNetworkLayer::getPacket(uint8_t* buffer, size_t maxBufferLength) {
|
||||
// triggers if the packetizer detects that it has a packet
|
||||
if (! _packetizer->hasPacket()){
|
||||
return false;
|
||||
}
|
||||
|
||||
Packet packet = _packetizer->getPacket();
|
||||
for(size_t i = packet.startIndex; i <= packet.endIndex; i++) {
|
||||
buffer[i] = (*_bus)[i];
|
||||
}
|
||||
|
||||
// clear the packet
|
||||
_packetizer->clearPacket();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void PhotonNetworkLayer::clearPackets() {
|
||||
while(_packetizer->hasPacket()){
|
||||
_packetizer->clearPacket();
|
||||
}
|
||||
}
|
||||
|
||||
bool PhotonNetworkLayer::transmitPacket(const uint8_t *buffer, size_t buffer_length) {
|
||||
_packetizer->writePacket(buffer, buffer_length);
|
||||
|
||||
// TODO Handle write packet status result
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
#ifndef _PHOTON_NETWORK_LAYER_H
|
||||
#define _PHOTON_NETWORK_LAYER_H
|
||||
|
||||
#include "define.h"
|
||||
|
||||
#include "FeederFloor.h"
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include "PhotonPacketHandler.h"
|
||||
#include "Stream.h"
|
||||
|
||||
#include <rs485/filters/filter_by_value.h>
|
||||
#include <rs485/rs485bus.hpp>
|
||||
#include <rs485/packetizer.h>
|
||||
|
||||
#define PHOTON_NETWORK_MAX_PDU 32
|
||||
#define PHOTON_PROTOCOL_CHECKSUM_LENGTH 2
|
||||
|
||||
class PhotonNetworkLayer
|
||||
{
|
||||
public:
|
||||
PhotonNetworkLayer(
|
||||
RS485Bus<RS485_BUS_BUFFER_SIZE>* bus,
|
||||
Packetizer* packetizer,
|
||||
FilterByValue* addressFilter,
|
||||
FeederFloor* feederFloor);
|
||||
|
||||
void setLocalAddress(uint8_t address);
|
||||
uint8_t getLocalAddress();
|
||||
|
||||
bool getPacket(uint8_t* buffer, size_t maxBufferLength);
|
||||
|
||||
bool transmitPacket(const uint8_t *buffer, size_t buffer_length);
|
||||
|
||||
void clearPackets();
|
||||
|
||||
private:
|
||||
RS485Bus<RS485_BUS_BUFFER_SIZE>* _bus;
|
||||
Packetizer* _packetizer;
|
||||
FilterByValue* _addressFilter;
|
||||
FeederFloor* _feederFloor;
|
||||
uint8_t _send_buffer[64];
|
||||
|
||||
uint8_t _local_address;
|
||||
};
|
||||
|
||||
#endif //_PHOTON_NETWORK_LAYER_H
|
||||
@@ -0,0 +1,14 @@
|
||||
#ifndef _PHOTON_PROTOCOL_HANDLER_H
|
||||
#define _PHOTON_PROTOCOL_HANDLER_H
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
|
||||
class PhotonNetworkLayer;
|
||||
|
||||
class PhotonPacketHandler {
|
||||
public:
|
||||
virtual void handle(PhotonNetworkLayer *instance, uint8_t *buffer, size_t buffer_length) = 0;
|
||||
};
|
||||
|
||||
#endif //_PHOTON_PROTOCOL_HANDLER_H
|
||||
@@ -0,0 +1,64 @@
|
||||
#include "bootloader.h"
|
||||
#include "stm32f0xx_hal.h"
|
||||
#include "stm32f0xx.h"
|
||||
#include "stm32_def.h"
|
||||
|
||||
#define SYSMEM_ADDRESS 0x1FFFEC00
|
||||
|
||||
|
||||
void reboot_into_bootloader() {
|
||||
const uint32_t jump_address = *(__IO uint32_t*)(SYSMEM_ADDRESS + 4);
|
||||
|
||||
/* Disable peripherals */
|
||||
HAL_RCC_DeInit();
|
||||
HAL_DeInit();
|
||||
|
||||
/* Reset SysTick */
|
||||
SysTick->CTRL = 0;
|
||||
SysTick->LOAD = 0;
|
||||
SysTick->VAL = 0;
|
||||
|
||||
/* Disable PLL */
|
||||
|
||||
/* Disable interrupts */
|
||||
__disable_irq();
|
||||
|
||||
/* HACK
|
||||
Workaround issue where RS485 transceiver prevents using the system bootloader via UART.
|
||||
|
||||
This occurs with some variants of the RS485 transceiver we use because we forgot pullup/pulldowns on the ~RE/DE
|
||||
pins. They're left floating and sometimes that means the ~RE is enabled which prevents the UART programmer from
|
||||
communicating with the STM32.
|
||||
|
||||
This works around this by configuring those pins before jumping to the bootloader and *locking* them, so that
|
||||
they stick. This only work when jumping to the bootloader from the firmware, it won't work on a cold boot.
|
||||
*/
|
||||
__HAL_RCC_GPIOA_CLK_ENABLE();
|
||||
/* ~RE pin should be pulled high to disable the RS485 receiver. */
|
||||
GPIO_InitTypeDef RE_Init;
|
||||
RE_Init.Pin = GPIO_PIN_11;
|
||||
RE_Init.Mode = GPIO_MODE_OUTPUT_PP;
|
||||
RE_Init.Pull = GPIO_PULLUP;
|
||||
RE_Init.Speed = GPIO_SPEED_FREQ_LOW;
|
||||
HAL_GPIO_Init(GPIOA, &RE_Init);
|
||||
HAL_GPIO_WritePin(GPIOA, RE_Init.Pin, GPIO_PIN_SET);
|
||||
HAL_GPIO_LockPin(GPIOA, RE_Init.Pin);
|
||||
|
||||
/* DE should be pulled low to disable the RS485 transmitter */
|
||||
GPIO_InitTypeDef DE_Init;
|
||||
DE_Init.Pin = GPIO_PIN_12;
|
||||
DE_Init.Mode = GPIO_MODE_OUTPUT_PP;
|
||||
DE_Init.Pull = GPIO_PULLDOWN;
|
||||
DE_Init.Speed = GPIO_SPEED_FREQ_LOW;
|
||||
HAL_GPIO_Init(GPIOA, &DE_Init);
|
||||
HAL_GPIO_WritePin(GPIOA, DE_Init.Pin, GPIO_PIN_RESET);
|
||||
HAL_GPIO_LockPin(GPIOA, DE_Init.Pin);
|
||||
|
||||
/* Jump to bootloader */
|
||||
__HAL_SYSCFG_REMAPMEMORY_SYSTEMFLASH();
|
||||
__set_MSP(*(__IO uint32_t*)SYSMEM_ADDRESS);
|
||||
__enable_irq();
|
||||
((void (*)(void)) jump_address)();
|
||||
|
||||
while(1);
|
||||
}
|
||||
@@ -0,0 +1,3 @@
|
||||
#pragma once
|
||||
|
||||
void reboot_into_bootloader();
|
||||
@@ -0,0 +1,36 @@
|
||||
#define DE PA12
|
||||
#define _RE PA11
|
||||
|
||||
#define LED_R PA7
|
||||
#define LED_G PA6
|
||||
#define LED_B PA4
|
||||
|
||||
#define SW1 PB1
|
||||
#define SW2 PB0
|
||||
|
||||
#define PEEL1 PB3
|
||||
#define PEEL2 PA15
|
||||
|
||||
#define ONE_WIRE PA8
|
||||
#define MOTOR_ENABLE PA2
|
||||
|
||||
#define LONG_PRESS_DELAY 500
|
||||
|
||||
#define RS485_BUS_BUFFER_SIZE 64
|
||||
|
||||
// #define PVT Motors (long shaft, long cable, manually flipping power wires)
|
||||
//#define PVT
|
||||
|
||||
#ifdef PVT
|
||||
// PVT Motor
|
||||
#define DRIVE1 PB4
|
||||
#define DRIVE2 PB5
|
||||
#define DRIVE_ENC_A PB6
|
||||
#define DRIVE_ENC_B PB7
|
||||
#else
|
||||
// MP Motor
|
||||
#define DRIVE1 PB5
|
||||
#define DRIVE2 PB4
|
||||
#define DRIVE_ENC_A PB7
|
||||
#define DRIVE_ENC_B PB6
|
||||
#endif
|
||||
@@ -0,0 +1,307 @@
|
||||
/*
|
||||
Photon Feeder Firmware
|
||||
Part of the LumenPnP Project
|
||||
MPL v2
|
||||
2025
|
||||
*/
|
||||
|
||||
#include "define.h"
|
||||
|
||||
#ifdef UNIT_TEST
|
||||
#include <ArduinoFake.h>
|
||||
#else
|
||||
#include <Arduino.h>
|
||||
#include <HardwareSerial.h>
|
||||
#include <OneWire.h>
|
||||
#include <ArduinoUniqueID.h>
|
||||
#include <rs485/rs485bus.hpp>
|
||||
#endif // UNIT_TEST
|
||||
|
||||
#ifndef MOTOR_DEPS
|
||||
#define MOTOR_DEPS
|
||||
|
||||
#include <RotaryEncoder.h>
|
||||
|
||||
#endif
|
||||
|
||||
#include "FeederFloor.h"
|
||||
#include "PhotonFeeder.h"
|
||||
#include "PhotonFeederProtocol.h"
|
||||
#include "PhotonNetworkLayer.h"
|
||||
|
||||
#include <rs485/rs485bus.hpp>
|
||||
#include <rs485/bus_adapters/hardware_serial.h>
|
||||
#include <rs485/filters/filter_by_value.h>
|
||||
#include <rs485/protocols/photon.h>
|
||||
#include <rs485/packetizer.h>
|
||||
|
||||
#include "bootloader.h"
|
||||
|
||||
#define BAUD_RATE 57600
|
||||
|
||||
//-----
|
||||
// Global Variables
|
||||
//-----
|
||||
|
||||
#ifdef UNIT_TEST
|
||||
StreamFake ser();
|
||||
#else
|
||||
HardwareSerial ser(PA10, PA9);
|
||||
#endif // ARDUINO
|
||||
|
||||
// EEPROM
|
||||
OneWire oneWire(ONE_WIRE);
|
||||
FeederFloor feederFloor(&oneWire);
|
||||
|
||||
// RS485
|
||||
HardwareSerialBusIO busIO(&ser);
|
||||
RS485Bus<RS485_BUS_BUFFER_SIZE> bus(busIO, _RE, DE);
|
||||
PhotonProtocol photon_protocol;
|
||||
Packetizer packetizer(bus, photon_protocol);
|
||||
FilterByValue addressFilter(0);
|
||||
|
||||
// Encoder
|
||||
RotaryEncoder encoder(DRIVE_ENC_A, DRIVE_ENC_B, RotaryEncoder::LatchMode::TWO03);
|
||||
|
||||
// Flags
|
||||
bool drive_mode = false;
|
||||
bool driving = false;
|
||||
bool driving_direction = false;
|
||||
|
||||
// Feeder Class Instances
|
||||
PhotonFeeder *feeder;
|
||||
PhotonFeederProtocol *protocol;
|
||||
PhotonNetworkLayer *network;
|
||||
|
||||
//-------
|
||||
//FUNCTIONS
|
||||
//-------
|
||||
|
||||
void checkPosition()
|
||||
{
|
||||
encoder.tick(); // just call tick() to check the state.
|
||||
}
|
||||
|
||||
//-------
|
||||
//SETUP
|
||||
//-------
|
||||
|
||||
void setup() {
|
||||
pinMode(LED_R, OUTPUT);
|
||||
pinMode(LED_G, OUTPUT);
|
||||
pinMode(LED_B, OUTPUT);
|
||||
feeder->set_rgb(false, false, false);
|
||||
|
||||
pinMode(SW1, INPUT_PULLUP);
|
||||
pinMode(SW2, INPUT_PULLUP);
|
||||
pinMode(MOTOR_ENABLE, OUTPUT);
|
||||
digitalWrite(MOTOR_ENABLE, HIGH);
|
||||
|
||||
// Setup Feeder
|
||||
feeder = new PhotonFeeder(DRIVE1, DRIVE2, PEEL1, PEEL2, LED_R, LED_G, LED_B, &encoder);
|
||||
network = new PhotonNetworkLayer(&bus, &packetizer, &addressFilter, &feederFloor);
|
||||
protocol = new PhotonFeederProtocol(feeder, &feederFloor, network, UniqueID, UniqueIDsize);
|
||||
|
||||
byte addr = feederFloor.read_floor_address();
|
||||
|
||||
if(addr == 0xFF){ // not detected, turn red
|
||||
feeder->set_rgb(true, false, false);
|
||||
}
|
||||
else if (addr == 0x00){ //not programmed, turn blue
|
||||
feeder->set_rgb(false, false, true);
|
||||
}
|
||||
|
||||
//Starting rs-485 serial
|
||||
ser.begin(BAUD_RATE);
|
||||
|
||||
// attach interrupts for encoder pins
|
||||
attachInterrupt(digitalPinToInterrupt(DRIVE_ENC_A), checkPosition, CHANGE);
|
||||
attachInterrupt(digitalPinToInterrupt(DRIVE_ENC_B), checkPosition, CHANGE);
|
||||
|
||||
feeder->resetEncoderPosition(0);
|
||||
feeder->setMmPosition(0);
|
||||
|
||||
}
|
||||
|
||||
void lifetime(){
|
||||
// lifetime testing loop
|
||||
uint32_t counter = millis();
|
||||
uint32_t interval = 3000;
|
||||
while(true){
|
||||
if(millis() > counter + interval){
|
||||
//reset counter to millis()
|
||||
counter = millis();
|
||||
//move
|
||||
feeder->feedDistance(40, true);
|
||||
feeder->resetEncoderPosition(0);
|
||||
feeder->setMmPosition(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void showVersion(){
|
||||
|
||||
feeder->showVersion();
|
||||
|
||||
}
|
||||
|
||||
void topShortPress(){
|
||||
//turn led white for movement
|
||||
feeder->set_rgb(true, true, true);
|
||||
// move forward 2mm
|
||||
feeder->feedDistance(20, true);
|
||||
}
|
||||
|
||||
void bottomShortPress(){
|
||||
//turn led white for movement
|
||||
feeder->set_rgb(true, true, true);
|
||||
// move forward 2mm
|
||||
feeder->feedDistance(20, false);
|
||||
|
||||
if (feeder->getMoveResult() == PhotonFeeder::FeedResult::SUCCESS){
|
||||
feeder->set_rgb(false, false, false);
|
||||
}
|
||||
else{
|
||||
feeder->set_rgb(true, false, false);
|
||||
}
|
||||
}
|
||||
|
||||
void topLongPress(){
|
||||
//we've got a long top press, lets drive forward, tape or film depending on drive_mode
|
||||
if(drive_mode){
|
||||
feeder->peel(true);
|
||||
}
|
||||
else{
|
||||
//resetting first feed, since we could now have a new tape type
|
||||
feeder->_first_feed_since_load = true;
|
||||
feeder->drive(true);
|
||||
}
|
||||
// set flag for concurrency to know driving state
|
||||
driving = true;
|
||||
driving_direction = true;
|
||||
}
|
||||
|
||||
void bottomLongPress(){
|
||||
// moving in reverse, motor selected by drive_mode
|
||||
if(drive_mode){
|
||||
feeder->peel(false);
|
||||
}
|
||||
else{
|
||||
//resetting first feed, since we could now have a new tape type
|
||||
feeder->_first_feed_since_load = true;
|
||||
feeder->drive(false);
|
||||
}
|
||||
// set flag for concurrency to know driving state
|
||||
driving = true;
|
||||
driving_direction = false;
|
||||
|
||||
}
|
||||
|
||||
void bothLongPress(){
|
||||
//both are pressed, switching if we are driving tape or film
|
||||
|
||||
if(drive_mode){
|
||||
feeder->set_rgb(false, false, true);
|
||||
drive_mode = false;
|
||||
}
|
||||
else{
|
||||
feeder->set_rgb(true, true, false);
|
||||
drive_mode = true;
|
||||
}
|
||||
|
||||
//if both are held for a long time, we show current version id
|
||||
uint32_t timerStart = millis();
|
||||
|
||||
bool alreadyFlashed = false;
|
||||
|
||||
while( (!digitalRead(SW1) || !digitalRead(SW2))){
|
||||
//do nothing while waiting for debounce
|
||||
if((timerStart + 2000 < millis()) && !alreadyFlashed){
|
||||
feeder->set_rgb(false, false, false);
|
||||
showVersion();
|
||||
alreadyFlashed = true;
|
||||
}
|
||||
|
||||
// if held for a really long time, reboot into the bootloader
|
||||
if((timerStart + 4000 < millis())){
|
||||
for (int n = 0; n < 10; n++) {
|
||||
feeder->set_rgb(!(n % 2), false, n % 2);
|
||||
delay(100);
|
||||
}
|
||||
}
|
||||
if((timerStart + 6000 < millis())){
|
||||
feeder->set_rgb(true, false, true);
|
||||
reboot_into_bootloader();
|
||||
}
|
||||
}
|
||||
|
||||
//delay for debounce
|
||||
delay(50);
|
||||
feeder->set_rgb(false, false, false);
|
||||
}
|
||||
|
||||
inline void checkButtons() {
|
||||
if(!driving){
|
||||
// Checking bottom button
|
||||
if(!digitalRead(SW1)){
|
||||
delay(LONG_PRESS_DELAY);
|
||||
// if bottom long press
|
||||
if(!digitalRead(SW1)){
|
||||
// if both long press
|
||||
if(!digitalRead(SW2)){
|
||||
bothLongPress();
|
||||
}
|
||||
// if just bottom long press
|
||||
else{
|
||||
bottomLongPress();
|
||||
}
|
||||
}
|
||||
// if bottom short press
|
||||
else{
|
||||
bottomShortPress();
|
||||
}
|
||||
}
|
||||
// Checking top button
|
||||
if(!digitalRead(SW2)){
|
||||
delay(LONG_PRESS_DELAY);
|
||||
// if top long press
|
||||
if(!digitalRead(SW2)){
|
||||
// if both long press
|
||||
if(!digitalRead(SW1)){
|
||||
bothLongPress();
|
||||
}
|
||||
// if just top long press
|
||||
else{
|
||||
topLongPress();
|
||||
}
|
||||
}
|
||||
// if top short press
|
||||
else{
|
||||
topShortPress();
|
||||
}
|
||||
}
|
||||
}
|
||||
else{
|
||||
if((driving_direction && digitalRead(SW2)) || (!driving_direction && digitalRead(SW1))){
|
||||
//stop all motors
|
||||
feeder->halt();
|
||||
//reset encoder and mm position
|
||||
feeder->resetEncoderPosition(0);
|
||||
feeder->setMmPosition(0);
|
||||
driving = false;
|
||||
delay(5);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline void checkForRS485Packet() {
|
||||
protocol->tick();
|
||||
}
|
||||
|
||||
//------
|
||||
// MAIN CONTROL LOOP
|
||||
//------
|
||||
void loop() {
|
||||
checkButtons();
|
||||
checkForRS485Packet();
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
#include "util.h"
|
||||
|
||||
#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
|
||||
uint32_t htonl(uint32_t hostlong) {
|
||||
uint32_t tmp = (hostlong << 24) & 0xff000000;
|
||||
tmp |= (hostlong << 8) & 0x00ff0000;
|
||||
tmp |= (hostlong >> 8) & 0x0000ff00;
|
||||
tmp |= (hostlong >> 24) & 0x000000ff;
|
||||
return tmp;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
//#define htonl(x)
|
||||
//( (((uint8_t *)x)[0] << 24) | (((uint8_t *)x)[1] << 16) | (((uint8_t *)x)[2] << 8) | (((uint8_t *)x)[3]) )
|
||||
//( (((uint32_t)(x << 24 )) & 0xff000000) | (((uint32_t)(x << 8)) & 0x00ff0000) | ((x >> 8) & 0x0000ff00) | ((x >> 24) & 0x000000ff) )
|
||||
@@ -0,0 +1,26 @@
|
||||
#ifndef _UTIL_H
|
||||
#define _UTIL_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#ifndef NATIVE
|
||||
#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
|
||||
|
||||
uint32_t htonl(uint32_t hostlong);
|
||||
#define htons(x) ( ((x << 8) & 0xff00) | ((x >> 8) & 0xff) )
|
||||
#define ntohl(x) htonl(x)
|
||||
#define ntohs(x) htons(x)
|
||||
|
||||
#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
|
||||
|
||||
#define htonl(x) (x)
|
||||
#define htons(x) (x)
|
||||
#define ntohl(x) (x)
|
||||
#define ntohs(x) (x)
|
||||
|
||||
#else
|
||||
#error Unknown Endianness
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#endif //_UTIL_H
|
||||
@@ -0,0 +1,18 @@
|
||||
|
||||
struct version {
|
||||
uint8_t major;
|
||||
uint8_t minor;
|
||||
uint8_t patch;
|
||||
};
|
||||
|
||||
struct version beta = {0, 0, 0};
|
||||
|
||||
// The structs listed below are official releases of Photon
|
||||
// When prepping for a formal release, create a new struct named the scematic version number of the release with . replaced with _
|
||||
// The first element in the struct should equal the MAJOR revision number of the version. This indicates the color.
|
||||
// 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.
|
||||
// Save, commit, and push this change before attempting to make a release. It will fail compilation if this step is not completed.
|
||||
|
||||
struct version v1_0_3 = {1, 0, 3};
|
||||
|
||||
struct version v1_0_4 = {1, 0, 4};
|
||||
@@ -0,0 +1,309 @@
|
||||
#include <Arduino.h>
|
||||
#include <unity.h>
|
||||
#include <cstdio>
|
||||
|
||||
using namespace fakeit;
|
||||
|
||||
#include <IndexNetworkLayer.h>
|
||||
#include <IndexFeederProtocol.h>
|
||||
|
||||
typedef struct {
|
||||
uint8_t destination_address;
|
||||
uint8_t *data;
|
||||
size_t length;
|
||||
} transmit_arg_capture_t;
|
||||
|
||||
static std::vector<transmit_arg_capture_t> args;
|
||||
|
||||
static void reset_transmit_arg_capture() {
|
||||
for (transmit_arg_capture_t arg : args) {
|
||||
if (arg.data != NULL) {
|
||||
free(arg.data);
|
||||
}
|
||||
}
|
||||
args.clear();
|
||||
}
|
||||
|
||||
#define GET_FEEDER_ID 0x01
|
||||
#define INITIALIZE_FEEDER_ID 0x02
|
||||
#define GET_VERSION_ID 0x03
|
||||
#define MOVE_FEED_FORWARD_ID 0x04
|
||||
#define MOVE_FEED_BACKWARD_ID 0x05
|
||||
#define GET_FEEDER_ADDRESS_ID 0xC0
|
||||
|
||||
#define ERROR_SUCCESS 0x00
|
||||
#define ERROR_WRONG_FEEDER_UUID 0x01
|
||||
#define ERROR_MOTOR_FAULT 0x02
|
||||
#define ERROR_UNINITIALIZED_FEEDER 0x03
|
||||
|
||||
std::function<bool(uint8_t destination_address, const uint8_t *buffer, size_t buffer_length)> transmit_capture = [](uint8_t destination_address, const uint8_t *buffer, size_t buffer_length) {
|
||||
transmit_arg_capture_t arg_entry;
|
||||
arg_entry.destination_address = destination_address;
|
||||
arg_entry.length = buffer_length;
|
||||
|
||||
if (buffer_length > 0 && buffer != NULL) {
|
||||
arg_entry.data = (uint8_t *)malloc(buffer_length);
|
||||
memcpy(arg_entry.data, buffer, buffer_length);
|
||||
}
|
||||
|
||||
args.push_back(arg_entry);
|
||||
return true;
|
||||
};
|
||||
|
||||
static uint8_t feeder_address = 0x01;
|
||||
static uint8_t feeder_uuid[] = {0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xaa, 0xbb};
|
||||
|
||||
static Mock<Feeder> feeder_mock;
|
||||
static Feeder &feeder = feeder_mock.get();
|
||||
|
||||
static Mock<IndexNetworkLayer> network_mock;
|
||||
static IndexNetworkLayer &network = network_mock.get();
|
||||
|
||||
static void test_index_feeder_protocol_setup() {
|
||||
feeder_mock.Reset();
|
||||
When(Method(feeder_mock, init)).AlwaysReturn(true);
|
||||
|
||||
network_mock.Reset();
|
||||
reset_transmit_arg_capture();
|
||||
When(Method(network_mock, transmitPacket)).AlwaysDo(transmit_capture);
|
||||
When(Method(network_mock, getLocalAddress)).AlwaysReturn(feeder_address);
|
||||
}
|
||||
|
||||
static void test_index_feeder_protocol_teardown() {
|
||||
reset_transmit_arg_capture();
|
||||
}
|
||||
|
||||
static void test_index_feeder_protocol_get_feeder_id() {
|
||||
IndexFeederProtocol protocol(&feeder, feeder_uuid, sizeof(feeder_uuid));
|
||||
|
||||
uint8_t get_feeder[] = {GET_FEEDER_ID};
|
||||
protocol.handle(&network, get_feeder, sizeof(get_feeder));
|
||||
|
||||
Verify(Method(network_mock, transmitPacket)).Once();
|
||||
uint8_t expected[2 + sizeof(feeder_uuid)];
|
||||
expected[0] = feeder_address;
|
||||
expected[1] = ERROR_SUCCESS;
|
||||
memcpy(&expected[2], feeder_uuid, sizeof(feeder_uuid));
|
||||
TEST_ASSERT_EQUAL_UINT8(0, args[0].destination_address);
|
||||
TEST_ASSERT_EQUAL(sizeof(expected), args[0].length);
|
||||
TEST_ASSERT_EQUAL_MEMORY(expected, args[0].data, sizeof(expected));
|
||||
}
|
||||
|
||||
static void test_initialize_feeder(uint8_t *uuid, size_t uuid_length, uint8_t expected_status) {
|
||||
|
||||
if (uuid_length != 12) {
|
||||
TEST_FAIL_MESSAGE("Only 12 byte UUIDs are supported");
|
||||
}
|
||||
|
||||
IndexFeederProtocol protocol(&feeder, feeder_uuid, sizeof(feeder_uuid));
|
||||
|
||||
uint8_t init_feeder[1 + uuid_length];
|
||||
init_feeder[0] = INITIALIZE_FEEDER_ID;
|
||||
memcpy(&init_feeder[1], uuid, uuid_length);
|
||||
protocol.handle(&network, init_feeder, sizeof(init_feeder));
|
||||
|
||||
TEST_ASSERT_EQUAL_UINT8(0, args[0].destination_address);
|
||||
Verify(Method(network_mock, transmitPacket)).Once();
|
||||
|
||||
switch (expected_status)
|
||||
{
|
||||
case ERROR_SUCCESS: // SUCCESS
|
||||
{
|
||||
uint8_t expected[] = { feeder_address, expected_status };
|
||||
TEST_ASSERT_EQUAL(sizeof(expected), args[0].length);
|
||||
TEST_ASSERT_EQUAL_MEMORY(expected, args[0].data, sizeof(expected));
|
||||
|
||||
TEST_ASSERT_TRUE(protocol.isInitialized());
|
||||
Verify(Method(feeder_mock, init)).Once();
|
||||
}
|
||||
break;
|
||||
|
||||
case ERROR_WRONG_FEEDER_UUID: // Wrong Feeder Id
|
||||
{
|
||||
uint8_t expected[2 + sizeof(feeder_uuid)];
|
||||
expected[0] = feeder_address;
|
||||
expected[1] = ERROR_WRONG_FEEDER_UUID; // Wrong Feeder Id
|
||||
memcpy(&expected[2], feeder_uuid, sizeof(feeder_uuid));
|
||||
TEST_ASSERT_EQUAL(sizeof(expected), args[0].length);
|
||||
TEST_ASSERT_EQUAL_MEMORY(expected, args[0].data, sizeof(expected));
|
||||
|
||||
TEST_ASSERT_FALSE(protocol.isInitialized());
|
||||
Verify(Method(feeder_mock, init)).Never();
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
TEST_FAIL_MESSAGE("Unsupported Return Code");
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static void test_index_feeder_protocol_initialize_feeder_good_uuid() {
|
||||
test_initialize_feeder(feeder_uuid, sizeof(feeder_uuid), ERROR_SUCCESS);
|
||||
}
|
||||
|
||||
static void test_index_feeder_protocol_initialize_feeder_zero_uuid() {
|
||||
uint8_t uuid[12];
|
||||
memset(uuid, 0, sizeof(uuid));
|
||||
test_initialize_feeder(uuid, sizeof(uuid), ERROR_SUCCESS);
|
||||
}
|
||||
|
||||
static void test_index_feeder_protocol_initialize_feeder_invalid_uuid() {
|
||||
uint8_t uuid[12];
|
||||
memset(uuid, 0x11, sizeof(uuid));
|
||||
test_initialize_feeder(uuid, sizeof(uuid), ERROR_WRONG_FEEDER_UUID);
|
||||
}
|
||||
|
||||
static void test_index_feeder_protocol_get_version() {
|
||||
IndexFeederProtocol protocol(&feeder, feeder_uuid, sizeof(feeder_uuid));
|
||||
|
||||
uint8_t get_version[] = {GET_VERSION_ID};
|
||||
protocol.handle(&network, get_version, sizeof(get_version));
|
||||
|
||||
TEST_ASSERT_EQUAL_UINT8(0, args[0].destination_address);
|
||||
Verify(Method(network_mock, transmitPacket)).Once();
|
||||
uint8_t expected[] = { feeder_address, ERROR_SUCCESS, 0x01};
|
||||
TEST_ASSERT_EQUAL_UINT8(0, args[0].destination_address);
|
||||
TEST_ASSERT_EQUAL(sizeof(expected), args[0].length);
|
||||
TEST_ASSERT_EQUAL_MEMORY(expected, args[0].data, sizeof(expected));
|
||||
}
|
||||
|
||||
static void index_feeder_move_test(uint8_t distance, bool forward, bool initialized, Feeder::FeedResult feeder_status, uint8_t expected_status) {
|
||||
When(Method(feeder_mock, feedDistance)).AlwaysReturn(feeder_status);
|
||||
IndexFeederProtocol protocol(&feeder, feeder_uuid, sizeof(feeder_uuid));
|
||||
|
||||
// Initialize Feeder
|
||||
if (initialized) {
|
||||
uint8_t init_feeder[1 + sizeof(feeder_uuid)];
|
||||
init_feeder[0] = INITIALIZE_FEEDER_ID;
|
||||
memcpy(&init_feeder[1], feeder_uuid, sizeof(feeder_uuid));
|
||||
protocol.handle(&network, init_feeder, sizeof(init_feeder));
|
||||
|
||||
// Reset To Only Track The Move Commands
|
||||
reset_transmit_arg_capture();
|
||||
feeder_mock.ClearInvocationHistory();
|
||||
network_mock.ClearInvocationHistory();
|
||||
}
|
||||
|
||||
uint8_t move_feed[2];
|
||||
move_feed[0] = (forward) ? MOVE_FEED_FORWARD_ID : MOVE_FEED_BACKWARD_ID; // Adjust command based on direction
|
||||
move_feed[1] = distance;
|
||||
protocol.handle(&network, move_feed, sizeof(move_feed));
|
||||
|
||||
TEST_ASSERT_EQUAL_UINT8(0, args[0].destination_address);
|
||||
Verify(Method(network_mock, transmitPacket)).Once();
|
||||
|
||||
switch (expected_status) {
|
||||
case ERROR_SUCCESS: // SUCCESS
|
||||
{
|
||||
uint8_t expected[] = { feeder_address, expected_status };
|
||||
TEST_ASSERT_EQUAL(sizeof(expected), args[0].length);
|
||||
TEST_ASSERT_EQUAL_MEMORY(expected, args[0].data, sizeof(expected));
|
||||
Verify(Method(feeder_mock, feedDistance).Using(distance, forward)).Once();
|
||||
}
|
||||
break;
|
||||
case ERROR_MOTOR_FAULT: // Motor Fault
|
||||
{
|
||||
uint8_t expected[] = { feeder_address, ERROR_MOTOR_FAULT };
|
||||
TEST_ASSERT_EQUAL(sizeof(expected), args[0].length);
|
||||
TEST_ASSERT_EQUAL_MEMORY(expected, args[0].data, sizeof(expected));
|
||||
Verify(Method(feeder_mock, feedDistance).Using(distance, forward)).Once();
|
||||
}
|
||||
break;
|
||||
case ERROR_UNINITIALIZED_FEEDER: // Uninitialized Feeder
|
||||
{
|
||||
uint8_t expected[2 + sizeof(feeder_uuid)];
|
||||
expected[0] = feeder_address;
|
||||
expected[1] = ERROR_UNINITIALIZED_FEEDER;
|
||||
memcpy(&expected[2], feeder_uuid, sizeof(feeder_uuid));
|
||||
TEST_ASSERT_EQUAL(sizeof(expected), args[0].length);
|
||||
TEST_ASSERT_EQUAL_MEMORY(expected, args[0].data, sizeof(expected));
|
||||
Verify(Method(feeder_mock, feedDistance)).Never();
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static void test_index_feeder_protocol_move_feed_forward_ok() {
|
||||
index_feeder_move_test(40, true, true, Feeder::FeedResult::SUCCESS, ERROR_SUCCESS);
|
||||
}
|
||||
|
||||
static void test_index_feeder_protocol_move_feed_forward_motor_error() {
|
||||
index_feeder_move_test(40, true, true, Feeder::FeedResult::MOTOR_FAULT, ERROR_MOTOR_FAULT);
|
||||
}
|
||||
|
||||
static void test_index_feeder_protocol_move_feed_forward_invalid_distance() {
|
||||
index_feeder_move_test(39, true, true, Feeder::FeedResult::INVALID_LENGTH, ERROR_MOTOR_FAULT);
|
||||
}
|
||||
|
||||
static void test_index_feeder_protocol_move_feed_forward_uninitialized_feeder() {
|
||||
index_feeder_move_test(40, true, false, Feeder::FeedResult::SUCCESS, ERROR_UNINITIALIZED_FEEDER);
|
||||
|
||||
}
|
||||
|
||||
static void test_index_feeder_protocol_move_feed_backward_ok() {
|
||||
index_feeder_move_test(40, false, true, Feeder::FeedResult::SUCCESS, ERROR_SUCCESS);
|
||||
}
|
||||
|
||||
static void test_index_feeder_protocol_move_feed_backward_motor_error() {
|
||||
index_feeder_move_test(40, false, true, Feeder::FeedResult::MOTOR_FAULT, ERROR_MOTOR_FAULT);
|
||||
}
|
||||
|
||||
static void test_index_feeder_protocol_move_feed_backward_invalid_distance() {
|
||||
index_feeder_move_test(39, false, true, Feeder::FeedResult::INVALID_LENGTH, ERROR_MOTOR_FAULT);
|
||||
}
|
||||
|
||||
static void test_index_feeder_protocol_move_feed_backward_uninitialized_feeder() {
|
||||
index_feeder_move_test(40, false, false, Feeder::FeedResult::SUCCESS, ERROR_UNINITIALIZED_FEEDER);
|
||||
}
|
||||
|
||||
static void test_index_feeder_protocol_get_feeder_address_match() {
|
||||
IndexFeederProtocol protocol(&feeder, feeder_uuid, sizeof(feeder_uuid));
|
||||
|
||||
uint8_t payload[1 + sizeof(feeder_uuid)];
|
||||
payload[0] = GET_FEEDER_ADDRESS_ID;
|
||||
memcpy(&payload[1], feeder_uuid, sizeof(feeder_uuid));
|
||||
protocol.handle(&network, payload, sizeof(payload));
|
||||
|
||||
TEST_ASSERT_EQUAL_UINT8(0, args[0].destination_address);
|
||||
Verify(Method(network_mock, transmitPacket)).Once();
|
||||
|
||||
uint8_t expected[2 + sizeof(feeder_uuid)];
|
||||
expected[0] = feeder_address;
|
||||
expected[1] = ERROR_SUCCESS;
|
||||
memcpy(&expected[2], feeder_uuid, sizeof(feeder_uuid));
|
||||
TEST_ASSERT_EQUAL(sizeof(expected), args[0].length);
|
||||
TEST_ASSERT_EQUAL_MEMORY(expected, args[0].data, sizeof(expected));
|
||||
}
|
||||
|
||||
static void test_index_feeder_protocol_get_feeder_address_no_match() {
|
||||
IndexFeederProtocol protocol(&feeder, feeder_uuid, sizeof(feeder_uuid));
|
||||
|
||||
uint8_t payload[1 + sizeof(feeder_uuid)];
|
||||
payload[0] = GET_FEEDER_ADDRESS_ID;
|
||||
memset(&payload[1], 0, sizeof(feeder_uuid));
|
||||
protocol.handle(&network, payload, sizeof(payload));
|
||||
|
||||
Verify(Method(network_mock, transmitPacket)).Never();
|
||||
}
|
||||
|
||||
#define RUN_FEEDER_TEST(func) { test_index_feeder_protocol_setup(); RUN_TEST(func); test_index_feeder_protocol_teardown(); }
|
||||
|
||||
void index_feeder_protocol_tests() {
|
||||
RUN_FEEDER_TEST(test_index_feeder_protocol_get_feeder_id);
|
||||
RUN_FEEDER_TEST(test_index_feeder_protocol_initialize_feeder_good_uuid);
|
||||
RUN_FEEDER_TEST(test_index_feeder_protocol_initialize_feeder_zero_uuid);
|
||||
RUN_FEEDER_TEST(test_index_feeder_protocol_initialize_feeder_invalid_uuid);
|
||||
RUN_FEEDER_TEST(test_index_feeder_protocol_get_version);
|
||||
RUN_FEEDER_TEST(test_index_feeder_protocol_move_feed_forward_ok);
|
||||
RUN_FEEDER_TEST(test_index_feeder_protocol_move_feed_forward_motor_error);
|
||||
RUN_FEEDER_TEST(test_index_feeder_protocol_move_feed_forward_invalid_distance);
|
||||
RUN_FEEDER_TEST(test_index_feeder_protocol_move_feed_forward_uninitialized_feeder);
|
||||
RUN_FEEDER_TEST(test_index_feeder_protocol_move_feed_backward_ok);
|
||||
RUN_FEEDER_TEST(test_index_feeder_protocol_move_feed_backward_motor_error);
|
||||
RUN_FEEDER_TEST(test_index_feeder_protocol_move_feed_backward_invalid_distance);
|
||||
RUN_FEEDER_TEST(test_index_feeder_protocol_move_feed_backward_uninitialized_feeder);
|
||||
RUN_FEEDER_TEST(test_index_feeder_protocol_get_feeder_address_match);
|
||||
RUN_FEEDER_TEST(test_index_feeder_protocol_get_feeder_address_no_match);
|
||||
}
|
||||
@@ -0,0 +1,270 @@
|
||||
#include <Arduino.h>
|
||||
#include <unity.h>
|
||||
#include <cstdio>
|
||||
|
||||
using namespace fakeit;
|
||||
|
||||
#include <IndexNetworkLayer.h>
|
||||
|
||||
static void test_index_network_single_message_good_crc() {
|
||||
|
||||
// Validate A Good CRC
|
||||
uint8_t buf[] = {0x00, 0x01, 0x01, 0xb1, 0x90};
|
||||
|
||||
|
||||
Mock<IndexPacketHandler> handler_mock;
|
||||
When(Method(handler_mock, handle)).AlwaysReturn();
|
||||
IndexPacketHandler &handler = handler_mock.get();
|
||||
|
||||
When(Method(ArduinoFake(), millis)).Return(1);
|
||||
|
||||
Stream* stream = ArduinoFakeMock(Stream);
|
||||
IndexNetworkLayer network(stream, 0, &handler);
|
||||
|
||||
When(Method(ArduinoFake(Stream), available)).AlwaysReturn(0);
|
||||
network.tick();
|
||||
|
||||
Verify(Method(handler_mock, handle)).Never();
|
||||
|
||||
When(Method(ArduinoFake(Stream), available)).Return(sizeof(buf), 0);
|
||||
|
||||
std::function<size_t(char* buffer, size_t buffer_length)> fn = [buf](char *buffer, size_t buffer_length) {
|
||||
if (buffer_length >= sizeof(buf)) {
|
||||
memcpy(buffer, buf, buffer_length);
|
||||
return buffer_length;
|
||||
}
|
||||
return (size_t)0;
|
||||
};
|
||||
|
||||
When(Method(ArduinoFake(Stream), readBytes)).Do(fn);
|
||||
|
||||
network.tick();
|
||||
|
||||
Verify(Method(handler_mock, handle)).Once();
|
||||
}
|
||||
|
||||
static void test_index_network_multiple_message_good_crc() {
|
||||
// Validate Multiple Messages At Once
|
||||
uint8_t buf[] = {
|
||||
0x00, 0x04, 0x01, 0x02, 0x03, 0x04, 0x50, 0xd4,
|
||||
0x00, 0x02, 0x05, 0x06, 0x22, 0xb6
|
||||
};
|
||||
|
||||
Mock<IndexPacketHandler> handler_mock;
|
||||
When(Method(handler_mock, handle)).AlwaysReturn();
|
||||
IndexPacketHandler &handler = handler_mock.get();
|
||||
|
||||
When(Method(ArduinoFake(), millis)).Return(1);
|
||||
|
||||
Stream* stream = ArduinoFakeMock(Stream);
|
||||
IndexNetworkLayer network(stream, 0, &handler);
|
||||
|
||||
When(Method(ArduinoFake(Stream), available)).AlwaysReturn(0);
|
||||
When(Method(ArduinoFake(Stream), available)).Return(sizeof(buf), 0);
|
||||
|
||||
std::function<size_t(char* buffer, size_t buffer_length)> fn = [buf](char *buffer, size_t buffer_length) {
|
||||
if (buffer_length >= sizeof(buf)) {
|
||||
memcpy(buffer, buf, buffer_length);
|
||||
return buffer_length;
|
||||
}
|
||||
return (size_t)0;
|
||||
};
|
||||
|
||||
When(Method(ArduinoFake(Stream), readBytes)).Do(fn);
|
||||
|
||||
network.tick();
|
||||
|
||||
Verify(Method(handler_mock, handle)).Exactly(2);
|
||||
}
|
||||
|
||||
static void test_index_network_single_message_bad_crc() {
|
||||
// Validate A Bad CRC
|
||||
uint8_t buf[] = {0x00, 0x01, 0x01, 0xb1, 0x91};
|
||||
|
||||
Mock<IndexPacketHandler> handler_mock;
|
||||
When(Method(handler_mock, handle)).AlwaysReturn();
|
||||
IndexPacketHandler &handler = handler_mock.get();
|
||||
|
||||
When(Method(ArduinoFake(), millis)).Return(1);
|
||||
|
||||
Stream* stream = ArduinoFakeMock(Stream);
|
||||
IndexNetworkLayer network(stream, 0, &handler);
|
||||
|
||||
When(Method(ArduinoFake(Stream), available)).Return(sizeof(buf)).AlwaysReturn(0);
|
||||
|
||||
std::function<size_t(char* buffer, size_t buffer_length)> fn = [buf](char *buffer, size_t buffer_length) {
|
||||
if (buffer_length >= sizeof(buf)) {
|
||||
memcpy(buffer, buf, buffer_length);
|
||||
return buffer_length;
|
||||
}
|
||||
return (size_t)0;
|
||||
};
|
||||
|
||||
When(Method(ArduinoFake(Stream), readBytes)).Do(fn);
|
||||
|
||||
network.tick();
|
||||
|
||||
Verify(Method(handler_mock, handle)).Never();
|
||||
}
|
||||
|
||||
static void test_index_network_bad_crc_followed_by_good_crc() {
|
||||
Mock<IndexPacketHandler> handler_mock;
|
||||
When(Method(handler_mock, handle)).AlwaysReturn();
|
||||
IndexPacketHandler &handler = handler_mock.get();
|
||||
|
||||
// Validate A Bad CRC followed by a good CRC
|
||||
uint8_t buf[] = {
|
||||
0x00, 0x01, 0x01, 0xb1, 0x91,
|
||||
0x00, 0x04, 0x01, 0x02, 0x03, 0x04, 0x50, 0xd4
|
||||
};
|
||||
|
||||
When(Method(ArduinoFake(), millis)).Return(1);
|
||||
|
||||
Stream* stream = ArduinoFakeMock(Stream);
|
||||
IndexNetworkLayer network(stream, 0, &handler);
|
||||
|
||||
When(Method(ArduinoFake(Stream), available)).Return(sizeof(buf)).AlwaysReturn(0);
|
||||
|
||||
std::function<size_t(char* buffer, size_t buffer_length)> fn = [buf](char *buffer, size_t buffer_length) {
|
||||
if (buffer_length >= sizeof(buf)) {
|
||||
memcpy(buffer, buf, buffer_length);
|
||||
return buffer_length;
|
||||
}
|
||||
return (size_t)0;
|
||||
};
|
||||
|
||||
When(Method(ArduinoFake(Stream), readBytes)).Do(fn);
|
||||
|
||||
network.tick();
|
||||
|
||||
Verify(Method(handler_mock, handle)).Once();
|
||||
}
|
||||
|
||||
static void test_malformed_frame_with_interframe_time(unsigned long interframe_time, bool should_return) {
|
||||
Mock<IndexPacketHandler> handler_mock;
|
||||
When(Method(handler_mock, handle)).AlwaysReturn();
|
||||
IndexPacketHandler &handler = handler_mock.get();
|
||||
|
||||
// Message That Is 1 Octet To Short
|
||||
uint8_t buf[] = {0x00, 0x01, 0x01, 0xb1 };
|
||||
// Message That Is Valid
|
||||
uint8_t buf2[] = { 0x00, 0x04, 0x01, 0x02, 0x03, 0x04, 0x50, 0xd4 };
|
||||
|
||||
When(Method(ArduinoFake(), millis)).Return(1);
|
||||
|
||||
Stream* stream = ArduinoFakeMock(Stream);
|
||||
IndexNetworkLayer network(stream, 0, &handler);
|
||||
|
||||
When(Method(ArduinoFake(Stream), available)).Return(sizeof(buf)).AlwaysReturn(0);
|
||||
|
||||
std::function<size_t(char* buffer, size_t buffer_length)> fn = [buf](char *buffer, size_t buffer_length) {
|
||||
if (buffer_length >= sizeof(buf)) {
|
||||
memcpy(buffer, buf, buffer_length);
|
||||
return buffer_length;
|
||||
}
|
||||
return (size_t)0;
|
||||
};
|
||||
|
||||
When(Method(ArduinoFake(Stream), readBytes)).Do(fn);
|
||||
|
||||
network.tick();
|
||||
|
||||
// Check The Method Is Never Called Because We're In Mid Frame
|
||||
Verify(Method(handler_mock, handle)).Never();
|
||||
|
||||
// Call The Second Frame 200ms later which is after the timeout
|
||||
std::function<size_t(char* buffer, size_t buffer_length)> fn2 = [buf2](char *buffer, size_t buffer_length) {
|
||||
if (buffer_length >= sizeof(buf2)) {
|
||||
memcpy(buffer, buf2, buffer_length);
|
||||
return buffer_length;
|
||||
}
|
||||
return (size_t)0;
|
||||
};
|
||||
When(Method(ArduinoFake(Stream), available)).Return(sizeof(buf2)).AlwaysReturn(0);
|
||||
When(Method(ArduinoFake(Stream), readBytes)).Do(fn2);
|
||||
When(Method(ArduinoFake(), millis)).Return(interframe_time + 1);
|
||||
|
||||
network.tick();
|
||||
if (should_return) {
|
||||
Verify(Method(handler_mock, handle)).Once();
|
||||
} else {
|
||||
Verify(Method(handler_mock, handle)).Never();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static void test_malformed_frame_timeout() {
|
||||
test_malformed_frame_with_interframe_time(200, true);
|
||||
}
|
||||
|
||||
static void test_malformed_frame_just_past_timeout() {
|
||||
test_malformed_frame_with_interframe_time(101, true);
|
||||
}
|
||||
|
||||
static void test_malformed_frame_at_timeout() {
|
||||
test_malformed_frame_with_interframe_time(100, false);
|
||||
}
|
||||
|
||||
static void test_malformed_frame_without_timeout() {
|
||||
test_malformed_frame_with_interframe_time(10, false);
|
||||
}
|
||||
|
||||
static void test_transmit_packet() {
|
||||
Stream* stream = ArduinoFakeMock(Stream);
|
||||
IndexNetworkLayer network(stream, 0, NULL);
|
||||
|
||||
const uint8_t payload[] = {0x01, 0x00, 0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xaa, 0xbb, 0xcc};
|
||||
|
||||
When(OverloadedMethod(ArduinoFake(Stream), write, size_t(const uint8_t *, size_t)) ).Return(1, 1, sizeof(payload), 2);
|
||||
When(Method(ArduinoFake(Stream), flush)).Return();
|
||||
|
||||
bool result = network.transmitPacket(0x00, payload, sizeof(payload));
|
||||
|
||||
// Assert That It Transmitted
|
||||
TEST_ASSERT_TRUE(result);
|
||||
|
||||
// Verify The Address
|
||||
auto length_is_one = [](const uint8_t *buffer, size_t buffer_length) { return (buffer_length == 1); };
|
||||
auto payload_match = [&](const uint8_t *buffer, size_t buffer_length) { return ((buffer == payload) && (buffer_length == sizeof(payload))); };
|
||||
auto length_is_two = [](const uint8_t *buffer, size_t buffer_length) { return (buffer_length == 2); };
|
||||
|
||||
//TODO: Figure Out How To Validate The CRC
|
||||
Verify(
|
||||
OverloadedMethod(ArduinoFake(Stream), write, size_t(const uint8_t *, size_t)).Matching(length_is_one) * 2 +
|
||||
OverloadedMethod(ArduinoFake(Stream), write, size_t(const uint8_t *, size_t)).Matching(payload_match) +
|
||||
OverloadedMethod(ArduinoFake(Stream), write, size_t(const uint8_t *, size_t)).Matching(length_is_two)
|
||||
);
|
||||
|
||||
}
|
||||
|
||||
static void test_transmit_null() {
|
||||
Stream* stream = ArduinoFakeMock(Stream);
|
||||
IndexNetworkLayer network(stream, 0, NULL);
|
||||
bool result = network.transmitPacket(0x00, NULL, 0);
|
||||
TEST_ASSERT_FALSE(result);
|
||||
}
|
||||
|
||||
static void test_transmit_too_long() {
|
||||
#define TEST_LENGTH (INDEX_NETWORK_MAX_PDU + 1)
|
||||
Stream* stream = ArduinoFakeMock(Stream);
|
||||
IndexNetworkLayer network(stream, 0, NULL);
|
||||
uint8_t payload[TEST_LENGTH];
|
||||
memset(payload, 0, TEST_LENGTH);
|
||||
bool result = network.transmitPacket(0x00, payload, sizeof(payload));
|
||||
TEST_ASSERT_FALSE(result);
|
||||
}
|
||||
|
||||
void index_network_layer_tests() {
|
||||
RUN_TEST(test_index_network_single_message_good_crc);
|
||||
RUN_TEST(test_index_network_multiple_message_good_crc);
|
||||
RUN_TEST(test_index_network_single_message_bad_crc);
|
||||
RUN_TEST(test_index_network_bad_crc_followed_by_good_crc);
|
||||
RUN_TEST(test_malformed_frame_timeout);
|
||||
RUN_TEST(test_malformed_frame_just_past_timeout);
|
||||
RUN_TEST(test_malformed_frame_at_timeout);
|
||||
RUN_TEST(test_malformed_frame_without_timeout);
|
||||
RUN_TEST(test_transmit_packet);
|
||||
RUN_TEST(test_transmit_null);
|
||||
RUN_TEST(test_transmit_too_long);
|
||||
|
||||
}
|
||||
@@ -0,0 +1,20 @@
|
||||
#include <Arduino.h>
|
||||
#include <unity.h>
|
||||
|
||||
extern void index_network_layer_tests();
|
||||
extern void index_feeder_protocol_tests();
|
||||
|
||||
void setUp(void) {
|
||||
ArduinoFakeReset();
|
||||
}
|
||||
|
||||
int process() {
|
||||
UNITY_BEGIN();
|
||||
index_network_layer_tests();
|
||||
index_feeder_protocol_tests();
|
||||
return UNITY_END();
|
||||
}
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
return process();
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
#include <Arduino.h>
|
||||
#include <unity.h>
|
||||
|
||||
int process() {
|
||||
UNITY_BEGIN();
|
||||
//index_protocol_tests();
|
||||
return UNITY_END();
|
||||
}
|
||||
|
||||
void setup() {
|
||||
// NOTE!!! Wait for >2 secs
|
||||
// if board doesn't support software reset via Serial.DTR/RTS
|
||||
|
||||
delay(2000);
|
||||
process();
|
||||
}
|
||||
|
||||
void loop() {
|
||||
digitalWrite(13, HIGH);
|
||||
delay(100);
|
||||
digitalWrite(13, LOW);
|
||||
delay(500);
|
||||
}
|
||||
Reference in New Issue
Block a user