Merge pull request #18 from grf14003/tactor-processor-implentation

final tactor output processor code

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)

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), &degree };
+	sendAttribute(degreeAttrib);
 }
 
 bool AlgoBreadcrumbs::loopCondition()

bfs/implementations/breadcrumbs/ea_tactor/ResponseHelper.cpp

@@ -0,0 +1,138 @@
+#include "ResponseHelper.h"
+
+#include <cstring>
+#include <cstdio>
+
+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;
+}

bfs/implementations/breadcrumbs/gen/TactorOutputIOProcessor.cpp

@@ -0,0 +1,23 @@
+
+#include <iostream>
+
+#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;
+}

bfs/implementations/breadcrumbs/include/BreadcrumbsConstants.hpp

@@ -0,0 +1,8 @@
+#ifndef BREADCRUMBS_CONSTANTS_HPP
+#define BREADCRUMBS_CONSTANTS_HPP
+
+
+#define ATTRKEY_EATAC_DEGREE "EATACDEG"
+
+
+#endif

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 <iostream>
+#include <string>
+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

bfs/implementations/breadcrumbs/include/TactorOutputIOProcessor.hpp

@@ -0,0 +1,31 @@
+#ifndef TACTOR_OUTPUT_IO_PROCESSOR_HPP
+#define TACTOR_OUTPUT_IO_PROCESSOR_HPP
+
+#include <map>
+
+#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

bfs/implementations/breadcrumbs/io_procs/TactorOutputIOProcessor.cpp

@@ -0,0 +1,134 @@
+#include "TactorOutputIOProcessor.hpp"
+#include "DataSyncThread.hpp"
+#include "Attribute.hpp"
+#include <iostream>
+#include <random>
+
+//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<double>(i), pulsePosition, tactorNumber);
+		double distance2 = getTactorDistance(pulsePosition, static_cast<double>(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<string, Attribute> 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";
+	}
+}