int SendData()
{
int i = 0;
int count = 0;
char ioControl[3];
int bytesRecieved = 0;
while (true)
{
bytesRecieved = SerialPort.ReadData(ioControl, 1);
printf("%c %d\t", ioControl[0], bytesRecieved);
if (ioControl[0] == 'r')
{
for (i = 0; i <= NUMBER_OF_BUTTONS; i++)
SerialPort.SendData(Controller[i].passedValue, sizeof(Controller[i].passedValue));
if (leapConnected == true)
{
for (i = 0; i < NUMBER_OF_LEAP_INPUTS; i++)
SerialPort.SendData(Leapvalues[i].passedValue, sizeof(Leapvalues[i].passedValue));
}
else
{
for (i = 0; i < NUMBER_OF_LEAP_INPUTS; i++)
SerialPort.SendData("0", 1);
}
//Sleep(59);
count++;
if (count > 5)
{
SerialPort.Close();
Sleep(1000);
SerialPort.Open(PORT_NUM, BAUD);
return 0;
}
}
else if (ioControl[0] == 'q')
{
return 1;
}
else
{
/*printf("Error: Unknown codeword passed from arduino [%c], aborting send\n", ioControl[0]);
Sleep(60);*/
}
}
}
int main(int argc, char *argv[])
{
filename = "myFile.txt";
port = 5;
baudRate = 9600;
if(argc == 1)
{
cout << "No parameters specified, using defaults." << endl;
cout << "To specify use DantecTraverse.exe [port [filename [baudRate]]]" << endl;
}
else if(argc >= 2)
{
port = fromString<int>(argv[1]);
}
else if(argc >= 3)
{
filename = argv[2];
}
else if(argc >= 4)
{
baudRate = fromString<int>(argv[3]);
}
cout << "Parameters are: COM " << port << "; filename: \"" << filename << "\"; Baud Rate: " << baudRate << endl;
if(!serialPort.Open(port, baudRate))
{
cout << "Cannot open serial port " << port << " with Baud Rate " << baudRate << endl;
throw 1;
}
while(1)
{
//checks if the file ends with a full stop on a seperate line, this signifies that YAPP is finished writing to the file
ifstream iFile(filename.c_str());
string content((istreambuf_iterator<char>(iFile)), istreambuf_iterator<char>());
if(content.rfind("\n.") != string::npos)
{
cout << content << endl;
doCommand(content);
ofstream oFile(filename.c_str(), fstream::trunc);
}
int waiting = serialPort.ReadDataWaiting();
if(waiting)
{
char* buffer = new char[waiting];
serialPort.ReadData(buffer, waiting);
string incomming = buffer;
cout << "Incoming: " << buffer << endl;
delete buffer;
}
}
}
int main(char argc, char *argv[])
{
int port = 3, baudRate = 9600, dispType = 0;
if (argc >= 2)
port = atoi(argv[2]);
if (argc >= 3)
baudRate = atoi(argv[3]);
if (argc >= 4)
dispType = atoi(argv[4]);
CSerial serial;
if (!serial.Open(port, baudRate))
return 0;
int curT=0, oldT=0;
while (1)
{
curT = GetTickCount();
if (curT - oldT > 10)
{
char buffer[256];
int nBytesRead = serial.ReadData(buffer, sizeof(buffer));
if (nBytesRead > 0)
{
for (int i = 0; i < nBytesRead; ++i)
{
switch (dispType)
{
case 0: printf("%c", buffer[i]); break;
case 1: printf("%d", buffer[i]); break;
case 2: printf("%x", buffer[i]); break;
}
}
printf("\n");
}
oldT = curT;
}
}
serial.Close();
return 1;
}
void _stdcall ReadCOM(HWND hWnd, UINT message, UINT iTimerID, DWORD dwTime)
{
unsigned char s[128];
char r[32];
memset(s, 0, sizeof(s));
memset(r, 0, sizeof(r));
int len=com.ReadData(s, sizeof(s));
if (len >= 32)
{
// MessageBox(NULL, r, "Read COM", MB_ICONINFORMATION);
sprintf(r, "%1X%02X-%02X%02X-%02X%02X#%02X%02X%02X%02X\0",
s[28], s[29],
s[30], s[31],
s[14], s[15],
s[10], s[11], s[12], s[13]);
Broadcast(r);
}
}
int SendData()
{
int i = 0;
int count = 0;
char ioControlBuffer[3];
char ioControl[3];
int bytesRecieved = 0;
SerialPort.SendData("hgl", 3);
while (true)
{
bytesRecieved = SerialPort.ReadData(ioControlBuffer, 1);
ioControl[0] = ioControlBuffer[0];
printf("%c\t%d\t", ioControlBuffer[0], bytesRecieved);
if (ioControl[0] == 'r')
{
for (i = 0; i <= NUMBER_OF_BUTTONS; i++)
SerialPort.SendData(Controller[i].passedValue, sizeof(Controller[i].passedValue));
count++;
if (count > 5)
{
Sleep(750);
return 0;
}
}
else if (ioControl[0] == 'q')
{
return 1;
}
else
{
printf("Error: Unknown codeword passed from arduino [%c], aborting send\n", ioControl[0]);
}
}
}
int main(int argc , char* argv[]){
for (int portNo = 0; portNo<5 ; ++portNo){
if(serial.findAvailablePorts(portNo))
cout<<"COM"<<portNo<<" available!"<<endl;
}
if(serial.Open(3,115200)){
cout<<"Connection initialized!"<<endl;
}
char szMessage[3] = {0x75, 0x92 , 0xE7};
serial.SendData(szMessage,3);
char* lpBuffer = new char[100];
char setOn[6] = {0x52, 0x86 , 0xD4 , 0x5A , 0x80 , 0xDA};
Sleep(300);
serial.ReadData(lpBuffer , 30);
if(controlComms.checkValidity(lpBuffer,20)!=0){
cout<<"Device is ready!"<<endl;
} else{
cout<<"Device is not ready!"<<endl;
}
Sleep(1000);
//Sleep(1000);
serial.SendData(setOn,sizeof(setOn));
Sleep(300);
thread t1(readData,"");
t1.join();
serial.Close();
printf("\n");
return 0;
}
void sensorFeedback(CPipe motorDone, CPipe motor, MotorData ST400, CSerial serial){
while(motorDone.TestForData() == 0){
//Check if a sensor detects something, otherwise wait until
//motor indicates it has completed current instructions
/*Insert sensor logic here (maybe a separate thread or new class?)*/
//Make sure ST400.stop is set to 1 if a sensor interrupt occurs
//and write to ST400Control
if (serial.Open(4, 9600))
{
char* lpBuffer = new char[100];
int nBytesRead = serial.ReadData(lpBuffer, 100);
strtok(lpBuffer,"\n");
if(atoi(lpBuffer) == 1){
ST400.stop=1;
cout<<"HIT"<<endl;
//cout<<"YAY I READ FROM ARDRUINO!"<<endl;
motor.Write(&ST400, sizeof(ST400));
ST400.stop=0;
ST400.leftWheel=0; //Prevent rover from executing remaining steps
ST400.rightWheel=0;
break;
}
else{
ST400.stop=0;
cout<<"MISS"<<endl;
}
}
}
};
void serial_receive(){
int nBytesRead = 0,buf_idx =0,found = 0;
while(1){
if (serial.Open(4, 9600)&&serialEnable==1){
char* lpBuffer = new char[500];
char myrightBuffer[500];
while(found == 0){
nBytesRead = serial.ReadData(lpBuffer, 500);
char *start = strstr(lpBuffer, "R");
char *endptr = strstr(lpBuffer,"+");
if(start) {
strncpy(myrightBuffer, start, endptr-start);
break;
}
}
cout <<"[MSP430]> "<< myrightBuffer << endl;
serialEnable = 0;
delete []lpBuffer;
}
}
}
BOOL readData(string str){
char* lpBuffer = new char[100];
while(true){
if(serial.ReadDataWaiting()){
try{
int bytesRead = serial.ReadData(lpBuffer,30);
controlComms.printChars(lpBuffer);
Sleep(500);
delete []lpBuffer;
} catch(exception ex){
}
}
Sleep(100);
}
if(serial.IsOpened()) serial.Close();
}
int main()
{
//Leap Motion Vairables
Leap::Controller controller;
Leap::Frame frame;
Leap::HandList hands;
Leap::Hand h1;
Leap::FingerList fingers;
Leap::Finger index;
Leap::Finger thumb;
Leap::PointableList pointables;
float indexX = 0, indexY = 0, indexZ = 0, thumbX = 0, thumbY = 0, thumbZ = 0, sum = 0;
unsigned long cycles = 0;
// TCP Variables
WSADATA wsaData;
SOCKET connectSocket = INVALID_SOCKET;
struct addrinfo* result = NULL;
struct addrinfo* ptr = NULL;
struct addrinfo hints;
char cSendBuf[512][512];
char sSendBuf[512];
int iResult;
int recvBufLen = DEFAULT_BUFLEN;
int i = 0;
iResult = WSAStartup(MAKEWORD(2, 2), &wsaData);
if (iResult != 0) {
printf("WSAStartup failed with error: %d\n", iResult);
return 1;
}
// Initialize all address info to 0 to start.
SecureZeroMemory(&hints, sizeof(hints));
hints.ai_family = AF_UNSPEC; // Doesn't matter if we use IPV4 or IPV6
hints.ai_socktype = SOCK_STREAM; // TCP Stream sockets
hints.ai_protocol = IPPROTO_TCP;
// Resolve the server address and port
iResult = getaddrinfo("127.0.0.1", DEFAULT_PORT, &hints, &result);
if (iResult != 0) {
printf("getaddrinfo failed with error: %d\n", iResult);
WSACleanup();
return 1;
}
// Attempt to connect to an address until one succeeds
for (ptr = result; ptr != NULL; ptr = ptr->ai_next) {
// create a socket for connecting to the server
connectSocket = socket(ptr->ai_family, ptr->ai_socktype, ptr->ai_protocol);
if (connectSocket == INVALID_SOCKET) {
printf("socket failed with error: %ld\n", WSAGetLastError());
WSACleanup();
return 1;
}
// Connect to the server
iResult = connect(connectSocket, ptr->ai_addr, (int)ptr->ai_addrlen);
if (iResult == SOCKET_ERROR) {
closesocket(connectSocket);
connectSocket = INVALID_SOCKET;
continue;
}
break;
}
// Deallocate the address info
freeaddrinfo(result);
if (connectSocket == INVALID_SOCKET) {
printf("Unable to connect to server!\n");
WSACleanup();
return 1;
}
// Setup serial port connection and needed variables.
SerialPort.Open(PORT_NUM, BAUD);
Controller[20].value = 9; //Verification Byte sent to make sure everything else ends up in the right location
FillByteSize();
while (true)
{
cycles++;
UpdateControllerState(); //Updates all values on the controller
WORD wButtons = g_Controllers[CONTROLLER1].state.Gamepad.wButtons;
//Stores all of the values from the controller into the controller structure
Controller[0].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbRX;
Controller[1].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbRY;
Controller[2].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbLX;
Controller[3].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbLY;
Controller[4].value = (g_Controllers[CONTROLLER1].state.Gamepad.bRightTrigger);
Controller[5].value = (g_Controllers[CONTROLLER1].state.Gamepad.bLeftTrigger);
Controller[6].value = (wButtons & XINPUT_GAMEPAD_RIGHT_THUMB);
Controller[7].value = (wButtons & XINPUT_GAMEPAD_LEFT_THUMB);
Controller[8].value = (wButtons & XINPUT_GAMEPAD_RIGHT_SHOULDER);
Controller[9].value = (wButtons & XINPUT_GAMEPAD_LEFT_SHOULDER);
Controller[10].value = (wButtons & XINPUT_GAMEPAD_DPAD_UP);
Controller[11].value = (wButtons & XINPUT_GAMEPAD_DPAD_DOWN);
Controller[12].value = (wButtons & XINPUT_GAMEPAD_DPAD_LEFT);
Controller[13].value = (wButtons & XINPUT_GAMEPAD_DPAD_RIGHT);
Controller[14].value = (wButtons & XINPUT_GAMEPAD_A);
Controller[15].value = (wButtons & XINPUT_GAMEPAD_B);
Controller[16].value = (wButtons & XINPUT_GAMEPAD_Y);
Controller[17].value = (wButtons & XINPUT_GAMEPAD_X);
Controller[18].value = (wButtons & XINPUT_GAMEPAD_START);
Controller[19].value = (wButtons & XINPUT_GAMEPAD_BACK);
CheckDeadZone();
if (controller.isConnected() == true)
{
sum = 0;
frame = controller.frame();
hands = frame.hands();
h1 = hands[0];
fingers = frame.fingers();
thumb = fingers[0];
index = fingers[1];
pointables = frame.pointables();
Leapvalues[0].value = h1.palmVelocity().x;
Leapvalues[1].value = h1.palmVelocity().y;
Leapvalues[2].value = h1.palmVelocity().z;
Leapvalues[3].value = h1.direction().pitch()*Leap::RAD_TO_DEG;
Leapvalues[4].value = h1.direction().yaw()*Leap::RAD_TO_DEG;
Leapvalues[5].value = h1.direction().roll()*Leap::RAD_TO_DEG;
indexX = index.tipPosition().x;
indexY = index.tipPosition().y;
indexZ = index.tipPosition().z;
thumbX = thumb.tipPosition().x;
thumbY = thumb.tipPosition().y;
thumbZ = thumb.tipPosition().z;
Leapvalues[6].value = sqrt(pow((indexX - thumbX), 2) + pow((indexY - thumbY), 2) + pow((indexZ - thumbZ), 2));
leapConnected = true;
CheckLeapDeadZone();
}
for (i = 6; i < NUMBER_OF_BUTTONS; i++) //DO NOT SET TO <= NUMBER_OF_BUTTONS, NOT A MISTAKE. Verification bit should always keep its value
{
{
Controller[i].value = AnalogToDigital(Controller[i].value); //converts all of the button presses on the controller to a binary value
}
}
//turns all of the numerical values into buffers that can be passed to the arduino
for (i = 0; i <= NUMBER_OF_BUTTONS; i++)
{
_itoa_s(Controller[i].value, Controller[i].passedValue, 10);
}
/*
for (i = 0; i < NUMBER_OF_BUTTONS; i++) {
_itoa_s(Controller[i].value, cSendBuf[i], 10);
cSendBuf[i][strlen(cSendBuf[i])] = '\0';
iResult = send(connectSocket, cSendBuf[0], (int)strlen(cSendBuf[0]), 0);
printf("String sent: %s\n", cSendBuf[0]);
// Check for errors
if (iResult == SOCKET_ERROR) {
printf("send failed with error: %d\n", WSAGetLastError());
closesocket(connectSocket);
WSACleanup();
return 1;
}
}
// Try to send the packet
iResult = send(connectSocket, "\n", (int)strlen("\n"), 0);
//printf("String sent: %s\n", sendBuf);
// Check for errors
if (iResult == SOCKET_ERROR) {
printf("send failed with error: %d\n", WSAGetLastError());
closesocket(connectSocket);
WSACleanup();
return 1;
}*/
if (leapConnected = true)
{
for (i = 0; i < NUMBER_OF_LEAP_INPUTS; i++)
{
_itoa_s(Leapvalues[i].value, Leapvalues[i].passedValue, 10);
}
}
/*Values recieved in this order:
0) YAW
1) PITCH
2) ROLL
3) ACCELERATION ON X AXIS
4) ACCELERATION ON Y AXIS
5) ACCELERATION ON Z AXIS
6) SONAR SENSOR DISTANCE (IN METERS)
*/
if (SendData() == 1)
{
for (i = 0; i < 7; i++)
{
while (SerialPort.ReadDataWaiting() < 3)
{
}
if (i > 2 && i < 6)
SerialPort.ReadData(received[i], 5);
else
SerialPort.ReadData(received[i], 4);
std::cout << received[i] << ' ';
// Added this 6.25.14
strcpy(sSendBuf, received[i]);
sSendBuf[strlen(sSendBuf)] = '\0';
iResult = send(connectSocket, sSendBuf, (int)strlen(sSendBuf), 0);
printf("String sent: %s\n", sSendBuf);
// Check for errors
if (iResult == SOCKET_ERROR) {
printf("send failed with error: %d\n", WSAGetLastError());
closesocket(connectSocket);
WSACleanup();
return 1;
}
// Try to send the packet
iResult = send(connectSocket, "\n", (int)strlen("\n"), 0);
//printf("String sent: %s\n", sendBuf);
// Check for errors
if (iResult == SOCKET_ERROR) {
printf("send failed with error: %d\n", WSAGetLastError());
closesocket(connectSocket);
WSACleanup();
return 1;
}
}
}
//std::cout << recieved[0];
printf("\t%d", cycles);
printf("\n");
//Sleep(500); <<'\t' << recieved[1]
}
closesocket(connectSocket);
WSACleanup();
return 0;
}
