From fe263def45395b51f92f7ccade002787cec3fd58 Mon Sep 17 00:00:00 2001 From: Nate Shaw Date: Fri, 8 May 2020 20:09:23 -0400 Subject: [PATCH] final tactor output processor code --- .../breadcrumbs/CMakeLists.txt | 4 + .../breadcrumbs/algos/AlgoBreadcrumbs.cpp | 4 + .../breadcrumbs/ea_tactor/ResponseHelper.cpp | 138 ++++++++++++++++++ .../gen/TactorOutputIOProcessor.cpp | 23 +++ .../include/BreadcrumbsConstants.hpp | 8 + .../breadcrumbs/include/ResponseHelper.h | 23 +++ .../include/TactorOutputIOProcessor.hpp | 31 ++++ .../io_procs/TactorOutputIOProcessor.cpp | 134 +++++++++++++++++ 8 files changed, 365 insertions(+) create mode 100644 bfs/implementations/breadcrumbs/ea_tactor/ResponseHelper.cpp create mode 100644 bfs/implementations/breadcrumbs/gen/TactorOutputIOProcessor.cpp create mode 100644 bfs/implementations/breadcrumbs/include/BreadcrumbsConstants.hpp create mode 100644 bfs/implementations/breadcrumbs/include/ResponseHelper.h create mode 100644 bfs/implementations/breadcrumbs/include/TactorOutputIOProcessor.hpp create mode 100644 bfs/implementations/breadcrumbs/io_procs/TactorOutputIOProcessor.cpp diff --git a/bfs/implementations/breadcrumbs/CMakeLists.txt b/bfs/implementations/breadcrumbs/CMakeLists.txt index 64a6766..b2ac1e1 100644 --- a/bfs/implementations/breadcrumbs/CMakeLists.txt +++ b/bfs/implementations/breadcrumbs/CMakeLists.txt @@ -2,3 +2,7 @@ set (VirtualOutputIOProcessor_IMPL_LIBS "ea_tdk" PARENT_SCOPE) + +set (TactorOutputIOProcessor_IMPL_LIBS + "ea_tdk" + PARENT_SCOPE) diff --git a/bfs/implementations/breadcrumbs/algos/AlgoBreadcrumbs.cpp b/bfs/implementations/breadcrumbs/algos/AlgoBreadcrumbs.cpp index 0ced946..1ae76b8 100644 --- a/bfs/implementations/breadcrumbs/algos/AlgoBreadcrumbs.cpp +++ b/bfs/implementations/breadcrumbs/algos/AlgoBreadcrumbs.cpp @@ -1,5 +1,6 @@ #include "AlgoBreadcrumbs.hpp" +#include "BreadcrumbsConstants.hpp" void AlgoBreadcrumbs::loop() @@ -19,6 +20,9 @@ void AlgoBreadcrumbs::loop() sendAttribute(msgCountAttrib); } + int degree = 0; + Attribute degreeAttrib = { ATTRKEY_EATAC_DEGREE, sizeof(double), °ree }; + sendAttribute(degreeAttrib); } bool AlgoBreadcrumbs::loopCondition() diff --git a/bfs/implementations/breadcrumbs/ea_tactor/ResponseHelper.cpp b/bfs/implementations/breadcrumbs/ea_tactor/ResponseHelper.cpp new file mode 100644 index 0000000..35fa228 --- /dev/null +++ b/bfs/implementations/breadcrumbs/ea_tactor/ResponseHelper.cpp @@ -0,0 +1,138 @@ +#include "ResponseHelper.h" + +#include +#include + +void PrintPacketMsg(string msg, float timer, unsigned char*buffer, int size) { + + printf("[TDK] %i,%f,%s,", 0, timer, msg.c_str()); + + for (int i = 0; i < size; ++i) + printf("%X:", buffer[i]); + printf("\n"); +} +unsigned char ComputeCheckSum(unsigned char* buf, int size) { + unsigned char checksum = 0x00; + for (int i = 0; i < size; ++i) { + checksum = checksum ^ buf[i]; + } + checksum ^= 0xEA; + + return checksum; +} + int __stdcall ParsePacket(int deviceID, unsigned char packet[READPACKETLIMIT], int size) + { + //[1] of the packet data is the type. + unsigned char type = packet[1]; + //[2] if the packet data is the data len + unsigned char len = packet[2]; + + unsigned char * data = new unsigned char[len]; + memset(data, '\0', len); + memcpy(data, &packet[3], len); + unsigned char chksum = packet[len + 3]; + //comupting the checksum of the packet to ensure we got the entire packet correctly + unsigned char computed_chksum = ComputeCheckSum(packet, len + 3); + + if (computed_chksum == chksum) + { + switch (type) + { + //ack packet + case 0xc8: + PrintPacketMsg("ACK WITH DATA", 0, packet, size); + break; + //ack packet + case 0xc9: + PrintPacketMsg("ACK", 0, packet, size); + break; + //nak packet + case 0xc4: { + PrintPacketMsg("NAK", 0, packet, size); + //[0] response to wich command + unsigned char responseTo = data[0]; + //[1] reason code for nak + unsigned char reasonCode = data[1]; + switch (reasonCode) { + case 0x01: //The ETX value was not found in the expected place + PrintPacketMsg("ETX value was not found ERROR", 0, packet, size); + break; + case 0x02: //The Checksum value was invalid + PrintPacketMsg(" Checksum value was invalid ERROR", 0, packet, size); + break; + case 0x03: //Insufficient Data in packet + PrintPacketMsg("Insufficient Data ERROR", 0, packet, size); + break; + case 0x04: //An invalid command was used + PrintPacketMsg("invalid command ERROR", 0, packet, size); + break; + case 0x05: //Invaid Data was given with a valid command + PrintPacketMsg("Invalid Data ERROR", 0, packet, size); + break; + case 0x06: //Data length exceeds max payload + PrintPacketMsg("Data length exceeds ERROR", 0, packet, size); + break; + case 0x07: //Invalid Sequence Data + PrintPacketMsg(" Invalid Sequence Data ERROR", 0, packet, size); + break; + case 0x08: //Bus Error Sending to Node failed after three attempts + PrintPacketMsg(" Bus Error Sending ERROR", 0, packet, size); + break; + case 0x09: //Bus Master Times out while waiting on all bytes of incoming command to finish + PrintPacketMsg("Bus Master Times out ERROR", 0, packet, size); + break; + case 0x0A: //Node has a Fault Cannot Complete Request + PrintPacketMsg("Fault ERROR", 0, packet, size); + break; + case 0x0B: // Sequence is busy � you tried to perform a new sequence operation while the previous one is still in progress � sequences not implemented on TDK boards + PrintPacketMsg("NAK_SeqBusy ERROR", 0, packet, size); + break; + case 0x0C: // not used anymore + PrintPacketMsg("NAK_TimeOut ERROR", 0, packet, size); + break; + case 0x0D: // Ramp List is Full + PrintPacketMsg("NAK_RampListFull ERROR", 0, packet, size); + break; + case 0x0E: // Action List is Full + PrintPacketMsg("NAK_ActionListFull ERROR", 0, packet, size); + break; + case 0x0F:// The command you sent is probably valid, but it has not been implemented on this board � you shouldn�t see this outside of early beta firmware + PrintPacketMsg("NAK_NotImplemented ERROR", 0, packet, size); + break; + case 0x10:// Master Packet is Full - you shouldn�t see this one � it means the Bus on eTSAS or other new distrib system is very busy with action-lists & ramps and can�t fit a new command at the instant you sent it. + PrintPacketMsg("NAK_MasterPacketFullOrBusy ERROR", 0, packet, size); + break; + case 0x11: // error while processing a command which is already in the action list + PrintPacketMsg("NAK_ActionListTooManyBytes ERROR", 0, packet, size); + break; + case 0x12: // another error while processing a command which is already in the action list + PrintPacketMsg("NAK_ActionListTooFewBytes", 0, packet, size); + break; + case 0x13: //your packet is too big to fit in the TDK receive buffer + PrintPacketMsg("NAK_TooMuch", 0, packet, size); + break; + case 0x14: //Received a command while Self Test is running + PrintPacketMsg("NAK_Received a command while Self Test is running ERROR", 0, packet, size); + break; + default: //did not find the error within the lookup table. + PrintPacketMsg("UNKNOWN ERROR", 0, packet, size); + break; + + } + break; + } + case 0xEE: + PrintPacketMsg("0xEE", 0, packet, size); + return -1; + break; + } + delete[] data; + } else { + PrintPacketMsg("checksum was bad", 0, packet, + size); + delete[] data; + return -1; + } + + return 0; +} diff --git a/bfs/implementations/breadcrumbs/gen/TactorOutputIOProcessor.cpp b/bfs/implementations/breadcrumbs/gen/TactorOutputIOProcessor.cpp new file mode 100644 index 0000000..2066e88 --- /dev/null +++ b/bfs/implementations/breadcrumbs/gen/TactorOutputIOProcessor.cpp @@ -0,0 +1,23 @@ + +#include + +#include "DataSyncThread.hpp" +#include "TactorOutputIOProcessor.hpp" + + +int main() +{ + IOProcessor* client = new TactorOutputIOProcessor; + + if (!client->init()) + { + while (client->loopCondition()) + client->loop(); + + int result = client->close(); + delete client; + return result; + } + + return 0; +} diff --git a/bfs/implementations/breadcrumbs/include/BreadcrumbsConstants.hpp b/bfs/implementations/breadcrumbs/include/BreadcrumbsConstants.hpp new file mode 100644 index 0000000..6dbf2ae --- /dev/null +++ b/bfs/implementations/breadcrumbs/include/BreadcrumbsConstants.hpp @@ -0,0 +1,8 @@ +#ifndef BREADCRUMBS_CONSTANTS_HPP +#define BREADCRUMBS_CONSTANTS_HPP + + +#define ATTRKEY_EATAC_DEGREE "EATACDEG" + + +#endif \ No newline at end of file diff --git a/bfs/implementations/breadcrumbs/include/ResponseHelper.h b/bfs/implementations/breadcrumbs/include/ResponseHelper.h new file mode 100644 index 0000000..29a88bd --- /dev/null +++ b/bfs/implementations/breadcrumbs/include/ResponseHelper.h @@ -0,0 +1,23 @@ +#ifndef _EAIRESPONSEH_ +#define _EAIRESPONSEH_ + +// This file was copied from the "TDK" folder in the examples directory! + +#include +#include +using namespace std; + +#define READPACKETLIMIT 512 // maximum bytes of an entire read packet + +// The calling convention on windows is stdcall, +// on Linux, this identifier doesn't exist. +#ifndef _WIN32 +#ifndef __stdcall +#define __stdcall +#endif // __stdcall +#endif // !_WIN32 + +//Parse Packet is the callback used for the device packet data. +int __stdcall ParsePacket(int deviceID, unsigned char packet[READPACKETLIMIT], int size); + +#endif diff --git a/bfs/implementations/breadcrumbs/include/TactorOutputIOProcessor.hpp b/bfs/implementations/breadcrumbs/include/TactorOutputIOProcessor.hpp new file mode 100644 index 0000000..9f15020 --- /dev/null +++ b/bfs/implementations/breadcrumbs/include/TactorOutputIOProcessor.hpp @@ -0,0 +1,31 @@ +#ifndef TACTOR_OUTPUT_IO_PROCESSOR_HPP +#define TACTOR_OUTPUT_IO_PROCESSOR_HPP + +#include + +#include "IOProcessor.hpp" +#include "DataSyncThread.hpp" +#include "Attribute.hpp" +#include "BreadcrumbsConstants.hpp" +#include "TactorInterface.h" +#include "ResponseHelper.h" + + +class TactorOutputIOProcessor : public IOProcessor +{ +public: + using IOProcessor::IOProcessor; + + void loop(); + bool loopCondition(); + double getTactorDistance(double tactorA, double tactorB, int tactorNumber); + void pulseTactors(double degree, int tactorNumber, int pulseRadius); + void checkForError(int errorCode); + int initializeController(); + +private: + bool isControllerInit = false; + double tactorDegree = 0; +}; + +#endif \ No newline at end of file diff --git a/bfs/implementations/breadcrumbs/io_procs/TactorOutputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/TactorOutputIOProcessor.cpp new file mode 100644 index 0000000..e034006 --- /dev/null +++ b/bfs/implementations/breadcrumbs/io_procs/TactorOutputIOProcessor.cpp @@ -0,0 +1,134 @@ +#include "TactorOutputIOProcessor.hpp" +#include "DataSyncThread.hpp" +#include "Attribute.hpp" +#include +#include + +//position A and position B +double TactorOutputIOProcessor::getTactorDistance(double tactorA, double tactorB, int tactorNumber) { + double tactorAOffset = tactorA + tactorNumber; + double difference = abs(tactorA - tactorB); + // We need to compute this for the case that one of the tactor positions is near 0 degrees + double offsetDifference = abs(tactorAOffset - tactorB); + if (offsetDifference < difference) + return offsetDifference; + return difference; +} + +/** + * degree: the degree that the tactors should point + * tactorNumber: the number of tactors + * pulseRadius: the number of tactors next to the degree that can be vibrating. + */ +void TactorOutputIOProcessor::pulseTactors(double degree, int tactorNumber, int pulseRadius) { + double pulsePosition = (degree / 360.0) * tactorNumber; + int deviceId = 0; + + // Record all the tactor magnitudes so we can compare them to the max and normalize them later on + + double* tactorMags = new double[tactorNumber]; + + double maxTactorMag = 0; + for (int i = 0; i < tactorNumber; i++) { + double distance1 = getTactorDistance(static_cast(i), pulsePosition, tactorNumber); + double distance2 = getTactorDistance(pulsePosition, static_cast(i), tactorNumber); + double distance = distance2; + if (distance1 < distance2) { + distance = distance1; + } + if (distance < pulseRadius) { + tactorMags[i] = abs(distance - pulseRadius); + if (tactorMags[i] > maxTactorMag) + maxTactorMag = tactorMags[i]; + } + else { + tactorMags[i] = -1; + } + } + // Pulsing the tactors at their normalized values + for (int i = 0; i < tactorNumber; i++) + // Pulse the tactor at a frequency proportional to the magnitude above + if (tactorMags[i] > 0) { + double magnitude = tactorMags[i] / maxTactorMag; + cout << "Pulsing " << i << " at a magnitude of " << magnitude << endl; + //setting frequency for all tactors + checkForError(ChangeFreq(deviceId, 0, 300, 0)); + //setting gain for varying tactors + checkForError(ChangeGain(deviceId, i, magnitude * 254 + 1, 0)); + checkForError(Pulse(deviceId, i, 150, 10)); + } + else { + checkForError(ChangeGain(deviceId, i, 1, 0)); + } + + //deallocate the array + delete[] tactorMags; +} + + +int TactorOutputIOProcessor::initializeController() { + + char* connectionName = (char*)"COM3"; //name of the tactor controller + int deviceId = -1; + + checkForError(InitializeTI()); + + //Trying to discover the tactor device + int deviceCount = Discover(DEVICE_TYPE_SERIAL); + + if (deviceCount > 0) { + //attempt to connect to device + //Get Discovered Name or connectionName works just fine + deviceId = Connect(GetDiscoveredDeviceName(0), DEVICE_TYPE_SERIAL, ParsePacket); + checkForError(deviceId); + } + else { + if (deviceCount == 0) + cout << "Discover found 0 devices\n"; + else + checkForError(deviceCount); + return 0; + } + if (deviceCount >= 0) + //Using the TI_Update method to return any errors that may have occured + checkForError(UpdateTI()); + return 0; +} + + +void TactorOutputIOProcessor::loop() +{ + + if (!isControllerInit) { + initializeController(); + isControllerInit = true; + } + + if (getComms()->areIncomingAttributesAvailable()) { + map attributes = getComms()->getIncomingAttributesMap(); + //looking for degree value + auto newDegree = attributes.find(string(ATTRKEY_EATAC_DEGREE)); + //if degree value available + if (newDegree != attributes.end()) { + this->tactorDegree = *((double*) ((*newDegree).second.getValue())); + } + } + pulseTactors(this->tactorDegree, 16, 2); + Sleep(5000); //five second delay between each pulse + +} + +bool TactorOutputIOProcessor::loopCondition() +{ + return true; +} + +void TactorOutputIOProcessor::checkForError(int errorCode) +{ + //check if something went wrong + if (errorCode < 0) + { + //gets the last error code recorded in the tactorinterface + std::cout << "Error Code " << GetLastEAIError() << "\nCheck EAI_Defines.h For Reason\n"; + } +} \ No newline at end of file