int main(int argc, char **argv) { const char serialBuffer[6] = "hello"; int serialOpen, nBytesRead; serialOpen=serial.Open(6,9600); cout<<serialOpen; if(serialOpen){ nBytesRead=serial.SendData(serialBuffer,strlen(serialBuffer)); //ASSERT(nBytesRead==strlen(serialBuffer)); //delete [] serialBuffer; } serial.Close(); cout << "\n"; cout << "=========================================================\n"; cout << "getTristarLog aborted. \n\n"; return 0; }
void sentMapToMSP(){ cout << "[Serial] Opening Serial Port Comm. . . . " <<endl; if (serial.Open(4, 9600)){ cout << "[Serial] Port opened successfully" << endl; }else{ cout << "[Serial] Failed to open port!" << endl; return ; } // send SOM_MAP to MSP 430 for(int i = 0 ; i < ROW ; i++ ){ for(int j = 0 ; j < COL ; j++ ){ for(int e = 0 ; e < element_count ; e++){ cout << "["<<i<<"]["<<j<<"]["<<e<<"]"; serial_sent_double(som_map[i][j].weights[e]); } } } for(int i = 0 ; i < ROW ; i++ ){ for(int j = 0 ; j < COL ; j++ ){ cout << "["<<i<<"]["<<j<<"]"; serial_sent_int(plotter[i][j]); } } }
int WINAPI _tWinMain (HINSTANCE /*hInst*/, HINSTANCE /*hInstPrev*/, LPTSTR /*lptszCmdLine*/, int /*nCmdShow*/) { CSerial serial; LONG lLastError = ERROR_SUCCESS; // Attempt to open the serial port (COM1) lLastError = serial.Open(_T("COM1"),0,0,false); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to open COM-port")); // Setup the serial port (9600,N81) using hardware handshaking lLastError = serial.Setup(CSerial::EBaud9600,CSerial::EData8,CSerial::EParNone,CSerial::EStop1); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port setting")); // Setup handshaking lLastError = serial.SetupHandshaking(CSerial::EHandshakeHardware); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port handshaking")); // The serial port is now ready and we can send/receive data. If // the following call blocks, then the other side doesn't support // hardware handshaking. lLastError = serial.Write("1\n"); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to send data")); // Close the port again serial.Close(); return 0; }
void FlashTool::enterBootloader() { CSerial serial; if(!serial.Open(ui->ucPortBox->itemData(ui->ucPortBox->currentIndex()).toString())) { QMessageBox::critical(this, "Error", "Could not open serial port."); return; } serial.Setup(CSerial::EBaud115200); serial.SetupHandshaking(CSerial::EHandshakeOff); serial.SetEventChar(0x0D); int version; proto::SimplePacket<proto::CMD_INIT> init; for(version = 0; version <= proto::VERSION+10; ++version) { init.header.version = version; init.checksum = proto::packetChecksum(init.header, 0); serial.write(&init, sizeof(init)); if(serial.WaitEvent(200) == ERROR_SUCCESS) break; } proto::Packet<proto::CMD_RESET, proto::Reset> reset; reset.header.version = version; memcpy(&reset.payload.key, proto::RESET_KEY, sizeof(proto::RESET_KEY)); reset.updateChecksum(); serial.write(&reset, sizeof(reset)); }
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 FlashTool::updatePorts() { CSerial serial; ui->progPortBox->clear(); ui->ucPortBox->clear(); for(int i = 1; i < 50; ++i) { QString path = "\\\\.\\COM" + QString::number(i); if(serial.Open(path)) { ui->progPortBox->addItem("COM" + QString::number(i), path); ui->ucPortBox->addItem("COM" + QString::number(i), path); serial.close(); } } ui->progPortBox->addItem("usb (e.g. AVRISP mkII)", "usb"); }
int SerialSetup (CSerial::EBaudrate baud, CSerial::EDataBits data, CSerial::EParity par, CSerial::EStopBits stop, CSerial::EHandshake handshake) { // Attempt to open the serial port (COM1) lLastError = serial.Open(_T("COM1"),0,0,false); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to open COM-port")); // Setup the serial port (9600,N81) using hardware handshaking lLastError = serial.Setup(baud, data, par, stop); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port setting")); // Setup handshaking lLastError = serial.SetupHandshaking(handshake); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port handshaking")); // if no error return 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 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; } } }
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; } } } };
bool transmit::changelamp(bool flag, int id, bool status, bool oldstatus){ char* buffer = new char[4]; bool tmp; if(flag){ buffer[0] = '?'; } else{ buffer[0] = '!'; } if(true){ switch(id+1){ case 1: buffer[1] = '1'; break; case 2: buffer[1] = '2'; break; case 3: buffer[1] = '3'; break; case 4: buffer[1] = '4'; break; case 5: buffer[1] = '5'; break; case 6: buffer[1] = '6'; break; case 7: buffer[1] = '7'; break; case 8: buffer[1] = '8'; break; case 9: buffer[1] = '9'; break; case 10: buffer[1] = 'a'; break; case 11: buffer[1] = 'b'; break; case 12: buffer[1] = 'c'; break; case 13: buffer[1] = 'd'; break; case 14: buffer[1] = 'e'; break; case 15: buffer[1] = 'f'; break; default: break; } } if(status == oldstatus){ tmp = false; return tmp; } if(status && status != oldstatus){ buffer[2] = 'f'; tmp = true; } if(!status && status != oldstatus){ buffer[2] = '0'; tmp = true; } buffer[3] = char(13); if(true){ CSerial* serial = new CSerial(); if(!serial->Open(2, 9600)) { qDebug() << "Could not open COM"; } else{ serial->SendData(buffer, 4); serial->Close(); } delete serial; } qDebug() << buffer[0]; qDebug() << buffer[1]; qDebug() << buffer[2]; qDebug() << buffer[3]; return tmp; }
int _tmain(int argc, _TCHAR* argv[]) { printf("Camera Tracker\n"); #ifdef ENABLE_SERIAL serial.Open(4, 57600); resetGyro = true; SendQuadcopterData(); #endif VideoCapture cap(0); Mat img; bgImg = imread(bgFilename); if (!bgImg.empty()) { printf("Loaded BG img\n"); frameCounter = 10; } char * imageWindowName = "Image"; char * diffWindowName = "Diff"; namedWindow(imageWindowName); namedWindow(diffWindowName); createTrackbar("Lift", imageWindowName, &lift, 255, onLiftChange); createTrackbar("Yaw", imageWindowName, &yaw, 255, onYawChange); createTrackbar("Pitch", imageWindowName, &pitch, 255, onPitchChange); createTrackbar("Roll", imageWindowName, &roll, 255, onRollChange); createTrackbar("GyroDiv", imageWindowName, &gyroDiv, 255, onGyroDivChange); while (1) { cap.read(img); if (img.empty()) { printf("Unable to grab frame\n"); return 0; } if (frameCounter < 5) img.copyTo(bgImg); subtract(img, bgImg, fgImg); threshold(fgImg, fgImg, 150, 255, CV_THRESH_BINARY); imshow(imageWindowName, img); imshow(diffWindowName, fgImg); moveWindow(imageWindowName, 0, 0); moveWindow(diffWindowName, 0, img.rows + 300); char c = cvWaitKey(5); if (c == 27) break; if (c == 'r') frameCounter = 0; if (c == 'g') { resetGyro = true; SendQuadcopterData(); } if (c == ' ') { lift = 0; SendQuadcopterData(); } if (c == 's') { printf("Saving background image\n"); imwrite(bgFilename, img); } //heartbeat if ((frameCounter % 15) == 0) SendQuadcopterData(); frameCounter++; } #ifdef ENABLE_SERIAL serial.Close(); #endif return 0; }
/** <policy-file-request/>: Send flex policy TT: Test ET: Exit OCx: Open COMx for reading CC: Close COM **/ void OnRead(SOCKET client) { char buf[BUFFER_SIZE]; memset(buf, 0, BUFFER_SIZE); recv(client, buf, BUFFER_SIZE, 0); // MessageBox(NULL, buf, "Socket Receive", MB_ICONINFORMATION); if (strstr(buf, CMD_POLICY) == buf) { sprintf(buf, FLEX_DOMAIN_POLICY); send(client, buf, strlen(buf) + 1, 0); } else if (strstr(buf, CMD_TEST) == buf) { sprintf(buf, "Welcome to Flex-Aide!\0"); send(client, buf, strlen(buf) + 1, 0); } else if (strstr(buf, CMD_EXIT) == buf) { PostQuitMessage(0); } else if (strstr(buf, CMD_OPENCOM) == buf) { int port = (buf[2] == 0) ? 1 : buf[2] - '0'; if (port < SERIAL_MIN || port > SERIAL_MAX) port = SERIAL_MIN; if (!com.Open(port, 57600)) { send(client, SERIAL_ERR, strlen(SERIAL_ERR) + 1, 0); } else { send(client, SERIAL_OK, strlen(SERIAL_OK) + 1, 0); } SetTimer(wNoop, TIMER_ID, 1000, ReadCOM); } else if (strstr(buf, CMD_CLOSECOM) == buf) { com.Close(); KillTimer(wNoop, TIMER_ID); } else if (strstr(buf, CMD_EXECUTE) == buf) { UINT uCmdShow = (buf[2] == 'H') ? SW_HIDE : SW_SHOWNORMAL; char cmd[MAX_PATH], fcmd[MAX_PATH], ncmd[MAX_PATH]; memset(cmd, 0, MAX_PATH); memset(fcmd, 0, MAX_PATH); memset(ncmd, 0, MAX_PATH); strncpy(cmd, buf + 3, strlen(buf) - 3); LPCSTR lpCmdLine; if (strstr(cmd, "..\\") == cmd) { strncpy(ncmd, cmd + 3, strlen(cmd) - 3); sprintf(fcmd, "%s\\%s", szParentPath, ncmd); lpCmdLine = fcmd; } else { lpCmdLine = cmd; } // WinExec(lpCmdLine, uCmdShow); SHELLEXECUTEINFO exec = {0}; exec.cbSize = sizeof(SHELLEXECUTEINFO); exec.fMask = SEE_MASK_NOCLOSEPROCESS; exec.hwnd = NULL; exec.lpVerb = NULL; exec.lpFile = lpCmdLine; exec.lpParameters = ""; exec.lpDirectory = NULL; exec.nShow = uCmdShow; exec.hInstApp = NULL; ShellExecuteEx(&exec); // WaitForSingleObject(exec.hProcess,INFINITE); } else { send(client, buf, strlen(buf) + 1, 0); } // MessageBox(NULL, buf, "Socket Send", MB_ICONINFORMATION); }
int main (){ CProcess p1("C:\\March01\\Debug\\ColourDetection.exe", // pathlist to child program executable NORMAL_PRIORITY_CLASS, // priority OWN_WINDOW, // process has its own window ACTIVE // process is active immediately ); CProcess p3("C:\\March01\\Debug\\ST400Control.exe", // pathlist to child program executable NORMAL_PRIORITY_CLASS, // priority OWN_WINDOW, // process has its own window ACTIVE // process is active immediately ); double debugging; double obstacleSensor = 0; double steps = 400; // Set to 400 half-steps per Revolution. string sensorVoltage; double sensorVolt; struct botData roverData; // For Reading data from Image Processor. roverData.posXbot = 0.0; roverData.posYbot = 0.0; roverData.g_angle = 0.0; CSerial serial; // CREATE A SERIAL Object if (serial.Open(8, 9600)) printf("Port opened successfully"); else printf("Failed to open port!"); cout << "Please Enter Debugging Mode \n"; cout << "0 - Send A Number of Steps to Left and Right Wheels. \n"; cout << "1 - Rover Travels to a X-Y Coordinate we type in. \n"; cout << "2 - Rover Follows One Ball autonomously. \n"; cout << "3 - Find all balls autonomously. \n"; cin >> debugging; Rover rover1; if(debugging==0){ cout << "Entered Debugging Mode 0: Enter Left followed by Right Wheel Motor Steps \n"; CProcess p("C:\\March01\\Debug\\KeyboardInput.exe", // pathlist to child program executable NORMAL_PRIORITY_CLASS, // priority OWN_WINDOW, // process has its own window ACTIVE // process is active immediately ); cout << "\n Right after Process created"; CPipe pleft("leftMotorPipe",1024); CPipe pright("rightMotorPipe",1024); cout << "\n After pleft and pright pipes created\n\n"; double leftWheelRead=0; double rightWheelRead=0; double leftFirst=0; double rightFirst=0; ST400.leftWheel=0; ST400.rightWheel=0; ST400.stop = 0; CPipe motor("MotorData",1024); // WRITE-ONLY pipeline to ST400Control CPipe motorDone("Stopped", 1024); // READ-ONLY pipeline from ST400Control int status = 0; // =1 if robot motion is complete while(true){ //Check if the user has input values for both the left and right wheels and read the data if they have. if(pleft.TestForData()>=sizeof(leftWheelRead) && pright.TestForData()>=sizeof(rightWheelRead)) { pleft.Read(&leftWheelRead,sizeof(leftWheelRead)); cout << "leftWheel is: " <<leftWheelRead << "\n"; pright.Read(&rightWheelRead,sizeof(rightWheelRead)); cout << "rightWheel is: " <<rightWheelRead << "\n"; }// //Nested while loop - only executes when there are steps to be travelled while(leftWheelRead!=0 && rightWheelRead!=0) { //If the steps are not equal for each motor, turn rover if(abs(leftWheelRead) != abs(rightWheelRead)){ //Turn Left if(leftWheelRead>rightWheelRead) { leftFirst = (leftWheelRead - rightWheelRead)/2.0; rightFirst = -leftFirst; leftWheelRead = rightWheelRead; printf("Turning left...\n"); } //Turn right else{ rightFirst = (rightWheelRead - leftWheelRead)/2.0; leftFirst = -rightFirst; rightWheelRead = leftWheelRead; printf("Turning right...\n"); } ST400.leftWheel = (leftFirst<5000) ? leftFirst : 5000; ST400.rightWheel = (rightFirst<5000) ? rightFirst : 5000; //Send the instructions to ST400Control motor.Write(&ST400, sizeof(ST400)); //sensorFeedback(motorDone, motor, ST400, serial); // THE WHILE LOOP REPLACED HERE. motorDone.Read(&status,sizeof(status)); //Empty pipe to prevent infinite loop if(status==1) { printf("Motion completed...\n"); } } //Otherwise move straight ahead else{ ST400.rightWheel= (rightWheelRead < 5000) ? rightWheelRead : 5000; ST400.leftWheel= (leftWheelRead < 5000) ? leftWheelRead : 5000; leftWheelRead=0; rightWheelRead=0; motor.Write(&ST400,sizeof(ST400)); printf("Driving straight ahead...\n"); //sensorFeedback(motorDone, motor, ST400, serial);// THE WHILE LOOP REPLACED HERE. motorDone.Read(&status,sizeof(status)); //Empty pipe to prevent infinite loop if(status==1) { printf("Motion completed...\n"); } } } char cc = cvWaitKey(1); if(cc == 27) break; } } // End of Debugging 0. else if (debugging == 1){ //double xInput; //ifstream inFile; // //inFile.open("c:\\xcoord.txt"); //if (!inFile) { // cout << "Unable to open file"; // exit(1); // terminate with error //} // //while (inFile >> xInput) { // cout << "X coordinate is: " << xInput << endl; //} // //inFile.close(); // //double yInput; //ifstream inFile1; // //inFile1.open("c:\\ycoord.txt"); //if (!inFile1) { // cout << "Unable to open file"; // exit(1); // terminate with error //} // //while (inFile1 >> yInput) { // cout << "Y coordinate is: " << yInput << endl; //} // //inFile1.close(); CPipe motor("MotorData",1024); //WRITE-ONLY pipeline to ST400Control CPipe motorDone("Stopped",1024); //READ-ONLY pipeline from ST400Control ST400.leftWheel=0; ST400.rightWheel=0; ST400.stop=0; int status = 0; //=1 if robot motion is complete CPipe pMainPipe("MainPipe",1024); while(true){ cout << "Entered Debugging Mode 1: Enter Coordinates for Rover to Go to. \n" ; double xInput, yInput; cout << "Please Enter Destination X Coordinate: [0 to 720] \n" ; cin >> xInput; cout << "Please Enter Destination Y Coordinate: [0 to 480] \n" ; cin >> yInput; //char c = cvWaitKey(1); //if(c == 27) break; while(roverData.posXbot < 170 || roverData.posXbot > 570 || roverData.posYbot < 2 || roverData.posYbot > 474){ pMainPipe.Read(&roverData,sizeof(roverData)); } rover1.coordinate.x = roverData.posXbot; // RANDY: Display rover x coordinate from this variable. rover1.coordinate.y = roverData.posYbot; // RANDY: Display rover y coordinate from this variable. rover1.direction = roverData.g_angle; // RANDY: Display rover angle from this variable. Point2D ball(xInput, yInput); double ballxUpper = ball.x+30; double ballxLower = ball.x-30; double ballyUpper = ball.y+30; double ballyLower = ball.y-30; if((rover1.coordinate.x < ballxUpper) && (rover1.coordinate.x > ballxLower) && (rover1.coordinate.y < ballyUpper) && (rover1.coordinate.y > ballyLower)){ //return 0; } // Calculate Heading. rover1.headingRange = rover1.calcHeading(ball,rover1.coordinate,rover1.direction); double headingInRevolutions = rover1.headingRange.heading*980/360;// * (1/(10*PI))*((16*PI)/360); // RANDY: Display headingInRevolutions and rename to turning steps. // Calculate Range. double rangeInRevolutions = rover1.headingRange.range*6.04;// * 0.508/(10*PI); // RANDY: Display rangeinRevolutions and rename to range steps. /* Motor control starts here ************************************************************************/ if(headingInRevolutions < -50){ // If ClockWise. LEFTWHEEL = MORE. // double turnRate; ST400.leftWheel = headingInRevolutions; ST400.rightWheel = -headingInRevolutions; //ST400.leftWheel = ST400.leftWheel/2.0; //ST400.rightWheel = -ST400.leftWheel; motor.Write(&ST400, sizeof(ST400)); headingInRevolutions = 0; // Reset Turning to 0 so that rover will now go straight. //sensorFeedback(motorDone, motor, ST400, serial);// THE WHILE LOOP REPLACED HERE. motorDone.Read(&status,sizeof(status)); //Empty pipe to prevent infinite loop while(motorDone.TestForData() == 0){ // Wait until done. - Added. } } else if(headingInRevolutions > 50){ // If CounterClockWise. RIGHTWHEEL = MORE. //ST400.leftWheel = (headingInRevolutions<5000) ? -headingInRevolutions/2 : 5000; //ST400.rightWheel = (headingInRevolutions<5000) ? headingInRevolutions/2 : 5000; ST400.leftWheel = -headingInRevolutions; ST400.rightWheel = headingInRevolutions; motor.Write(&ST400, sizeof(ST400)); headingInRevolutions = 0; // Reset Turning to 0 so that rover will now go straight. //sensorFeedback(motorDone, motor, ST400, serial);// THE WHILE LOOP REPLACED HERE. motorDone.Read(&status,sizeof(status)); //Empty pipe to prevent infinite loop while(motorDone.TestForData() != 0){ // Wait until done. - Added. } } //if(headingInRevolutions==0){ // If Straight. ST400.leftWheel = rangeInRevolutions; ST400.rightWheel = rangeInRevolutions; if(motor.TestForData() == 0 && ST400.rightWheel!=0){ motor.Write(&ST400,sizeof(ST400)); printf("Going Straight: wrote identical data to both wheels\n"); //Reset these commands to 0 after sending the instruction //so the motors run for the input steps only once ST400.leftWheel=0; ST400.rightWheel=0; //sensorFeedback(motorDone, motor, ST400, serial);// THE WHILE LOOP REPLACED HERE. motorDone.Read(&status,sizeof(status)); //Empty pipe to prevent infinite loop while(motorDone.TestForData() != 0){ // Wait until done. - Added. } } if(motor.TestForData()==0) { printf("Pipe empty\n"); } //char cc = cvWaitKey(1); //if(cc == 27) break; //} // End of While Loop. //serial.Close(); } }// End of Debugging Mode 1.
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; }
int main(int argc, char* argv[]) { //Initialize OpenCV CvSize size640x480 = cvSize(640, 480); CvCapture* p_capWebcam; IplImage* p_imgOriginal; IplImage* hsv; //Initialize globals used throughout the program ofstream dataFile; int netTime = 0; int intervalTime = 1000; int hourCount = 1; int avgCounter = 1; float avgH =0, avgS =0, avgV =0, avgH2 =0, avgS2 =0, avgV2 =0; //More misc initialization for OpenCV and the program CvMemStorage* p_strStorage; char charCheckForEscKey; p_capWebcam = cvCaptureFromCAM(0); //Safety check to make sure the camera is there if(p_capWebcam == NULL) { printf("error: capture is NULL \n"); getchar(); return(-1); } //Declare video feed window cvNamedWindow("Original", CV_WINDOW_AUTOSIZE); //Create new image that will be the converted hsv values hsv = cvCreateImage(size640x480, IPL_DEPTH_8U, 3); //Infinite loop (until esc sequence) while(1) { //Get the current frame p_imgOriginal = cvQueryFrame(p_capWebcam); //Convert RGB to HSV in a separate image cvCvtColor( p_imgOriginal, hsv, CV_BGR2HSV ); //Safety check to make sure camera actually captured stuff if(p_imgOriginal == NULL) { printf("error: frame is NULL \n"); getchar(); break; } //Initilization of vars for the average algorithm float H=0, S=0, V=0, R=0, G=0, B=0, pixCount =1; float H2=0, S2=0, V2=0, R2=0, G2=0, B2=0, pixCount2 = 1; CvScalar curValHSV; CvScalar curValRGB; //Average algorithm that takes the average for 2 reactors and 2 sets of values (RGB and HSV) for(int row=190; row<290; row++){ for(int col=150; col<250; col++){ curValHSV = cvGet2D(hsv,row,col); curValRGB = cvGet2D(p_imgOriginal,row,col); H=(H*(pixCount-1)+curValHSV.val[0])/pixCount; S=(S*(pixCount-1)+curValHSV.val[1])/pixCount; V=(V*(pixCount-1)+curValHSV.val[2])/pixCount; B=(B*(pixCount-1)+curValRGB.val[0])/pixCount; G=(G*(pixCount-1)+curValRGB.val[1])/pixCount; R=(R*(pixCount-1)+curValRGB.val[2])/pixCount; pixCount++; } for(int col=390; col<490; col++){ curValHSV = cvGet2D(hsv,row,col); curValRGB = cvGet2D(p_imgOriginal,row,col); H2=(H2*(pixCount-1)+curValHSV.val[0])/pixCount; S2=(S2*(pixCount-1)+curValHSV.val[1])/pixCount; V2=(V2*(pixCount-1)+curValHSV.val[2])/pixCount; B2=(B2*(pixCount-1)+curValRGB.val[0])/pixCount; G2=(G2*(pixCount-1)+curValRGB.val[1])/pixCount; R2=(R2*(pixCount-1)+curValRGB.val[2])/pixCount; pixCount2++; } } printf("H1=%f S1=%f V1=%f\n", H, S, V); printf("H2=%f S2=%f V2=%f\n", H2, S2, V2); //Draw circles of the average colour of each reactor cvCircle(p_imgOriginal, cvPoint(213,500), 50, CV_RGB(R, G, B), CV_FILLED); cvCircle(p_imgOriginal, cvPoint(426,500), 50, CV_RGB(R2, G2, B2), CV_FILLED); //Draw helper "cross hair" rectangles cvRectangle(p_imgOriginal, cvPoint(150,190), cvPoint(250,290), CV_RGB(255,0,0), 3); cvRectangle(p_imgOriginal, cvPoint(390,190), cvPoint(490,290), CV_RGB(255,0,0), 3); //Allocate necessary memory p_strStorage = cvCreateMemStorage(0); //Display video feed cvShowImage("Original", p_imgOriginal); //Start serial communication CSerial serial; //If opening the serial port is succesful if (serial.Open(3, 9600)) { //If the algae are green and are unsaturated/dying if( (H>28 && H<44) && S<230){ //Send the microcontroller the message to turn the motor static char szMessage = '1'; serial.SendData(&szMessage, 1); serial.Close(); char * outputc = new char[6]; outputc[6]=0; //Wait for servo success code and temperature for(int i=0; i<6; i++){ outputc[i]=readCharLoop(); } printf("Temperature: %s degrees C \n\n", outputc); int tens = outputc[2]-48; int ones = outputc[3]-48; int dec = outputc[5]-48; float num = tens*10+ones+0.1*dec; } else{ static char szMessage = '0'; serial.SendData(&szMessage, 1); serial.Close(); char * outputc = new char[5]; outputc[5]=0; //Wait for temperature to come in for(int i=0; i<5; i++){ outputc[i]=readCharLoop(); } printf("Temperature: %s degrees C \n\n", outputc); int tens = outputc[1]-48; int ones = outputc[2]-48; int dec = outputc[4]-48; float num = tens*10+ones+0.1*dec; } } else printf("Failed to open port!\n\n\n"); //Release memory cvReleaseMemStorage(&p_strStorage); //If esc key pressed, then break loop charCheckForEscKey = cvWaitKey(10); if(charCheckForEscKey == 27) break; //Placeholder time interval component (replace with time.h functions) Sleep(intervalTime); netTime += intervalTime; //Save data locally to a .txt file if(netTime/1000/60/60 >= hourCount) { avgH=(avgH*(avgCounter-1)+H)/avgCounter; avgS=(avgS*(avgCounter-1)+S)/avgCounter; avgV=(avgV*(avgCounter-1)+V)/avgCounter; avgH2=(avgH2*(avgCounter-1)+H2)/avgCounter; avgS2=(avgS2*(avgCounter-1)+S2)/avgCounter; avgV2=(avgV2*(avgCounter-1)+V2)/avgCounter; if (avgCounter >=5){ dataFile.open("data.txt", std::ios_base::app); dataFile << "Time since beginning: "<< hourCount << ":00 hours \n" ; dataFile << "HSV (SD): " << avgH << ", " << avgS << ", "<< avgV << "\n"; dataFile << "HSV (C): " << avgH2 << ", " << avgS2 << ", "<< avgV2 << "\n\n"; dataFile.close(); hourCount +=1; avgCounter=0; avgH =0; avgS =0; avgV =0; avgH2 =0; avgS2 =0; avgV2 =0; } avgCounter+=1; } } // end while //Release more memory and destroy the window when done cvReleaseCapture(&p_capWebcam); cvDestroyWindow("Original"); return(0); }
int __cdecl _tmain (int /*argc*/, char** /*argv*/) { CSerial serial; LONG lLastError = ERROR_SUCCESS; // Attempt to open the serial port (COM1) lLastError = serial.Open(_T("COM1"),0,0,false); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to open COM-port")); // Setup the serial port (9600,8N1, which is the default setting) lLastError = serial.Setup(CSerial::EBaud9600,CSerial::EData8,CSerial::EParNone,CSerial::EStop1); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port setting")); // Register only for the receive event lLastError = serial.SetMask(CSerial::EEventBreak | CSerial::EEventCTS | CSerial::EEventDSR | CSerial::EEventError | CSerial::EEventRing | CSerial::EEventRLSD | CSerial::EEventRecv); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port event mask")); // Use 'non-blocking' reads, because we don't know how many bytes // will be received. This is normally the most convenient mode // (and also the default mode for reading data). lLastError = serial.SetupReadTimeouts(CSerial::EReadTimeoutNonblocking); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port read timeout.")); // Keep reading data, until an EOF (CTRL-Z) has been received bool fContinue = true; do { // Wait for an event lLastError = serial.WaitEvent(); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to wait for a COM-port event.")); // Save event const CSerial::EEvent eEvent = serial.GetEventType(); // Handle break event if (eEvent & CSerial::EEventBreak) { printf("\n### BREAK received ###\n"); } // Handle CTS event if (eEvent & CSerial::EEventCTS) { printf("\n### Clear to send %s ###\n", serial.GetCTS()?"on":"off"); } // Handle DSR event if (eEvent & CSerial::EEventDSR) { printf("\n### Data set ready %s ###\n", serial.GetDSR()?"on":"off"); } // Handle error event if (eEvent & CSerial::EEventError) { printf("\n### ERROR: "); switch (serial.GetError()) { case CSerial::EErrorBreak: printf("Break condition"); break; case CSerial::EErrorFrame: printf("Framing error"); break; case CSerial::EErrorIOE: printf("IO device error"); break; case CSerial::EErrorMode: printf("Unsupported mode"); break; case CSerial::EErrorOverrun: printf("Buffer overrun"); break; case CSerial::EErrorRxOver: printf("Input buffer overflow"); break; case CSerial::EErrorParity: printf("Input parity error"); break; case CSerial::EErrorTxFull: printf("Output buffer full"); break; default: printf("Unknown"); break; } printf(" ###\n"); } // Handle ring event if (eEvent & CSerial::EEventRing) { printf("\n### RING ###\n"); } // Handle RLSD/CD event if (eEvent & CSerial::EEventRLSD) { printf("\n### RLSD/CD %s ###\n", serial.GetRLSD()?"on":"off"); } // Handle data receive event if (eEvent & CSerial::EEventRecv) { // Read data, until there is nothing left DWORD dwBytesRead = 0; char szBuffer[101]; do { // Read data from the COM-port lLastError = serial.Read(szBuffer,sizeof(szBuffer)-1,&dwBytesRead); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to read from COM-port.")); if (dwBytesRead > 0) { // Finalize the data, so it is a valid string szBuffer[dwBytesRead] = '\0'; // Display the data printf("%s", szBuffer); // Check if EOF (CTRL+'[') has been specified if (strchr(szBuffer,EOF_Char)) fContinue = false; } } while (dwBytesRead == sizeof(szBuffer)-1); } } while (fContinue); // Close the port again serial.Close(); return 0; }
int __cdecl _tmain (int argc, char** argv) { //------------ I N I C I O M A I N D E L P R O G R A M A D E L R O B O T-----------// //inicializaion de variables Aria::init(); ArArgumentParser parser(&argc, argv); parser.loadDefaultArguments(); ArSimpleConnector simpleConnector(&parser); ArRobot robot; ArSonarDevice sonar; ArAnalogGyro gyro(&robot); robot.addRangeDevice(&sonar); ActionGos go(500, 350); robot.addAction(&go, 48); ActionTurns turn(400, 110); robot.addAction(&turn, 49); ActionTurns turn2(400, 110); robot.addAction(&turn2, 49); // presionar tecla escape para salir del programa ArKeyHandler keyHandler; Aria::setKeyHandler(&keyHandler); robot.attachKeyHandler(&keyHandler); printf("Presionar ESC para salir\n"); // uso de sonares para evitar colisiones con las paredes u // obstaculos grandes, mayores a 8cm de alto ArActionLimiterForwards limiterAction("limitador velocidad cerca", 300, 600, 250); ArActionLimiterForwards limiterFarAction("limitador velocidad lejos", 300, 1100, 400); ArActionLimiterTableSensor tableLimiterAction; robot.addAction(&tableLimiterAction, 100); robot.addAction(&limiterAction, 95); robot.addAction(&limiterFarAction, 90); // Inicializon la funcion de goto ArActionGoto gotoPoseAction("goto"); robot.addAction(&gotoPoseAction, 50); // Finaliza el goto si es que no hace nada ArActionStop stopAction("stop"); robot.addAction(&stopAction, 40); // Parser del CLI if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); exit(1); } // Conexion del robot if (!simpleConnector.connectRobot(&robot)) { printf("Could not connect to robot... exiting\n"); Aria::exit(1); } robot.runAsync(true); // enciende motores, apaga sonidos robot.enableMotors(); robot.comInt(ArCommands::SOUNDTOG, 0); // Imprimo algunos datos del robot como posicion velocidad y bateria robot.lock(); ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV", robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage()); robot.unlock(); const int duration = 100000; //msec ArLog::log(ArLog::Normal, "Completados los puntos en %d segundos", duration/1000); // ============================ INICIO CONFIG COM =================================// CSerial serial; LONG lLastError = ERROR_SUCCESS; // Trata de abrir el com seleccionado lLastError = serial.Open(_T("COM3"),0,0,false); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Imposible abrir el COM")); // Inicia el puerto serial (9600,8N1) lLastError = serial.Setup(CSerial::EBaud9600,CSerial::EData8,CSerial::EParNone,CSerial::EStop1); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Imposible setear la config del COM")); // Register only for the receive event lLastError = serial.SetMask(CSerial::EEventBreak | CSerial::EEventCTS | CSerial::EEventDSR | CSerial::EEventError | CSerial::EEventRing | CSerial::EEventRLSD | CSerial::EEventRecv); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port event mask")); // Use 'non-blocking' reads, because we don't know how many bytes // will be received. This is normally the most convenient mode // (and also the default mode for reading data). lLastError = serial.SetupReadTimeouts(CSerial::EReadTimeoutNonblocking); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port read timeout.")); // ============================ FIN CONFIG COM =================================// bool first = true; int goalNum = 0; int color = 3; ArTime start; start.setToNow(); while (Aria::getRunning()) { robot.lock(); // inicia el primer punto if (first || gotoPoseAction.haveAchievedGoal()) { first = false; goalNum++; //cambia de 0 a 1 el contador printf("El contador esta en: --> %d <---\n",goalNum); if (goalNum > 20) goalNum = 1; //comienza la secuencia de puntos if (goalNum == 1) { gotoPoseAction.setGoal(ArPose(1150, 0)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); // Imprimo algunos datos del robot como posicion velocidad y bateria robot.lock(); ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV", robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage()); robot.unlock(); // Create the sound queue. ArSoundsQueue soundQueue; // Run the sound queue in a new thread soundQueue.runAsync(); std::vector<const char*> filenames; filenames.push_back("sound-r2a.wav"); soundQueue.play(filenames[0]); } else if (goalNum == 2) { printf("Gira 90 grados izquierda\n"); robot.unlock(); turn.myActivate = 1; turn.myDirection = 1; turn.activate(); ArUtil::sleep(1000); turn.deactivate(); turn.myActivate = 0; turn.myDirection = 0; robot.lock(); } else if (goalNum == 3) { gotoPoseAction.setGoal(ArPose(1150, 2670)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); // Imprimo algunos datos del robot como posicion velocidad y bateria robot.lock(); ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV", robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage()); robot.unlock(); } else if (goalNum == 4) { printf("Gira 90 grados izquierda\n"); robot.unlock(); turn2.myActivate = 1; turn2.myDirection = 1; turn2.activate(); ArUtil::sleep(1000); turn2.deactivate(); turn2.myActivate = 0; turn2.myDirection = 0; robot.lock(); } else if (goalNum == 5) { gotoPoseAction.setGoal(ArPose(650, 2670)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } else if (goalNum == 6) { printf("Gira 90 grados izquierda\n"); robot.unlock(); turn2.myActivate = 1; turn2.myDirection = 1; turn2.activate(); ArUtil::sleep(1000); turn2.deactivate(); turn2.myActivate = 0; turn2.myDirection = 0; robot.lock(); } else if (goalNum == 7) { gotoPoseAction.setGoal(ArPose(650, 0)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } else if (goalNum == 8) { gotoPoseAction.setGoal(ArPose(1800,1199)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } else if (goalNum == 9) { gotoPoseAction.setGoal(ArPose(2600, 1199)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } else if (goalNum == 10) { if (color == 1) { gotoPoseAction.setGoal(ArPose(2800, 850)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 2) { gotoPoseAction.setGoal(ArPose(3500, 1199)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 3) { gotoPoseAction.setGoal(ArPose(2800, 1550)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 11) { if (color == 1) { gotoPoseAction.setGoal(ArPose(2800, 613)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 2) { printf("Gira 180 grados derecha\n"); robot.unlock(); turn2.myActivate = 1; turn2.myDirection = 2; turn2.activate(); ArUtil::sleep(2000); turn2.deactivate(); turn2.myActivate = 0; turn2.myDirection = 0; robot.lock(); goalNum = 19; } if (color == 3) { gotoPoseAction.setGoal(ArPose(2800, 1785)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 12) { if (color == 1) { gotoPoseAction.setGoal(ArPose(3300, 413)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 3) { gotoPoseAction.setGoal(ArPose(3300, 1985)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 13) { if (color == 1) { gotoPoseAction.setGoal(ArPose(3500, 413)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 3) { gotoPoseAction.setGoal(ArPose(3500, 1985)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 14) { robot.unlock(); //Valor para el while bool fContinue = true; // <<<<<<------------- 1 Parte Secuencia: BAJA BRAZO ------------->>>>>> // lLastError = serial.Write("b"); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to send data")); //-------------------------E S C U C H A C O M ----------------------------// do { // Wait for an event lLastError = serial.WaitEvent(); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to wait for a COM-port event.")); // Save event const CSerial::EEvent eEvent = serial.GetEventType(); // Handle break event if (eEvent & CSerial::EEventBreak) { printf("\n### BREAK received ###\n"); } // Handle CTS event if (eEvent & CSerial::EEventCTS) { printf("\n### Clear to send %s ###\n", serial.GetCTS()?"on":"off"); } // Handle DSR event if (eEvent & CSerial::EEventDSR) { printf("\n### Data set ready %s ###\n", serial.GetDSR()?"on":"off"); } // Handle error event if (eEvent & CSerial::EEventError) { printf("\n### ERROR: "); switch (serial.GetError()) { case CSerial::EErrorBreak: printf("Break condition"); break; case CSerial::EErrorFrame: printf("Framing error"); break; case CSerial::EErrorIOE: printf("IO device error"); break; case CSerial::EErrorMode: printf("Unsupported mode"); break; case CSerial::EErrorOverrun: printf("Buffer overrun"); break; case CSerial::EErrorRxOver: printf("Input buffer overflow"); break; case CSerial::EErrorParity: printf("Input parity error"); break; case CSerial::EErrorTxFull: printf("Output buffer full"); break; default: printf("Unknown"); break; } printf(" ###\n"); } // Handle ring event if (eEvent & CSerial::EEventRing) { printf("\n### RING ###\n"); } // Handle RLSD/CD event if (eEvent & CSerial::EEventRLSD) { printf("\n### RLSD/CD %s ###\n", serial.GetRLSD()?"on":"off"); } // Handle data receive event if (eEvent & CSerial::EEventRecv) { // Read data, until there is nothing left DWORD dwBytesRead = 0; char szBuffer[101]; do { // Lee datos del Puerto COM lLastError = serial.Read(szBuffer,sizeof(szBuffer)-1,&dwBytesRead); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to read from COM-port.")); if (dwBytesRead > 0) { //Preseteo color int color = 0; // Finaliza el dato, asi que sea una string valida szBuffer[dwBytesRead] = '\0'; // Display the data printf("%s", szBuffer); // <<<<<<----------- 2 Parte Secuencia: CIERRA GRIPPER ----------->>>>>> // if (strchr(szBuffer,76)) { lLastError = serial.Write("c"); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to send data")); } // <<<<<<------------- 3 Parte Secuencia: SUBE BRAZO ------------->>>>>> // if (strchr(szBuffer,117)) { lLastError = serial.Write("s"); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to send data")); } // <<<<<<------------- 4 Parte Secuencia: COLOR ------------->>>>>> // if (strchr(szBuffer,72)) { lLastError = serial.Write("C"); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to send data")); } // <<<<<<---------- 5.1 Parte Secuencia: COLOR ROJO---------->>>>>> // if (strchr(szBuffer,82)) { color = 1; //salir del bucle fContinue = false; } // <<<<<<---------- 5.2 Parte Secuencia: COLOR AZUL ---------->>>>>> // if (strchr(szBuffer,66)) { color = 2; //salir del bucle fContinue = false; } // <<<<<<---------- 5.3 Parte Secuencia: COLOR VERDE ---------->>>>>> // if (strchr(szBuffer,71)) { color = 3; //salir del bucle fContinue = false; } } } while (dwBytesRead == sizeof(szBuffer)-1); } } while (fContinue); // Close the port again serial.Close(); robot.lock(); } else if (goalNum == 15) { printf("Gira 180 grados derecha\n"); robot.unlock(); turn2.myActivate = 1; turn2.myDirection = 2; turn2.activate(); ArUtil::sleep(2000); turn2.deactivate(); turn2.myActivate = 0; turn2.myDirection = 0; robot.lock(); } else if (goalNum == 16) { if (color == 1) { gotoPoseAction.setGoal(ArPose(3300, 413)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 3) { gotoPoseAction.setGoal(ArPose(3300, 1985)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 17) { if (color == 1) { gotoPoseAction.setGoal(ArPose(2800, 603)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 3) { gotoPoseAction.setGoal(ArPose(2800, 1795)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 18) { if (color == 1) { gotoPoseAction.setGoal(ArPose(2800, 860)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 3) { gotoPoseAction.setGoal(ArPose(2800, 1540)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 19) { gotoPoseAction.setGoal(ArPose(2600, 1199)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } else if (goalNum == 20) { gotoPoseAction.setGoal(ArPose(1800, 1199)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } if(start.mSecSince() >= duration) { ArLog::log(ArLog::Normal, "No puede llegar al punto, y la aplicacion saldra en %d", duration/1000); gotoPoseAction.cancelGoal(); robot.unlock(); ArUtil::sleep(3000); break; } robot.unlock(); ArUtil::sleep(10); } // Robot desconectado al terminal el sleep Aria::shutdown(); //------------ F I N M A I N D E L P R O G R A M A D E L R O B O T-----------// return 0; }
int main() { unsigned long cycles = 0; int i = 0; // ******************************************************************************************************************** // IMPORTANT: The serial port numbers here must correspond to those of the arduino on the ROV. The first number is the // COM # and the second number is the baud rate set on the arduino. SerialPort.Open(8, 115200); // ******************************************************************************************************************** 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(); 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); } if (SendData() == 1) { //SerialPort.ReadData(recieved[0], 3); } std::cout << Controller[5].value << Controller[4].value << Controller[1].value << std::endl; // This shows the value being passed to the arduino from the selected controller button (#3 = Left stick Y position) } return 0; }