B4L/feeder_mk2
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

firmware updated with encoder support, and support for the f031.

Browse Source
This commit is contained in:
Stephen Hawes
2022-07-28 14:40:30 -04:00
parent 51b8ca6efb
commit e51ccdd232
19 changed files with 1874 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
code/firmware/.gitignore vendored Normal file
View File

@@ -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

36
code/firmware/platformio.ini Normal file
View File

@@ -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/*

23
code/firmware/src/Feeder.h Normal file
View File

@@ -0,0 +1,23 @@
#ifndef _FEEDER_H
#define _FEEDER_H
#include <cstddef>
#include <cstdint>
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

187
code/firmware/src/IndexFeeder.cpp Normal file
View File

@@ -0,0 +1,187 @@
#include <Arduino.h>
#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);
}

46
code/firmware/src/IndexFeeder.h Normal file
View File

@@ -0,0 +1,46 @@
#ifndef _INDEX_FEEDER_H
#define _INDEX_FEEDER_H
#include "Feeder.h"
#include <functional>
#ifndef MOTOR_DEPS
#define MOTOR_DEPS
#include <RotaryEncoder.h>
#include <FastPID.h>
#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

269
code/firmware/src/IndexFeederProtocol.cpp Normal file
View File

@@ -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: <feeder address> 0x03 <uuid:12>
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: <command id>
// Response: <feeder address> <ok> <uuid:12>
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: <command id> <uuid:12>
//Response: <feeder address> <ok>
// 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: <command id> <uuid:12>
//Response: <feeder address> <ok> <uuid:12>
// 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);
}

34
code/firmware/src/IndexFeederProtocol.h Normal file
View File

@@ -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

193
code/firmware/src/IndexNetworkLayer.cpp Normal file
View File

@@ -0,0 +1,193 @@
#include "IndexNetworkLayer.h"
#include "Arduino.h"
#include <cstring>
#include <cstdio>
#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)
}
}

62
code/firmware/src/IndexNetworkLayer.h Normal file
View File

@@ -0,0 +1,62 @@
#ifndef _INDEX_PROTOCOL_H
#define _INDEX_PROTOCOL_H
#include <cstddef>
#include <cstdint>
#include <IndexPacketHandler.h>
#include <FastCRC.h>
#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

14
code/firmware/src/IndexPacketHandler.h Normal file
View File

@@ -0,0 +1,14 @@
#ifndef _INDEX_PROTOCOL_HANDLER_H
#define _INDEX_PROTOCOL_HANDLER_H
#include <cstddef>
#include <cstdint>
class IndexNetworkLayer;
class IndexPacketHandler {
public:
virtual void handle(IndexNetworkLayer *instance, uint8_t *buffer, size_t buffer_length) = 0;
};
#endif

35
code/firmware/src/define.h Normal file
View File

@@ -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

302
code/firmware/src/main.cpp Normal file
View File

@@ -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 <ArduinoFake.h>
#else
#include <Arduino.h>
#include <HardwareSerial.h>
#include <OneWire.h>
#include <DS2431.h>
#include <ArduinoUniqueID.h>
#endif // UNIT_TEST
#ifndef MOTOR_DEPS
#define MOTOR_DEPS
#include <RotaryEncoder.h>
//#include <PID_v1.h>
#endif
#include <IndexFeeder.h>
#include <IndexFeederProtocol.h>
#include <IndexNetworkLayer.h>
#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
}

0
code/firmware/src/util Normal file
View File

16
code/firmware/src/util.cpp Normal file
View File

@@ -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) )

26
code/firmware/src/util.h Normal file
View File

@@ -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

309
code/firmware/test/test_desktop/test_index_feeder_protocol.cpp Normal file
View File

@@ -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);
}

270
code/firmware/test/test_desktop/test_index_network_layer.cpp Normal file
View File

@@ -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);
}

20
code/firmware/test/test_desktop/test_main.cpp Normal file
View File

@@ -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();
}

23
code/firmware/test/test_embedded/test_main.cpp Normal file
View File

@@ -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);
}