diff --git a/code/firmware/.gitignore b/code/firmware/.gitignore new file mode 100644 index 0000000..53720ec --- /dev/null +++ b/code/firmware/.gitignore @@ -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 diff --git a/code/firmware/platformio.ini b/code/firmware/platformio.ini new file mode 100644 index 0000000..772d074 --- /dev/null +++ b/code/firmware/platformio.ini @@ -0,0 +1,36 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:feeder] +platform = ststm32 +board = nucleo_f031k6 +framework = arduino +upload_protocol = blackmagic +debug_tool = blackmagic +board_build.mcu = stm32f031k6t6 +lib_extra_dirs = ../../../lib/cpp +build_flags = -ggdb +debug_build_flags = -Os -ggdb2 -g2 +lib_deps = + paulstoffregen/OneWire@^2.3.5 + tommag/DS2431@^1.1 + frankboesing/FastCRC@^1.31 + ricaun/ArduinoUniqueID@^1.1.0 + mathertel/RotaryEncoder@^1.5.3 + br3ttb/PID@^1.2.1 + r-downing/AutoPID@^1.0.3 + mike-matera/FastPID@^1.3.1 +test_ignore = test_desktop +monitor_speed = 115200 +check_tool = cppcheck +check_flags = + cppcheck: -j 4 +check_patterns = + src/* diff --git a/code/firmware/src/Feeder.h b/code/firmware/src/Feeder.h new file mode 100644 index 0000000..5eb9c98 --- /dev/null +++ b/code/firmware/src/Feeder.h @@ -0,0 +1,23 @@ +#ifndef _FEEDER_H +#define _FEEDER_H + +#include +#include + +class Feeder { + + public: + enum FeedResult + { + SUCCESS, + INVALID_LENGTH, + MOTOR_FAULT, + UNKNOWN_ERROR + }; + + virtual bool init() = 0; + virtual FeedResult feedDistance(uint8_t tenths_mm, bool forward) = 0; + +}; + +#endif //_INDEX_FEEDER_PROTOCOL_H \ No newline at end of file diff --git a/code/firmware/src/IndexFeeder.cpp b/code/firmware/src/IndexFeeder.cpp new file mode 100644 index 0000000..7424df3 --- /dev/null +++ b/code/firmware/src/IndexFeeder.cpp @@ -0,0 +1,187 @@ +#include +#include "IndexFeeder.h" + +#define TENTH_MM_PER_PIP 40 +#define DELAY_FORWARD_DRIVE 10 +#define DELAY_BACKWARD_DRIVE 10 +#define DELAY_PAUSE 50 +#define DRIVE_LEVEL 200 +#define TICKS_PER_TENTH_MM 10 + +#define TENSION_TIMEOUT 400 + +//pid settings and gains +#define OUTPUT_MIN 0 +#define OUTPUT_MAX 255 +#define KP .12 +#define KI .0003 +#define KD 0 + +// Unit Tests Fail Because This Isn't Defined In ArduinoFake for some reason +#ifndef INPUT_ANALOG +#define INPUT_ANALOG 0x04 +#endif + +IndexFeeder::IndexFeeder(uint8_t drive1_pin, uint8_t drive2_pin, uint8_t peel1_pin, uint8_t peel2_pin, RotaryEncoder* encoder) : + _drive1_pin(drive1_pin), + _drive2_pin(drive2_pin), + _peel1_pin(peel1_pin), + _peel2_pin(peel2_pin), + _encoder(encoder) { + init(); +} + +bool IndexFeeder::init() { + + pinMode(_drive1_pin, OUTPUT); + pinMode(_drive2_pin, OUTPUT); + pinMode(_peel1_pin, OUTPUT); + pinMode(_peel2_pin, OUTPUT); + + return true; +} + +Feeder::FeedResult IndexFeeder::feedDistance(uint8_t tenths_mm, bool forward) { + + int test = abs(tenths_mm) % TENTH_MM_PER_PIP; + + // if (abs(tenths_mm) % TENTH_MM_PER_PIP != 0) { + // // The Index Feeder has only been tested and calibrated for moves of 4mm (One Pip) so far. + // // If any other value is supplied, indicate it is invalid. + // return Feeder::FeedResult::INVALID_LENGTH; + // } + + bool success = (forward) ? moveForward(tenths_mm) : moveBackward(tenths_mm); + if (!success) { + return FeedResult::MOTOR_FAULT; + } + + + return Feeder::FeedResult::SUCCESS; +} + +//returns true if we reached position within timeout, false if we didn't +bool IndexFeeder::moveInternal(uint32_t timeout, bool forward, uint8_t tenths_mm) { + signed long start_pos, goal_pos, ticks_to_move; + + //move based on encoder value + //all this does is move the tape a number of ticks, and that's it! + ticks_to_move = tenths_mm * TICKS_PER_TENTH_MM; + start_pos = _encoder->getPosition(); + + if (forward != true) { + ticks_to_move = ticks_to_move * -1; + } + + goal_pos = start_pos + ticks_to_move; + + unsigned long start_time = millis(); + + bool ret = false; + + float Kp=0.1, Ki=0.5, Kd=0, Hz=10; + int output_bits = 8; + bool output_signed = true; + FastPID pid(Kp, Ki, Kd, Hz, output_bits, output_signed); + pid.setOutputRange(-255, 255); + + while(millis() < start_time + timeout){ + + signed long current_pos = _encoder->getPosition(); + + +// Dumb if statement implementation + + // if(fabs(current_pos - goal_pos) < 3){ + // break; + // } + + // if(goal_pos > current_pos + 100){ + // analogWrite(_drive1_pin, 0); + // analogWrite(_drive2_pin, 255); + // } + // else if (goal_pos < current_pos - 100){ + // analogWrite(_drive1_pin, 255); + // analogWrite(_drive2_pin, 0); + // } + // else if (goal_pos > current_pos){ + // analogWrite(_drive1_pin, 0); + // analogWrite(_drive2_pin, 100); + // } + // else if (goal_pos < current_pos){ + // analogWrite(_drive1_pin, 100); + // analogWrite(_drive2_pin, 0); + // } + + +// PID implementation + + signed long output = pid.step(goal_pos, current_pos); + + // Stop early if we've hit steady state + if(fabs(goal_pos - current_pos) < 3){ // if we're at setpoint, set return value to true and break + ret = true; + break; + } + else { //if not at setpoint yet + if(output > 0){ + analogWrite(_drive1_pin, 0); + analogWrite(_drive2_pin, output*2); + } + else { + output = abs(output); + analogWrite(_drive1_pin, output*2); + analogWrite(_drive2_pin, 0); + } + } + + + } + + // be sure to turn off motors + analogWrite(_drive1_pin, 0); + analogWrite(_drive2_pin, 0); + + return ret; +} + +bool IndexFeeder::tension(uint32_t timeout) { + unsigned long start_millis, current_millis; + + //tension film + + return true; +} + +bool IndexFeeder::moveForward(uint8_t tenths_mm) { + // First, ensure everything is stopped + stop(); + + //move tape + moveInternal(2000, true, tenths_mm); + + // move film + return tension(TENSION_TIMEOUT); +} + +bool IndexFeeder::moveBackward(uint8_t tenths_mm) { + // First, ensure everything is stopped + stop(); + + // Next, unspool some film to give the tape slack. imprecise amount because we retention later + + // move tape backward + moveInternal(2000, false, tenths_mm); + + + //tension film again + return tension(TENSION_TIMEOUT); +} + +void IndexFeeder::stop() { + // Stop Everything + analogWrite(_drive1_pin, 0); + analogWrite(_drive2_pin, 0); + analogWrite(_peel1_pin, 0); + analogWrite(_peel2_pin, 0); +} \ No newline at end of file diff --git a/code/firmware/src/IndexFeeder.h b/code/firmware/src/IndexFeeder.h new file mode 100644 index 0000000..167c9ed --- /dev/null +++ b/code/firmware/src/IndexFeeder.h @@ -0,0 +1,46 @@ +#ifndef _INDEX_FEEDER_H +#define _INDEX_FEEDER_H + +#include "Feeder.h" +#include + +#ifndef MOTOR_DEPS + #define MOTOR_DEPS + #include + #include + +#endif + + + +class IndexFeeder : public Feeder { + + public: + IndexFeeder(uint8_t drive1_pin, uint8_t drive2_pin, uint8_t peel1_pin, uint8_t peel2_pin, RotaryEncoder* encoder); + bool init() override; + Feeder::FeedResult feedDistance(uint8_t tenths_mm, bool forward) override; + bool moveInternal(uint32_t timeout, bool forward, uint8_t tenths_mm); + + private: + uint8_t _drive1_pin; + uint8_t _drive2_pin; + + uint8_t _peel1_pin; + uint8_t _peel2_pin; + + RotaryEncoder* _encoder; + + double* _Setpoint; + double* _Input; + double* _Output; + + bool moveForward(uint8_t tenths_mm); + bool moveBackward(uint8_t tenths_mm); + void stop(); + //bool moveInternal(uint32_t timeout, bool forward, uint8_t tenths_mm); + bool tension(uint32_t timeout); + bool loopback(); + +}; + +#endif //_INDEX_FEEDER_H \ No newline at end of file diff --git a/code/firmware/src/IndexFeederProtocol.cpp b/code/firmware/src/IndexFeederProtocol.cpp new file mode 100644 index 0000000..bc473e6 --- /dev/null +++ b/code/firmware/src/IndexFeederProtocol.cpp @@ -0,0 +1,269 @@ +#include "IndexFeederProtocol.h" +#include "IndexNetworkLayer.h" + +#define MAX_PROTOCOL_VERSION 1 + +typedef enum { + STATUS_OK = 0x00, + STATUS_WRONG_FEEDER_ID = 0x01, + STATUS_MOTOR_FAULT = 0x02, + STATUS_UNINITIALIZED_FEEDER = 0x03, + 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, + + // Broadcast Commands + GET_FEEDER_ADDRESS = 0xc0, +} FeederCommand; + +typedef struct { + FeederCommand command_id; + size_t min_payload_length; + size_t max_payload_length; +} protocol_command_t; + +// Note that this array MUST be sorted as the function handle makes that assumption for efficiency +static protocol_command_t commands[] = { + {GET_FEEDER_ID, 0, 0}, + {INITIALIZE_FEEDER, 12, 12}, + {GET_VERSION, 0, 0}, + {MOVE_FEED_FORWARD, 1, 1}, + {MOVE_FEED_BACKWARD, 1, 1}, + {GET_FEEDER_ADDRESS, 12, 12} +}; +static const size_t num_commands = sizeof(commands) / sizeof(protocol_command_t); + +static const uint8_t zero_uuid[UUID_LENGTH] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + +#define GET_FEEDER_ID_RESPONSE_LENGTH (2 + UUID_LENGTH) +#define INITIALIZE_FEEDER_RESPONSE_LENGTH 2 +#define GET_VERSION_RESPONSE_LENGTH 3 +#define GET_FEEDER_ADDRESS_RESPONSE_LENGTH (2 + UUID_LENGTH) + +#define MOVE_FEED_RESPONSE_LENGTH 2 + +#define WRONG_FEEDER_RESPONSE_LENGTH (2 + UUID_LENGTH) + +#define UNINITIALIZED_FEEDER_RESPONSE_LENGTH (2 + UUID_LENGTH) + +IndexFeederProtocol::IndexFeederProtocol(Feeder *feeder, const uint8_t *uuid, size_t uuid_length) : _feeder(feeder), _initialized(false) { + memset(_uuid, 0, UUID_LENGTH); + memcpy(_uuid, uuid, (uuid_length < UUID_LENGTH) ? uuid_length : UUID_LENGTH); +} + +void IndexFeederProtocol::handle(IndexNetworkLayer *instance, uint8_t *buffer, size_t buffer_length) { + + // The buffer must be at least one octet long as it needs to contain the + // command id. + if (buffer_length == 0) { + return; + } + + protocol_command_t *command_entry = NULL; + uint8_t command_id = buffer[0]; + + // Check if the command exists and the length is found. + if ( !checkLength(command_id, buffer_length - 1) ) { + return; + } + + uint8_t *command_payload = &buffer[1]; + size_t command_payload_size = buffer_length - 1; + + switch(command_id) { + case GET_FEEDER_ID: + handleGetFeederId(instance); + break; + case INITIALIZE_FEEDER: + handleInitializeFeeder(instance, command_payload, command_payload_size); + break; + case GET_VERSION: + handleGetVersion(instance); + break; + case MOVE_FEED_FORWARD: + handleMoveFeedForward(instance, command_payload, command_payload_size); + break; + case MOVE_FEED_BACKWARD: + handleMoveFeedBackward(instance, command_payload, command_payload_size); + break; + case GET_FEEDER_ADDRESS: + handleGetFeederAddress(instance, command_payload, command_payload_size); + break; + default: + // Something has gone wrong if execution ever gets here. + break; + } +} + +bool IndexFeederProtocol::isInitialized() { + return _initialized; +} + + +bool IndexFeederProtocol::checkLength(uint8_t command_id, size_t command_payload_length) { + protocol_command_t *command_entry = NULL; + bool return_value = false; + + // Loop through the entries until the matching command id is found or + // the command id of the entry is greater numerically than the command id + // passed to the function. + for (size_t idx = 0; idx < num_commands; idx++) { + command_entry = &commands[idx]; + if (command_entry->command_id == command_id) { + break; + } else if (command_entry->command_id > command_id) { + command_entry = NULL; + break; + } + } + + if (command_entry != NULL && + command_entry->min_payload_length <= command_payload_length && + command_entry->max_payload_length >= command_payload_length) { + // The command entry is found and the payload is within the bounds + return_value = true; + } + + return return_value; +} + +bool IndexFeederProtocol::checkInitialized(IndexNetworkLayer *instance) { + // Checks if the feeder is initialized and returns an error if it hasn't been + // Response: 0x03 + + if (!_initialized) { + uint8_t response[UNINITIALIZED_FEEDER_RESPONSE_LENGTH]; + response[0] = instance->getLocalAddress(); + response[1] = STATUS_UNINITIALIZED_FEEDER; + memcpy(&response[2], _uuid, UUID_LENGTH); + instance->transmitPacket(INDEX_NETWORK_CONTROLLER_ADDRESS, response, UNINITIALIZED_FEEDER_RESPONSE_LENGTH); + } + + return _initialized; +} + + +void IndexFeederProtocol::handleGetFeederId(IndexNetworkLayer *instance) { + // Payload: + // Response: + + uint8_t response[GET_FEEDER_ID_RESPONSE_LENGTH]; + response[0] = instance->getLocalAddress(); + response[1] = STATUS_OK; + memcpy(&response[2], _uuid, UUID_LENGTH); + + instance->transmitPacket(INDEX_NETWORK_CONTROLLER_ADDRESS, response, GET_FEEDER_ID_RESPONSE_LENGTH); +} + +void IndexFeederProtocol::handleInitializeFeeder(IndexNetworkLayer *instance, uint8_t *payload, size_t payload_length) { + //Payload: + //Response: + + // Check uuid is correct or all zeros, if not return a Wrong Feeder UUID error + if (memcmp(payload, zero_uuid, UUID_LENGTH) != 0 && memcmp(payload, _uuid, UUID_LENGTH) != 0) { + uint8_t response[WRONG_FEEDER_RESPONSE_LENGTH]; + response[0] = instance->getLocalAddress(); + response[1] = STATUS_WRONG_FEEDER_ID; + memcpy(&response[2], _uuid, UUID_LENGTH); + instance->transmitPacket(INDEX_NETWORK_CONTROLLER_ADDRESS, response, WRONG_FEEDER_RESPONSE_LENGTH); + return; + } + + if (_feeder == NULL || !_feeder->init()) { + //TODO: Send A Response Here To Indicate That The Feeder Coulsn't Be Initialized + return; + } + + // Do Response + _initialized = true; + + // Start To Setup Response + uint8_t response[INITIALIZE_FEEDER_RESPONSE_LENGTH]; + response[0] = instance->getLocalAddress(); + response[1] = STATUS_OK; + instance->transmitPacket(INDEX_NETWORK_CONTROLLER_ADDRESS, response, INITIALIZE_FEEDER_RESPONSE_LENGTH); +} + +void IndexFeederProtocol::handleGetVersion(IndexNetworkLayer *instance) { + uint8_t response[GET_VERSION_RESPONSE_LENGTH]; + + // Build the response + response[0] = instance->getLocalAddress(); + response[1] = STATUS_OK; + response[2] = MAX_PROTOCOL_VERSION; + + // Transmit The Packet To The Central + instance->transmitPacket(INDEX_NETWORK_CONTROLLER_ADDRESS, response, GET_VERSION_RESPONSE_LENGTH); +} + +void IndexFeederProtocol::move(IndexNetworkLayer *instance, uint8_t distance, bool forward) { + + if (!checkInitialized(instance)) { + return; + } + + Feeder::FeedResult result = _feeder->feedDistance(distance, forward); + + switch (result) + { + case Feeder::FeedResult::SUCCESS: + { + uint8_t response[MOVE_FEED_RESPONSE_LENGTH]; + response[0] = instance->getLocalAddress(); + response[1] = STATUS_OK; + instance->transmitPacket(INDEX_NETWORK_CONTROLLER_ADDRESS, response, MOVE_FEED_RESPONSE_LENGTH); + } + break; + + case Feeder::FeedResult::INVALID_LENGTH: // For Now Handle Invalid Length & Motor Fault The Same + case Feeder::FeedResult::MOTOR_FAULT: + { + uint8_t response[MOVE_FEED_RESPONSE_LENGTH]; + response[0] = instance->getLocalAddress(); + response[1] = STATUS_MOTOR_FAULT; + instance->transmitPacket(INDEX_NETWORK_CONTROLLER_ADDRESS, response, MOVE_FEED_RESPONSE_LENGTH); + } + break; + + case Feeder::FeedResult::UNKNOWN_ERROR: + //TODO: Send The Appropriate Response + break; + + default: + break; + } +} + +void IndexFeederProtocol::handleMoveFeedForward(IndexNetworkLayer *instance, uint8_t *payload, size_t payload_length) { + move(instance, payload[0], true); +} + +void IndexFeederProtocol::handleMoveFeedBackward(IndexNetworkLayer *instance, uint8_t *payload, size_t payload_length) { + move(instance, payload[0], false); +} + +void IndexFeederProtocol::handleGetFeederAddress(IndexNetworkLayer *instance, uint8_t *payload, size_t payload_length) { + //Payload: + //Response: + + // Check For Feeder Match + if (memcmp(payload, _uuid, UUID_LENGTH) != 0) { + return; + } + + // Return The Address + uint8_t response[GET_FEEDER_ADDRESS_RESPONSE_LENGTH]; + response[0] = instance->getLocalAddress(); + response[1] = STATUS_OK; + memcpy(&response[2], _uuid, UUID_LENGTH); + + instance->transmitPacket(INDEX_NETWORK_CONTROLLER_ADDRESS, response, GET_FEEDER_ADDRESS_RESPONSE_LENGTH); +} \ No newline at end of file diff --git a/code/firmware/src/IndexFeederProtocol.h b/code/firmware/src/IndexFeederProtocol.h new file mode 100644 index 0000000..5121126 --- /dev/null +++ b/code/firmware/src/IndexFeederProtocol.h @@ -0,0 +1,34 @@ +#ifndef _INDEX_FEEDER_PROTOCOL_H +#define _INDEX_FEEDER_PROTOCOL_H + +#include "IndexPacketHandler.h" +#include "Feeder.h" + +#define UUID_LENGTH 12 + +class IndexFeederProtocol : public IndexPacketHandler { + + public: + IndexFeederProtocol(Feeder *feeder, const uint8_t *uuid, size_t uuid_length); + void handle(IndexNetworkLayer *instance, uint8_t *buffer, size_t buffer_length) override; + bool isInitialized(); + + private: + + Feeder *_feeder; + uint8_t _uuid[UUID_LENGTH]; + bool _initialized; + + bool checkLength(uint8_t command_id, size_t command_payload_length); + bool checkInitialized(IndexNetworkLayer *instance); + void handleGetFeederId(IndexNetworkLayer *instance); + void handleInitializeFeeder(IndexNetworkLayer *instance, uint8_t *payload, size_t payload_length); + void handleGetVersion(IndexNetworkLayer *instance); + void handleMoveFeedForward(IndexNetworkLayer *instance, uint8_t *payload, size_t payload_length); + void handleMoveFeedBackward(IndexNetworkLayer *instance, uint8_t *payload, size_t payload_length); + void handleGetFeederAddress(IndexNetworkLayer *instance, uint8_t *payload, size_t payload_length); + + void move(IndexNetworkLayer *instance, uint8_t distance, bool forwrd); +}; + +#endif //_INDEX_FEEDER_PROTOCOL_H \ No newline at end of file diff --git a/code/firmware/src/IndexNetworkLayer.cpp b/code/firmware/src/IndexNetworkLayer.cpp new file mode 100644 index 0000000..e5be49c --- /dev/null +++ b/code/firmware/src/IndexNetworkLayer.cpp @@ -0,0 +1,193 @@ +#include "IndexNetworkLayer.h" +#include "Arduino.h" +#include +#include +#ifndef NATIVE +#include "util.h" +#endif + +#define INDEX_PROTOCOL_DEFAULT_TIMEOUT_MS 100 +#define INDEX_INCOMING_BUFFER_SIZE 16 +#define RS485_CONTROL_DELAY 10 + +IndexNetworkLayer::IndexNetworkLayer(Stream* stream, uint8_t address, IndexPacketHandler* handler) : _stream(stream), _rs485_enable(false), _local_address(address), _handler(handler), _timeout_period(INDEX_PROTOCOL_DEFAULT_TIMEOUT_MS) { + reset(); +} + +IndexNetworkLayer::IndexNetworkLayer(Stream* stream, uint8_t de_pin, uint8_t re_pin, uint8_t address, IndexPacketHandler* handler) : _stream(stream), _rs485_enable(true), _de_pin(de_pin), _re_pin(re_pin), _local_address(address), _handler(handler), _timeout_period(INDEX_PROTOCOL_DEFAULT_TIMEOUT_MS) { + reset(); +} + +void IndexNetworkLayer::setTimeoutPeriod(uint32_t timeout) { + _timeout_period = timeout; +} + +uint32_t IndexNetworkLayer::getTimeoutPeriod() { + return _timeout_period; +} + +void IndexNetworkLayer::setLocalAddress(uint8_t address) { + _local_address = address; +} + +uint8_t IndexNetworkLayer::getLocalAddress() { + return _local_address; +} + +void IndexNetworkLayer::tick() { + // Read any available data from the stream and pass to the process function + // to parse + uint8_t buffer[INDEX_INCOMING_BUFFER_SIZE]; + size_t length = _stream->available(); + while (length > 0) { + uint32_t time = millis(); + if (length > sizeof(buffer)) { + length = sizeof(buffer); + } + length = _stream->readBytes(buffer, length); + process(buffer, length, time); + length = _stream->available(); + } +} + +bool IndexNetworkLayer::transmitPacket(uint8_t destination_address, const uint8_t *buffer, size_t buffer_length) { + + // Do some very basic integrity checks to make sure the call was valid + if (NULL == buffer || buffer_length > INDEX_NETWORK_MAX_PDU || buffer_length > UINT8_MAX) { + return false; + } + + uint8_t length = buffer_length; + uint8_t crc_array[INDEX_PROTOCOL_CHECKSUM_LENGTH]; + uint16_t crc = _CRC16.modbus(&destination_address, 1); + crc = _CRC16.modbus_upd(&length, 1); + crc = _CRC16.modbus_upd(buffer, buffer_length); + crc = htons(crc); + + crc_array[0] = (uint8_t)((crc >> 8) & 0x0ff); + crc_array[1] = (uint8_t)(crc & 0x0ff); + + + if (_rs485_enable) { + digitalWrite(_de_pin, HIGH); // Enable The Transmitter (DE pin) + //digitalWrite(_re_pin, HIGH); // Disable The Receiver (/RE pin) + delay(RS485_CONTROL_DELAY); + } + + // Transmit The Address + _stream->write(&destination_address, 1); + + // Transmit The Length + _stream->write(&length, 1); + + // Transmit The Data + _stream->write(buffer, buffer_length); + + // Transmit CRC + _stream->write(crc_array, INDEX_PROTOCOL_CHECKSUM_LENGTH); + + // Flush The Data + _stream->flush(); + + if (_rs485_enable) { + delay(RS485_CONTROL_DELAY); + digitalWrite(_de_pin, LOW); // Disable The Transmitter (DE pin) + digitalWrite(_re_pin, LOW); // Enable The Receiver (/RE pin) + delay(1); + } + + return true; +} + +void IndexNetworkLayer::process(uint8_t *buffer, size_t buffer_length, uint32_t time) { + + size_t idx = 0; + + // Check Timer Is Less Than _last_byte_time + uint32_t _timeout_threshold = _last_byte_time + _timeout_period; + + if (_timeout_threshold < time || _timeout_threshold < _last_byte_time) { + // The byte was not received in time; + reset(); + } + + // Set Last Byte Time or Reset + _last_byte_time = time; + + while (idx < buffer_length) { + // Process Byte + switch (_state) { + case AWAITING_ADDRESS: + _address = buffer[idx]; + _state = ADDRESS_RECEIVED; + break; + case ADDRESS_RECEIVED: + _length = buffer[idx]; + _index = 0; + if (_length > 0) { + _state = LENGTH_RECEIVED; + } else { + _state = PAYLOAD_RECEIVED; + } + break; + case LENGTH_RECEIVED: + // Only update the array if the value is smaller than the max PDU + // We don't just reset here are the packet format could be ok, and + // resetting could get us out of sync. + if (_index < INDEX_NETWORK_MAX_PDU) { + _payload[_index] = buffer[idx]; + } + _index++; + if (_index >= _length) { + _index = 0; + _state = PAYLOAD_RECEIVED; + } + break; + case PAYLOAD_RECEIVED: + // receive the checksum + _rx_checksum[_index++] = buffer[idx]; + if (_index >= INDEX_PROTOCOL_CHECKSUM_LENGTH) { + + // Cacluate The Frame Checksum + // Compare Frame Checksum + uint16_t payload_length = (_length < INDEX_NETWORK_MAX_PDU) ? _length : INDEX_NETWORK_MAX_PDU; + _CRC16.modbus(&_address, 1); + _CRC16.modbus_upd(&_length, 1); + uint16_t calc_crc = _CRC16.modbus_upd(_payload, payload_length); + + // Done with htons to ensure platform independence + uint16_t rx_crc = _rx_checksum[0] << 8 | _rx_checksum[1]; + rx_crc = ntohs(rx_crc); + + if (calc_crc == rx_crc && (_address == _local_address || _address == INDEX_NETWORK_BROADCAST_ADDRESS) ){ + // Time For Us To Dispatch This + if (_handler != NULL) { + _handler->handle(this, _payload, _length); + } + } + reset(); + } + + break; + } + + // Handle The Index + idx++; + } +} + +void IndexNetworkLayer::reset() { + // Reset The State Machine + _state = AWAITING_ADDRESS; + _address = 0; + _length = 0; + _index = 0; + memset(_payload, 0, INDEX_NETWORK_MAX_PDU); + memset(_rx_checksum, 0, INDEX_PROTOCOL_CHECKSUM_LENGTH); + _last_byte_time = 0; + + if (_rs485_enable) { + digitalWrite(_de_pin, LOW); // Disable The Transmitter (DE pin) + digitalWrite(_re_pin, LOW); // Enable The Receiver (/RE pin) + } +} diff --git a/code/firmware/src/IndexNetworkLayer.h b/code/firmware/src/IndexNetworkLayer.h new file mode 100644 index 0000000..0ac009f --- /dev/null +++ b/code/firmware/src/IndexNetworkLayer.h @@ -0,0 +1,62 @@ +#ifndef _INDEX_PROTOCOL_H +#define _INDEX_PROTOCOL_H + +#include +#include +#include +#include +#include "Stream.h" + +#define INDEX_NETWORK_MAX_PDU 32 +#define INDEX_PROTOCOL_CHECKSUM_LENGTH 2 + +#define INDEX_NETWORK_CONTROLLER_ADDRESS 0x00 +#define INDEX_NETWORK_BROADCAST_ADDRESS 0xFF + +class IndexNetworkLayer +{ +public: + IndexNetworkLayer(Stream* stream, uint8_t address, IndexPacketHandler* handler); + IndexNetworkLayer(Stream* stream, uint8_t de_pin, uint8_t re_pin, uint8_t address, IndexPacketHandler* handler); + + virtual void setTimeoutPeriod(uint32_t timeout); + virtual uint32_t getTimeoutPeriod(); + + virtual void setLocalAddress(uint8_t address); + virtual uint8_t getLocalAddress(); + + virtual void tick(); + + virtual bool transmitPacket(uint8_t destination_address, const uint8_t *buffer, size_t buffer_length); + +private: + FastCRC16 _CRC16; + + enum ProtocolState + { + AWAITING_ADDRESS, + ADDRESS_RECEIVED, + LENGTH_RECEIVED, + PAYLOAD_RECEIVED + }; + + IndexNetworkLayer::ProtocolState _state; + Stream* _stream; + bool _rs485_enable; + uint8_t _de_pin; + uint8_t _re_pin; + uint8_t _local_address; + IndexPacketHandler* _handler; + uint8_t _address; + uint8_t _length; + uint8_t _index; + uint8_t _payload[INDEX_NETWORK_MAX_PDU]; + uint8_t _rx_checksum[INDEX_PROTOCOL_CHECKSUM_LENGTH]; + uint32_t _last_byte_time; + uint32_t _timeout_period; + + void process(uint8_t *buffer, size_t buffer_length, uint32_t time); + void reset(); +}; + +#endif //_INDEX_PROTOCOL_H \ No newline at end of file diff --git a/code/firmware/src/IndexPacketHandler.h b/code/firmware/src/IndexPacketHandler.h new file mode 100644 index 0000000..5d61489 --- /dev/null +++ b/code/firmware/src/IndexPacketHandler.h @@ -0,0 +1,14 @@ +#ifndef _INDEX_PROTOCOL_HANDLER_H +#define _INDEX_PROTOCOL_HANDLER_H + +#include +#include + +class IndexNetworkLayer; + +class IndexPacketHandler { + public: + virtual void handle(IndexNetworkLayer *instance, uint8_t *buffer, size_t buffer_length) = 0; +}; + +#endif \ No newline at end of file diff --git a/code/firmware/src/define.h b/code/firmware/src/define.h new file mode 100644 index 0000000..ecced5a --- /dev/null +++ b/code/firmware/src/define.h @@ -0,0 +1,35 @@ +#define DE PA12 +#define _RE PA11 + +#define LED1 PA8 //yellow, on bottom of front face +#define LED2 PB1 //green, on top of front face +#define LED3 PB_2 //green, first on top face +#define LED4 PB3 //green, second on top face +#define LED5 PB4 //green, third on top face + +#define SW1 PA3 +#define SW2 PA2 + +#define DRIVE1 PB5 +#define DRIVE2 PB6 +#define PEEL1 PB7 +#define PEEL2 PB_8 + +#define ONE_WIRE PA0 + +#define DRIVE_ENC_A PA4 +#define DRIVE_ENC_B PA15 + +//#define FILM_TENSION PA3 + +//#define VCNT2020 PA1 +//#define QRE1113 PA4 +//----------------------- +//#define OPTO_SIG QRE1113 + + + +#define LONG_PRESS_DELAY 300 + +//#define DEBUG +//#define OPTO_DEBUG \ No newline at end of file diff --git a/code/firmware/src/main.cpp b/code/firmware/src/main.cpp new file mode 100644 index 0000000..81a1904 --- /dev/null +++ b/code/firmware/src/main.cpp @@ -0,0 +1,302 @@ +/* +INDEX PNP +Stephen Hawes 2021 + +This firmware is intended to run on the Index PNP feeder main board. +It is meant to index component tape forward, while also intelligently peeling the film covering from said tape. +When the feeder receives a signal from the host, it indexes a certain number of 'ticks' or 4mm spacings on the tape +(also the distance between holes in the tape) + +*/ +#include "define.h" + +#ifdef UNIT_TEST + #include +#else + #include + #include + #include + #include + #include +#endif // UNIT_TEST + +#ifndef MOTOR_DEPS +#define MOTOR_DEPS + +#include +//#include + +#endif + +#include +#include +#include + +#define BAUD_RATE 57600 + +// +//global variables +// +byte addr = 0; + +#ifdef UNIT_TEST +StreamFake ser(); +#else +HardwareSerial ser(PA10, PA9); +#endif // ARDUINO + +// EEPROM +OneWire oneWire(ONE_WIRE); +DS2431 eeprom(oneWire); + +// Encoder +RotaryEncoder encoder(DRIVE_ENC_A, DRIVE_ENC_B, RotaryEncoder::LatchMode::TWO03); + +// PID +double Setpoint, Input, Output; + +// Feeder Class Instances +IndexFeeder *feeder; +IndexFeederProtocol *protocol; +IndexNetworkLayer *network; + +#define ALL_LEDS_OFF() byte_to_light(0x00) +#define ALL_LEDS_ON() byte_to_light(0xff) + +//------- +//FUNCTIONS +//------- + +void checkPosition() +{ + encoder.tick(); // just call tick() to check the state. +} + +void byte_to_light(byte num){ + digitalWrite(LED1, !bitRead(num, 0)); + digitalWrite(LED2, !bitRead(num, 1)); + digitalWrite(LED3, !bitRead(num, 2)); + digitalWrite(LED4, !bitRead(num, 3)); + digitalWrite(LED5, !bitRead(num, 4)); +} + +byte read_floor_addr(){ + if(!oneWire.reset()) + { + //No DS2431 found on the 1-Wire bus + return 0xFF; + } + else{ + byte data[128]; + eeprom.read(0, data, sizeof(data)); + return data[0]; + } +} + +byte write_floor_addr(){ + //programs a feeder floor. + //successful programming returns the address programmed + //failed program returns 0x00 + + byte newData[] = {0,0,0,0,0,0,0,0}; + + byte current_selection = addr; + + ALL_LEDS_OFF(); + while(!digitalRead(SW1) || !digitalRead(SW2)){ + //do nothing + } + ALL_LEDS_ON(); + + while(true){ + //we stay in here as long as both buttons aren't pressed + if(!digitalRead(SW1) && current_selection < 31){ + delay(LONG_PRESS_DELAY); + if(!digitalRead(SW1) && !digitalRead(SW2)){ + break; + } + current_selection = current_selection + 1; + while(!digitalRead(SW1)){ + //do nothing + } + } + if(!digitalRead(SW2) && current_selection > 1){ + delay(LONG_PRESS_DELAY); + if(!digitalRead(SW1) && !digitalRead(SW2)){ + break; + } + current_selection = current_selection - 1; + while(!digitalRead(SW2)){ + //do nothing + } + } + + byte_to_light(current_selection); + } + + byte_to_light(0x00); + + newData[0] = current_selection; + word address = 0; + if (eeprom.write(address, newData, sizeof(newData))){ + addr = current_selection; + + while(!digitalRead(SW1) || !digitalRead(SW2)){ + //do nothing + } + + // If the network is configured, update the local address + // to the newly selected address. + if (network != NULL) { + network->setLocalAddress(addr); + } + + for (int i = 0; i < 32; i++){ + byte_to_light(i); + delay(40); + } + + byte_to_light(0x00); + + return newData[0]; + } + else + { + return 0x00; + } +} + +//------- +//SETUP +//------- + +void setup() { + + #ifdef DEBUG + Serial.println("INFO - Feeder starting up..."); + #endif + + //setting pin modes + pinMode(DE, OUTPUT); + pinMode(_RE, OUTPUT); + + pinMode(LED1, OUTPUT); + pinMode(LED2, OUTPUT); + pinMode(LED3, OUTPUT); + pinMode(LED4, OUTPUT); + pinMode(LED5, OUTPUT); + pinMode(SW1, INPUT_PULLUP); + pinMode(SW2, INPUT_PULLUP); + + //init led blink + ALL_LEDS_ON(); + delay(100); + ALL_LEDS_OFF(); + delay(100); + ALL_LEDS_ON(); + delay(100); + ALL_LEDS_OFF(); + + //setting initial pin states + digitalWrite(DE, HIGH); + digitalWrite(_RE, HIGH); + + // Reading Feeder Floor Address + byte floor_addr = read_floor_addr(); + + if(floor_addr == 0x00){ //floor 1 wire eeprom has not been programmed + //somehow prompt to program eeprom + } + else if(floor_addr == 0xFF){ + //no eeprom chip detected + } + else{ //successfully read address from eeprom + addr = floor_addr; + } + + //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); + + // Setup Feeder + feeder = new IndexFeeder(DRIVE1, DRIVE2, PEEL1, PEEL2, &encoder); + protocol = new IndexFeederProtocol(feeder, UniqueID, UniqueIDsize); + network = new IndexNetworkLayer(&ser, DE, _RE, addr, protocol); + +} + +//------ +//MAIN CONTROL LOOP +//------ +// cppcheck-suppress unusedFunction +void loop() { + + // Checking SW1 status to go forward, or initiate settings mode + if(!digitalRead(SW1)){ + delay(LONG_PRESS_DELAY); + + if(!digitalRead(SW1)){ + + if(!digitalRead(SW2)){ + //both are pressed, entering settings mode + write_floor_addr(); + } + else{ + //we've got a long press, lets go speedy + analogWrite(DRIVE1, 0); + analogWrite(DRIVE2, 255); + + while(!digitalRead(SW1)){ + //do nothing + } + + analogWrite(DRIVE1, 0); + analogWrite(DRIVE2, 0); + } + + } + else{ + feeder->feedDistance(40, true); + + } + } + + // Checking SW2 status to go backward + if(!digitalRead(SW2)){ + delay(LONG_PRESS_DELAY); + + if(!digitalRead(SW2)){ + + if(!digitalRead(SW1)){ + //both are pressed, entering settings mode + write_floor_addr(); + } + else{ + //we've got a long press, lets go speedy + analogWrite(DRIVE1, 255); + analogWrite(DRIVE2, 0); + + while(!digitalRead(SW2)){ + //do nothing + } + + analogWrite(DRIVE1, 0); + analogWrite(DRIVE2, 0); + } + + } + else{ + feeder->feedDistance(40, false); + } + } + + //listening on rs-485 for a command + if (network != NULL) { + network->tick(); + } + + // end main loop +} diff --git a/code/firmware/src/util b/code/firmware/src/util new file mode 100644 index 0000000..e69de29 diff --git a/code/firmware/src/util.cpp b/code/firmware/src/util.cpp new file mode 100644 index 0000000..2aa67bf --- /dev/null +++ b/code/firmware/src/util.cpp @@ -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) ) \ No newline at end of file diff --git a/code/firmware/src/util.h b/code/firmware/src/util.h new file mode 100644 index 0000000..777cecb --- /dev/null +++ b/code/firmware/src/util.h @@ -0,0 +1,26 @@ +#ifndef _UTIL_H +#define _UTIL_H + +#include + +#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 \ No newline at end of file diff --git a/code/firmware/test/test_desktop/test_index_feeder_protocol.cpp b/code/firmware/test/test_desktop/test_index_feeder_protocol.cpp new file mode 100644 index 0000000..573bf79 --- /dev/null +++ b/code/firmware/test/test_desktop/test_index_feeder_protocol.cpp @@ -0,0 +1,309 @@ +#include +#include +#include + +using namespace fakeit; + +#include +#include + +typedef struct { + uint8_t destination_address; + uint8_t *data; + size_t length; +} transmit_arg_capture_t; + +static std::vector 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 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_mock; +static Feeder &feeder = feeder_mock.get(); + +static Mock 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); +} \ No newline at end of file diff --git a/code/firmware/test/test_desktop/test_index_network_layer.cpp b/code/firmware/test/test_desktop/test_index_network_layer.cpp new file mode 100644 index 0000000..4f97691 --- /dev/null +++ b/code/firmware/test/test_desktop/test_index_network_layer.cpp @@ -0,0 +1,270 @@ +#include +#include +#include + +using namespace fakeit; + +#include + +static void test_index_network_single_message_good_crc() { + + // Validate A Good CRC + uint8_t buf[] = {0x00, 0x01, 0x01, 0xb1, 0x90}; + + + Mock 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 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 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 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 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 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 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 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 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 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 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); + +} \ No newline at end of file diff --git a/code/firmware/test/test_desktop/test_main.cpp b/code/firmware/test/test_desktop/test_main.cpp new file mode 100644 index 0000000..8455553 --- /dev/null +++ b/code/firmware/test/test_desktop/test_main.cpp @@ -0,0 +1,20 @@ +#include +#include + +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(); +} \ No newline at end of file diff --git a/code/firmware/test/test_embedded/test_main.cpp b/code/firmware/test/test_embedded/test_main.cpp new file mode 100644 index 0000000..f3ba110 --- /dev/null +++ b/code/firmware/test/test_embedded/test_main.cpp @@ -0,0 +1,23 @@ +#include +#include + +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); +} \ No newline at end of file