Permalink
Cannot retrieve contributors at this time
Name already in use
A tag already exists with the provided branch name. Many Git commands accept both tag and branch names, so creating this branch may cause unexpected behavior. Are you sure you want to create this branch?
bfs/bfs/implementations/breadcrumbs/io_procs/TactorOutputIOProcessor.cpp
Go to fileThis commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
134 lines (115 sloc)
3.91 KB
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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"; | |
} | |
} |