EngineClient::EngineClient( ds::App& app, const ds::cfg::Settings& settings, ds::EngineData& ed, const ds::RootList& roots) : inherited(app, settings, ed, roots) , mLoadImageService(*this, mIpFunctions) , mRenderTextService(mRenderTextThread) , mSender(mSendConnection) , mReceiver(mReceiveConnection) , mBlobReader(mReceiver.getData(), *this) , mSessionId(0) , mConnectionRenewed(false) , mServerFrame(-1) , mState(nullptr) , mIoInfo(*this) { // NOTE: Must be EXACTLY the same items as in EngineServer, in same order, // so that the BLOB ids match. HEADER_BLOB = mBlobRegistry.add([this](BlobReader& r) {receiveHeader(r.mDataBuffer);}); COMMAND_BLOB = mBlobRegistry.add([this](BlobReader& r) {receiveCommand(r.mDataBuffer);}); DELETE_SPRITE_BLOB = mBlobRegistry.add([this](BlobReader& r) {receiveDeleteSprite(r.mDataBuffer);}); CLIENT_STATUS_BLOB = mBlobRegistry.add([this](BlobReader& r) {receiveClientStatus(r.mDataBuffer); }); CLIENT_INPUT_BLOB = mBlobRegistry.add([this](BlobReader& r) {receiveClientInput(r.mDataBuffer); }); mReceiver.setHeaderAndCommandIds(HEADER_BLOB, COMMAND_BLOB); try { if (settings.getBool("server:connect", 0, true)) { mSendConnection.initialize(true, settings.getText("server:ip"), ds::value_to_string(settings.getInt("server:listen_port"))); mReceiveConnection.initialize(false, settings.getText("server:ip"), ds::value_to_string(settings.getInt("server:send_port"))); } } catch(std::exception &e) { DS_LOG_ERROR_M("EngineClient::EngineClient() initializing UDP: " << e.what(), ds::ENGINE_LOG); } setState(mClientStartedState); }
Connection::Connection(QTcpSocket *socket) : QObject() , m_connection(socket) , m_command(0) { connect(m_connection, SIGNAL(readyRead()), this, SLOT(receiveCommand())); connect(m_connection, SIGNAL(disconnected()), this, SLOT(closedConnection())); }
void NMEAServer::deliver(Command_ptr command){ if((*command).getReceiver()=="server"){ receiveCommand(command); } else{ receive(command); } }
STATUS fineTuneX(int argc) { do { IplImage* frame = 0; // Load image from IP camera if(argc == 1) getimage(); frame = cvLoadImage("image.jpg"); if(!frame) { printf("\n No input frame found !!!"); break; } // Step #1: Cutter detection and processing // Detect cutter if (cutterProcessing(frame) != STATUS_OK) break; // Step #2: Fruit detection and processing fruitProcessing(frame); // Step #3: Find nearby fruits findNearbyFruits(); if(count == 0) { break; } printf("\nFine tunnig along X axis...."); printf("\n fruitX=%d cutterX=%d",fruitX,cutterX); if(fruitX - cutterX > 10){ sendCommand(COMMAND_RIGHT,1,0); receiveCommand(&cmd,NULL,NULL); if(cmd == COMMAND_FAIL) printf("\Error right send command"); } else if(cutterX - fruitX > 10){ sendCommand(COMMAND_LEFT,1,0); receiveCommand(&cmd,NULL,NULL); if(cmd == COMMAND_FAIL) printf("\Error right send command"); }
Pager::Pager(KWMModuleApplication *a, char *name) : QWidget(NULL, "kwmpager") { kwmmapp = a; a->setMainWidget(this); kwmmapp -> connectToKWM(); KWM::setSticky(winId(), true); KWM::setDecoration(winId(), KWM::tinyDecoration | KWM::noFocus); KWM::setWmCommand(winId(), name); KWM::setUnsavedDataHint(winId(), false); int count = KWM::numberOfDesktops(); desktop_font = new QFont(); Desktop *desk; desktops.resize(count); for (int i = 0; i < count; i++) { desk = new Desktop(a, i + 1, this); desktops[i] = desk; } activeDesktop = desktops.at(KWM::currentDesktop() - 1); activeDesktop->activate(true); connect(kwmmapp, SIGNAL(desktopChange(int)) , SLOT(changeDesktop(int))); connect(kwmmapp, SIGNAL(init()), SLOT(initDesktops())); connect(kwmmapp, SIGNAL( desktopNumberChange(int)), SLOT(changeNumber(int))); connect(kwmmapp, SIGNAL( windowAdd(Window)), SLOT(addWindow(Window))); connect(kwmmapp, SIGNAL( windowRemove(Window)), SLOT(removeWindow(Window))); connect(kwmmapp, SIGNAL( windowChange(Window)), SLOT(windowChange(Window))); connect(kwmmapp, SIGNAL( windowRaise(Window)), SLOT(raiseWindow(Window))); connect(kwmmapp, SIGNAL( windowLower(Window)), SLOT(lowerWindow(Window))); connect(kwmmapp, SIGNAL( windowActivate(Window)), SLOT(windowActivate(Window))); connect(kwmmapp, SIGNAL( commandReceived(QString)), SLOT(receiveCommand(QString))); moved = false; readSettings(); initDesktops(); show(); placeIt(); }
void listenToCommands() { initCOMPort(); initUSBRadio(); lastSpeedSentTimer.start(); HANDLE waitHandles[3] = { startSignal, osStatusDongle.hEvent }; stateGoal = goalOnRight; stateBallSeen = ballOnRight; while (true) { float floorX, floorY; int currentx, currenty; findNearestFloorBall(floorX, floorY, currentx, currenty); float angle = atanf(floorY / floorX) * 180.0 / PI; swprintf_s(buffer, L"NX %.2f \n NY %.2f\n ang %.2f", floorX, floorY, angle); SetWindowText(infoGUI, buffer); swprintf_s(buffer, L"greencount %d", fieldGreenPixelCountShare); SetWindowText(infoGUI3, buffer); if (hCOMDongle != INVALID_HANDLE_VALUE) { if (lastSpeedSentTimer.time() > 0.5) { //if we send the speeds too often we get communication timeouts setSpeedBoth(currentDrivingState); lastSpeedSentTimer.start(); } } switch (WaitForMultipleObjects(2, waitHandles, FALSE, 50)) { //case WAIT_OBJECT_0: // prints(L"radio event %X\n", dwCommEvent); // //receiveCommand(); // //WaitCommEvent(hCOMRadio, &dwCommEvent, &osStatus); // break; case WAIT_OBJECT_0: prints(L"Start signal arrived.\n"); SetWindowText(stateStatusGUI, L"started"); dribblerON(); play(); dribblerOFF(); //discharge(); break; case WAIT_OBJECT_0 + 1: //info from the main board controller //prints(L"main board COM event %X\n", dwCommEventDongle); handleMainBoardCommunication(); WaitCommEvent(hCOMDongle, &dwCommEventDongle, &osStatusDongle); //listen to new events, ie beginning character break; } receiveCommand(); } }
void Connection::receiveCommand() { QByteArray arr = m_connection->readAll(); debugOutput(1, QString::fromLatin1("Command received: ") + (arr)); QList<CommandInfo> commands = availableCommands(); for(QList<CommandInfo>::iterator it = commands.begin(); it != commands.end(); ++it) { if (it->commandName == QString::fromLatin1(arr)) { debugOutput(1, "Found command in list"); disconnect(m_connection, SIGNAL(readyRead()), this, SLOT(receiveCommand())); AbstractCommand* command = (*it).commandFunc(); command->setSocket(m_connection); m_command = command; return; } } debugOutput(2, QString::fromLatin1("Unknown command received: ") + (arr)); }
int llclose(int fd){ alarm(0); if(!sendByte(fd, A_SEND, C_DISC)) return -1; //puts("sent disc"); Command command = receiveCommand(fd); //puts("eu recebi um comandito\n"); if(command.command != DISC){ //puts("didnt receive a disc"); return -1; } //puts("received a disc"); if(!sendByte(fd, A_RECEIVE, C_UA)) return -1; //puts("confirmed"); return 1; }
void GUIServer::run() { if (receiveCommand()) { m_UdpServer.beginPacket(m_UdpServer.remoteIP(), m_UdpServer.remotePort()); m_UdpServer.write(m_Buffer[1]); m_UdpServer.write(m_Buffer[0]); Serial.print(F("Request ID: ")); Serial.println(m_Buffer[0]); Serial.print(F("Method ID: ")); Serial.println(m_Buffer[1]); Serial.print(F("value: ")); Serial.println(m_Buffer[2]); switch (m_Buffer[1]) { case GETVERSION: m_UdpServer.write((uint8_t) 0); m_UdpServer.write(1); break; case GETALLSENSORS: getAllSensors(); break; case GETSENSORDATA: getSensorData(m_Buffer[2]); break; case SETSENSORCONFIG: switch (m_Buffer[3]) { case 1: break; case 2: setSensorConfig(m_Buffer[2], m_Buffer[3], (char*) &m_Buffer[4]); break; case 3: setSensorConfig(m_Buffer[2], m_Buffer[3], (char*) &m_Buffer[4]); break; case 4: setSensorConfig(m_Buffer[2], m_Buffer[3], (uint8_t) m_Buffer[4]); break; case 5: setSensorConfig(m_Buffer[2], m_Buffer[3], (uint8_t) m_Buffer[4]); break; } break; case GETALLACTUATORS: getAllActuators(); break; case GETACTUATORDATA: getActuatorData(m_Buffer[2]); break; case SETACTUATORDATA: switch (m_Buffer[3]) { case 1: case 3: case 4: case 5: case 6: setActuatorData(m_Buffer[2], m_Buffer[3], (uint8_t) m_Buffer[4]); break; case 2: setActuatorData(m_Buffer[2], m_Buffer[3], (char*) m_Buffer[4]); } break; case SETACTUATORCONFIG: switch (m_Buffer[3]) { case 1: break; case 2: setActuatorConfig(m_Buffer[2], m_Buffer[3], (char*) &m_Buffer[4]); break; case 3: break; case 4: break; case 5: break; } break; case GETCLOCKTIMERS: getClockTimers(m_Buffer[2]); break; default: break; } m_UdpServer.endPacket(); } }
int llwrite(int fd, unsigned char* buffer, int length){ //printf("entrei no llwrite\n"); char *bufferStuffed; char header[] = { FLAG, 0x03, I(ll.sequenceNumber), header[1]^header[2] }; char dataBCC = generateBCC((char*)buffer, length); char* toStuff = malloc(length+1); memcpy(toStuff, buffer, length); toStuff[length] = dataBCC; int n= byteStuffing(toStuff, length+1, &bufferStuffed); //int k; //puts(""); //for (k=0;k<n;k++){ //printf("\n%d\n", bufferStuffed[k]); //} // puts(""); char* message = (char*) malloc(n+6); memcpy(message, header, 4); memcpy(message+4, bufferStuffed, n); message[n+4] = FLAG; int wrote = 0; while(!wrote) wrote = write(fd, message, n+5); stats.dataFramesTransmitted++; //puts("message sent\n"); free(message); free(bufferStuffed); alarm(ll.timeOut); Command command = receiveCommand(fd); //was asked for new frame if (command.command == RR(!ll.sequenceNumber)){ ll.sequenceNumber = (!ll.sequenceNumber); retries = 0; //puts("fui confirmado"); alarm(0); return length; } //frame was rejected, resend if (command.command == REJ(ll.sequenceNumber) || (command.command == NONE && ll.numTransmissions > retries)){ if (command.command == NONE){ stats.timeouts++; //printf("\nNumber of retries: %d\n", retries); alarm(0); } //puts("byte was rejected,resending or no response\n"); stats.rejs++; return llwrite(fd, buffer, length); } else{ //puts("eu esperei mas o reader nao me quis responder :("); return -1; } return length; }
int llopen(const char* port, int mode){ srand(time(NULL)); stats.dataFramesTransmitted = 0; stats.timeouts=0; stats.rejs=0; installAlarm(); int fd; char fileName[15]; fd = open(port, O_RDWR|O_NOCTTY); if(fd <0){ perror(fileName); exit(-1); } struct termios oldTio, newTio; if(tcgetattr(fd, &oldTio) == -1){ perror("tcgetattr error"); exit(-1); } ll.oldtio = oldTio; memset(&newTio, 0, sizeof(newTio)); //possible error here. if error use bzero instead newTio.c_cflag = ll.baudRate | CS8 | CLOCAL | CREAD; newTio.c_iflag = IGNPAR; newTio.c_oflag = 0; /* set input mode (non-canonical, no echo,...) */ if(mode == RECEIVE){ newTio.c_lflag = 0; newTio.c_cc[VTIME] = 0; /* inter-character timer unused */ newTio.c_cc[VMIN] = 1; /* blocking read until 5 chars received */ tcflush(fd, TCIOFLUSH); if ( tcsetattr(fd,TCSANOW,&newTio) == -1) { perror("tcsetattr"); exit(-1); } //printf("Ready to read\n"); Command command = receiveCommand(fd); if(command.command == SET){ //puts("llopen_receive: received SET message\n"); while(!sendByte(fd, UA, 0x03)){} return fd; } return E_GENERIC; } else if(mode == SEND){ newTio.c_lflag = 0; newTio.c_cc[VTIME] = 10; /* inter-character timer unused */ newTio.c_cc[VMIN] = 0; /* blocking read until 5 chars received */ tcflush(fd, TCIOFLUSH); if ( tcsetattr(fd,TCSANOW,&newTio) == -1) { perror("tcsetattr"); exit(-1); } send: ; resend = 0; unsigned char SET[5] = {FLAG, A_SEND, C_SET, SET[1]^SET[2], FLAG}; if(write(fd, SET, 5) != 5) return -1; //printf("escrevi\n"); alarm(ll.timeOut); Command command = receiveCommand(fd); //printf("0x%02x command\n", command.command); if(command.command == NONE){ if (ll.numTransmissions < retries){ puts("llopen_writer timeouts exceeded"); return -1; } else{ //puts("llopen_writer timeout"); goto send; } } if (command.command == UA){ alarm(0); return fd; } return -1; } return 0; }
int llread(int fd, unsigned char **buffer){ //printf("preparing to read frame\n"); Command command = receiveCommand(fd); //puts("llread:received command"); int repeated; if (command.command == I(ll.sequenceNumber)) repeated = 0; else if (command.command == I(!ll.sequenceNumber)) repeated = 1; else if (command.command == DISC){ //printf("llread: disconnecting\n"); while(!sendByte(fd, 0x01, DISC)){} //puts("llread: disc confirmation sent\n"); command = receiveCommand(fd); if (command.command != UA){ //puts("llread: didnt receive UA after disc confirmation\n"); return E_GENERIC; } else{ //puts("llread: connection successfully closed\n"); return E_CLOSED; } } if (command.command == I(0) || command.command == I(1)){ //puts("llread: received a data frame\n"); //if we never saw this frame before, consider it stats.dataFramesTransmitted++; if(!repeated){ //puts("llread: new data frame\n"); int length = byteDeStuffing(&(command.data), command.size); //puts("llread: destuffing succeeded\n"); int bccOK = verifyBCC(command.data, length, command.data[length-1]); if(getRand() < RANDOM_REJ_CHANCE){ bccOK = 0; // puts("\n randomly rejecting good packet"); } //Reject frames with wrong BCC if(!bccOK){ //puts("llread: frame was damaged, rejecting and rereading\n"); stats.rejs++; while(!sendByte(fd, 0x03, REJ(ll.sequenceNumber) )){} return llread(fd, buffer); } //accept the frame and confirm it //puts("llread: frame bcc ok, accepting\n"); *buffer = (unsigned char*) malloc(length); memcpy(*buffer, command.data, length); while(!sendByte(fd,0x03, RR(!ll.sequenceNumber))){} //puts("llread: receiver ready sent, message confirmed\n"); ll.sequenceNumber = !ll.sequenceNumber; free(command.data); //printf("%d length\n", length); return length; } //this frame is repeated. Confirm it and ask for the new frame else{ //puts("llread: message repeated"); while(!sendByte(fd, 0x03, RR(ll.sequenceNumber))){} return E_GENERIC; } } else{ //printf("received unexpected command 0x%02x\n",command.command); return E_GENERIC; } return 0; }
void play() { HANDLE waitHandles[5] = { stopSignal, osStatusDongle.hEvent, calibratingSignal, newImageAnalyzed }; int currentx = 320, currenty = 0, FPS2Count = 0; int timeOut = 30; float movingTime; //timeout after sending each command objectCollection goals, goals2; float nearestBallFloorX = 0, nearestBallFloorY = 0, initialBallDistance = 0; ignoreX = 0, ignoreY = 0; Timer ignoreTimer, ballSearchRotationSpeedTimer, chargingTimer, wanderingTimer, frameTimer; float lastBallFoundTime = 0, lastGoalFoundTime = 0, wanderingTime = 0; bool rotateFast = true; FPS2Timer.start(); FPS2Count = 0; frameTimer.start(); bool turnFast = true; state = lookForBall; SetWindowText(goalGuessState, L"Goal on right"); isBallInDribbler = false; prints(L"Started, driving to ball\n"); SetWindowText(stateStatusGUI, L"Driving to ball"); charged = false, ignoreBall = false; charge(); chargingTimer.start(); sendString(hCOMDongle, "9:bl\n"); lastBallFoundTimer.start(); lastGoalFoundTimer.start(); movingTimer.start(); while (true) { //display test stuff float floorX, floorY; int currentx, currenty; findNearestFloorBall(floorX, floorY, currentx, currenty); float angle = atanf(floorY / floorX) * 180.0 / PI; swprintf_s(buffer, L"NX %.2f \n NY %.2f\n ang %.2f", floorX, floorY, angle); SetWindowText(infoGUI, buffer); //end display test stuff switch (WaitForMultipleObjects(4, waitHandles, FALSE, timeOut)) { //case WAIT_OBJECT_0: //start of a new command from the radio detected // //ResetEvent(osStatus.hEvent); // prints(L"radio event %X\n", dwCommEvent); // //receiveCommand(); //receive and interpret the command // //WaitCommEvent(hCOMRadio, &dwCommEvent, &osStatus); //listen to new events, ie beginning character // continue; case WAIT_OBJECT_0: //stop signal prints(L"Stop signal arrived.\n"); SetWindowText(stateStatusGUI, L"stopped"); currentDrivingState.angle = 0, currentDrivingState.speed = 0, currentDrivingState.vx = 0, currentDrivingState.vy = 0, currentDrivingState.angularVelocity = 0; setSpeedAngle(currentDrivingState); ResetEvent(startSignal); return; case WAIT_OBJECT_0 + 1: //info from the main board controller //prints(L"main board COM event %X\n", dwCommEvent); handleMainBoardCommunication(); WaitCommEvent(hCOMDongle, &dwCommEventDongle, &osStatusDongle); //listen to new events, ie beginning character continue; case WAIT_OBJECT_0 + 2: //calibrating signal SetWindowText(stateStatusGUI, L"calibrating"); rotateAroundCenter(0); //WaitForSingleObject(calibratingEndSignal, INFINITE); continue; case WAIT_OBJECT_0 + 3: //new image analyzed //update FPS rate after every 5 images ++FPS2Count; if (FPS2Count % 5 == 0) { float FPS = 5.0 / FPS2Timer.time(); swprintf_s(buffer, L"FPS2: %.2f", FPS); SetWindowText(statusFPS2, buffer); FPS2Timer.start(); } if (FPS2Count % 10 == 0) { receiveCommand(); } break; default: continue; } if (attackBlue) { //set the side goals = goalsBlueShare; goals2 = goalsYellowShare; } else { goals = goalsYellowShare; goals2 = goalsBlueShare; } if (ignoreBall) { //ignore the ball just kicked for 1s if (ignoreTimer.time() > 1.0) { ignoreBall = false; ignoreX = 0, ignoreY = 0; } else { float ignoreXNew = ignoreX, ignoreYNew = ignoreY; findNearestFloorObjectToOldObject(ignoreXNew, ignoreYNew, ballsShare); //if we cant see the ball behind the dribbler, don't start ignoring a ball too far away ignoreX = ignoreXNew; ignoreY = ignoreYNew; swprintf_s(buffer, L"IgX %.2f IgY %.2f\n NX %.2f NY %.2f", ignoreX, ignoreY, ignoreXNew, ignoreYNew); SetWindowText(infoGUI2, buffer); } } else { SetWindowText(infoGUI2, L"Ignore OFF"); } //keep track of the likely direction to turn to to find the goal if ((goalsBlueShare.count > 0 || goalsYellowShare.count > 0) && !(goalsBlueShare.count == 1 && goalsYellowShare.count == 1)) { int goalX, goalY; State stateGoalNew; if (goals.count > 0) { findLargestObject(goalX, goalY, goals); stateGoalNew = goalX > 320 ? goalOnRight : goalOnLeft; } else if (goals2.count > 0) { findLargestObject(goalX, goalY, goals2); stateGoalNew = goalX > 320 ? goalOnLeft : goalOnRight; } if (stateGoalNew != stateGoal && state != lookForGoal) { stateGoal = stateGoalNew; if (stateGoal == goalOnRight) { SetWindowText(goalGuessState, L"Goal on the right"); } else { SetWindowText(goalGuessState, L"Goal on the left"); } } } if (ballsOnFieldInSight() > 0) { //keep track of the likely direction to turn to to find the ball for (int i = 0; i < ballsShare.count; ++i) { if (!ballsShare.data[i].isObjectAcrossLine) { if (ballsShare.data[i].x > 320) { stateBallSeen = ballOnRight; } else { stateBallSeen = ballOnLeft; } } } } float frameTime = frameTimer.time(); frameTimer.start(); if (ballsOnFieldInSight() == 0 && state == lookForBall && !isBallInDribbler) { lastBallFoundTime += frameTime; } else if (ballsOnFieldInSight > 0) { lastBallFoundTime = 0; } if (goals.count == 0 && state == lookForGoal) { lastGoalFoundTime += frameTime; } else if (goals.count > 0) { lastGoalFoundTime = 0; } if (lastBallFoundTime > 3.0 || lastGoalFoundTime > 3.0) { prints(L"Wandering for ball\n"); state = wanderForBall; movingTimer.start(); wanderingTimer.start(); wanderingTime = 0; lastBallFoundTime = 0; lastGoalFoundTime = 0; } if (fieldGreenPixelCountShare < FIELDGREENCOUNTTHRESHOLD && state != rotate90 && state != wanderForBall) { state = rotate90; movingTimer.start(); } if (state == lookForBall) { if (isBallInDribbler) { prints(L"Looking for goal\n"); state = lookForGoal; movingTimer.start(); } else { //prints(L"Looking for ball\n"); SetWindowText(stateStatusGUI, L"Looking for ball"); nearestBallFloorX = 0; int sign = stateBallSeen == ballOnRight ? -1 : 1; if (ballsOnFieldInSight() == 0) { if (fmodf(movingTimer.time(), 0.3) < 0.2) { rotateAroundCenter(sign * 200); } else { rotateAroundCenter(sign * 40); } } else { findNearestFloorBall(nearestBallFloorX, nearestBallFloorY, currentx, currenty); initialBallDistance = pow(nearestBallFloorX*nearestBallFloorX + nearestBallFloorY*nearestBallFloorY, 0.5); //prints(L"found nearest ball X: %.2f, Y: %.2f, currentx: %d, currenty: %d\n", nearestBallFloorX, nearestBallFloorY, currentx, currenty); prints(L"Driving to ball\n"); state = driveToBall; rotateAroundCenter(0); movingTimer.start(); } } } if (state == driveToBall) { if (isBallInDribbler) { rotateAroundCenter(0); Sleep(0); prints(L"Looking for goal\n"); state = lookForGoal; movingTimer.start(); } else { //prints(L"Driving to ball\n"); SetWindowText(stateStatusGUI, L"driving to ball"); float floorX, floorY; //findNearestObject(currentx, currenty, ballsShare); //prints(L"Floor coordinates x: %.2f, y: %.2f, currentx: %d, currenty: %d\n", floorX, floorY, currentx, currenty); if (findNearestFloorBall(floorX, floorY, currentx, currenty) == 0) { //prints(L"Drive state, but ballcount zero, starting to look for ball.\n"); state = lookForBall; movingTimer.start(); prints(L"Looking for ball\n"); continue; } driveToFloorXYPID(floorX, floorY, initialBallDistance); } } if (state == lookForGoal) { //prints(L"Looking for goal\n"); SetWindowText(stateStatusGUI, L"Looking for goal\n"); if (isBallInDribbler) { int x = 0, y = 0; float floorX, floorY; findLargestObject(x, y, goals); convertToFloorCoordinates(x, y, floorX, floorY, GOALMIDHEIGHT); //prints(L"Goal x: %d\n", x); if (!(goalsBlueShare.count == 1 && goalsYellowShare.count == 1) && (y > 0 && floorX > 0 && fabs(floorY) < 18.0 / 100.0 || abs(x-320) <= 35)) { state = kickBall; rotateAroundFront(0); prints(L"Kicking\n"); } else { int sign; if (stateGoal == goalOnRight) { sign = -1; } else { sign = 1; } if (goals.count == 0) { //no goals in sight, turn faster rotateAroundFront(sign * 200); } else{ //around 120 degs/s is max speed so that the goal doesn't go out, less than 30 degs/s is pointless float angle = atanf(tanf(angleOfView)*(x - 320.0) / 320.0); //roughly, in degrees float speedMultiplier = 1 - expf(-fabs(pow((x - 320.0) / 320.0, 4))); speedMultiplier = speedMultiplier > 1 ? 1 : speedMultiplier; float turningSpeed = 50 + (160 - 50)*speedMultiplier; rotateAroundFront(sign * turningSpeed); } } } else { state = lookForBall; movingTimer.start(); prints(L"Looking for ball\n"); } } if (state == kickBall) { SetWindowText(stateStatusGUI, L"Kicking"); if (!isBallInDribbler) { state = lookForBall; movingTimer.start(); } if (!charged) {//not charged, wait more if (chargingTimer.time() > 3.5) { //by this time we definitely should have gotten a charge command, charge again prints(L"No charge info received in time\n"); chargingTimer.start(); charge(); } continue; } kick(); Sleep(20); charged = false; //isBallInDribbler = false; //sendString(hCOMDongle, "9:bl\n"); ignoreBall = true; ignoreX = 25.0 / 100.0, ignoreY = 0; ignoreTimer.start(); state = lookForBall; movingTimer.start(); prints(L"Looking for ball\n"); //Sleep(80); charge(); chargingTimer.start(); } if (state == rotate90) { SetWindowText(stateStatusGUI, L"Rotating 90"); if (movingTimer.time() < 0.5) { rotateAroundCenter(180); continue; } else { prints(L"Looking for ball\n"); stateBallSeen = ballOnLeft; stateGoal = goalOnLeft; state = lookForBall; movingTimer.start(); } } //wander around, try to drive towards a goal far enough away, if can't find one, drive to a direction where there are no goals, lines and there is enough green if (state == wanderForBall) { wanderingTime += frameTime; SetWindowText(stateStatusGUI, L"Wandering"); if (wanderingTime > 4.0) { //if we didn't find a far away goal in 4 seconds int goalBlueX, goalBlueY; int goalYellowX, goalYellowY; findLargestObject(goalBlueX, goalBlueY, goalsBlueShare); findLargestObject(goalYellowX, goalYellowY, goalsYellowShare); if (goalsBlueShare.count != 0 && goalBlueX >= 150 && goalBlueX <= 500 || goalsYellowShare.count != 0 && goalYellowX >= 150 && goalYellowX <= 500 || isLineStraightAhead || fieldGreenPixelCountShare < FIELDGREENCOUNTTHRESHOLD) { rotateAroundCenter(100); wanderingTimer.start(); } else { if (wanderingTimer.time() > 1.0) { movingTimer.start(); state = lookForBall; prints(L"Looking for ball\n"); continue; } driveForward(0.5); } } else { if (goalsBlueShare.count == 0 && goalsYellowShare.count == 0 || isLineStraightAhead || fieldGreenPixelCountShare < FIELDGREENCOUNTTHRESHOLD) { rotateAroundCenter(120); wanderingTimer.start(); } else { if (wanderingTimer.time() > 1.0) { movingTimer.start(); state = lookForBall; prints(L"Looking for ball\n"); continue; } int goalX = 0, goalY = 0; findLargestObject(goalX, goalY, goalsBlueShare); if (goalY > 450) { driveForward(0.5); continue; } else { goalX = 0, goalY = 0; findLargestObject(goalX, goalY, goalsYellowShare); if (goalY > 450) { driveForward(0.5); } else { rotateAroundCenter(100); wanderingTimer.start(); } } } } } } }
void NMEAServer::run(){ shouldRun=true; while(shouldRun){ std::list<Message_ptr> msgs_cpy; { boost::mutex::scoped_lock lock(msgsMutex); if(msgs.empty()){ msgsCond.wait(lock); } msgs_cpy = std::list<Message_ptr>(msgs); } while(!msgs_cpy.empty()){ std::list<Endpoint_ptr> online_cpy; { boost::mutex::scoped_lock lock(onlineMutex); online_cpy = std::list<Endpoint_ptr>(online); } if(NMEAmsg_ptr tmp_msg = boost::dynamic_pointer_cast<NMEAmsg>(msgs_cpy.front())){ for (std::list<Endpoint_ptr>::const_iterator endpoint = online_cpy.begin(), end = online_cpy.end(); endpoint != end; ++endpoint) { NMEAEndpoint_ptr nmeaEnd = boost::dynamic_pointer_cast<NMEAEndpoint>(*endpoint); if(nmeaEnd){ nmeaEnd->deliver(tmp_msg); } } } else if(Command_ptr tmp_cmd = boost::dynamic_pointer_cast<Command>(msgs_cpy.front())){ if(tmp_cmd->getReceiver()=="*"){ receiveCommand(tmp_cmd); } if(tmp_cmd->getReceiver()=="server"){ receiveCommand(tmp_cmd); } else{ bool found=false; for (std::list<Endpoint_ptr>::const_iterator endpoint = online_cpy.begin(), end = online_cpy.end(); endpoint != end; ++endpoint) { std::string receiver = tmp_cmd->getReceiver(); boost::replace_all(receiver, "*", "(.*)"); boost::regex reg("^"+receiver+"$"); boost::cmatch matches; CommandEndpoint_ptr commandEnd = boost::dynamic_pointer_cast<CommandEndpoint>(*endpoint); if(commandEnd){ if(boost::regex_search(commandEnd->getId().c_str(), matches, reg)){ if(tmp_cmd->getSender()!=commandEnd){ commandEnd->receive(tmp_cmd); found=true; } } } } if(!found){ tmp_cmd->answer(Answer::UNKNOWN_RECEIVER, "The receiver "+tmp_cmd->getReceiver()+" was not found \n", this->shared_from_this()); } } } else if(Answer_ptr tmp_answer = boost::dynamic_pointer_cast<Answer>(msgs_cpy.front())){ if(tmp_answer->getReceiver()=="*"){ deliver(tmp_answer); } if(tmp_answer->getReceiver()=="server"){ deliver(tmp_answer); } else{ bool found=false; for (std::list<Endpoint_ptr>::const_iterator endpoint = online_cpy.begin(), end = online_cpy.end(); endpoint != end; ++endpoint) { std::string receiver = tmp_answer->getReceiver(); boost::replace_all(receiver, "*", "(.*)"); boost::regex reg("^"+receiver+"$"); boost::cmatch matches; CommandEndpoint_ptr commandEnd = boost::dynamic_pointer_cast<CommandEndpoint>(*endpoint); if(commandEnd){ if(boost::regex_search(commandEnd->getId().c_str(), matches, reg)){ if(tmp_answer->getSender()!=commandEnd){ commandEnd->deliver(tmp_answer); found=true; } } } } } } { boost::mutex::scoped_lock lock(msgsMutex); msgs.remove(msgs_cpy.front()); } msgs_cpy.pop_front(); } } }
void checkCommands(char *buffer){ //maybe for changing physical layer probabilities //printf("Getting data from link layer\n"); int size = dataLinkRecv(buffer); receiveCommand(buffer, size); }