bool Buffer::find(const QString& findText, int flags, bool forward, bool wrap, bool *searchWrapped) { if (findText.isEmpty()) { return false; } if (searchWrapped) { *searchWrapped = false; } // Perform the search setSearchFlags(flags); setTargetStart(forward ? currentPos() : currentPos() - 1); setTargetEnd(forward ? length() : 0); QByteArray findArray = findText.toUtf8(); int findPos = searchInTarget(findArray.length(), findArray); // If the search should wrap, perform the search again. if (findPos == -1 && wrap) { setTargetStart(forward ? 0 : length()); setTargetEnd(forward ? currentPos() : currentPos() - 1); findPos = searchInTarget(findArray.length(), findArray); if (searchWrapped) { *searchWrapped = true; } } if (findPos != -1) { setSel(targetStart(), targetEnd()); scrollRange(targetStart(), targetEnd()); } return findPos != -1; }
void OneRobotOffensePlay::Execute() { for(size_t i = 0; i < 6; i++) { // State machine to kick towards goal if(assignments[i] == 1) { // The offensive robot fprintf(stderr, "OffensiveRobot state %d\n", states[i]); if(states[i] == 0) { // Calculating position to kick state // needs to be tested int robotId = i; if (!_isYellowTeam) { i +=6; } // Sets the location of the goals vector<double> targetStart(2, 0); vector<double> targetEnd(2, 0); targetStart[1] = 0.35 * 1000; targetEnd[1] = -0.35 * 1000; // Only x coords of goals change if (_isYellowTeam) { targetStart[0] = -3 * 1000; targetEnd[0] = -3 * 1000; } else { targetStart[0] = 3 * 1000; targetEnd[0] = 3 * 1000; } MatrixXd ownTeamPos(6, 2); MatrixXd otherTeamPos(6, 2); // TODO botstates std::vector<Robot*>* otherTeamBots; if (_isYellowTeam) { otherTeamBots = SoccerFieldInfo::Instance()->blue_bots; } else { otherTeamBots = SoccerFieldInfo::Instance()->yellow_bots; } // Fills in matrix from bot states for (int k = 0; k<6; k++){ ownTeamPos(k, 0) = _robots->at(k)->CurrentState()[0]; ownTeamPos(k, 1) = _robots->at(k)->CurrentState()[1]; otherTeamPos(k, 0) = otherTeamBots->at(k)->_currentPosition[0]; otherTeamPos(k, 1) = otherTeamBots->at(k)->_currentPosition[1]; } // TODO Could be an eigen vector? std::vector<double> ballPos(3, 0); ballPos[0] = SoccerFieldInfo::Instance()->ball[0]; ballPos[1] = SoccerFieldInfo::Instance()->ball[1]; ballPos[2] = SoccerFieldInfo::Instance()->ball[2]; Eigen::MatrixXd shootingPosMatrix= Evaluation::openAngleFinder(ballPos, robotId, targetStart, targetEnd, ownTeamPos, otherTeamPos); if(shootingPosMatrix.rows() == 0) { fprintf(stderr, "No valid shot\n"); continue; } // TODO Why? for(int i = 0; i < shootingPosMatrix.rows(); ++i) { Eigen::VectorXd shootingPos = shootingPosMatrix.row(i); fprintf(stderr, "Shooting Pos %d: %f, %f\n", i, shootingPos[0], shootingPos[1]); } Eigen::VectorXd shootingPos = shootingPosMatrix.row(0); fprintf(stderr, "Calculating shooting angle!\n"); Eigen::Vector3d goal = _robots->at(i)->CurrentState(); fprintf(stderr, "Robot State: %f %f %f\n", goal[0], goal[1], goal[2]); // double changeInAngle = (goal[2] - _isYellowTeam->at(i)->CurrentState()[2]) / 2; // goal[0] = 2 * 89 * sin(changeInAngle) * cos(changeInAngle); // goal[1] = 2 * 89 * sin(changeInAngle) * sin(changeInAngle); // double desiredShotAngle = (shootingPos[0] + shootingPos[1]) / 2; Eigen::Vector3d goalStateToKickFrom = Evaluation::GetGoalPositionToBall(desiredShotAngle); wayPointsToGoalState.push_back(goalStateToKickFrom); fprintf(stderr, "Final goal: %f, %f, %f\n", goalStateToKickFrom[0], goalStateToKickFrom[1], goalStateToKickFrom[2] * 180 / M_PI); Eigen::Vector2d robotAxis(cos(_robots->at(i)->CurrentState()[2]), sin(_robots->at(i)->CurrentState()[2])); Eigen::Vector2d goalStateAxis(cos(goalStateToKickFrom[2]), sin(goalStateToKickFrom[2])); if(robotAxis.dot(goalStateAxis) > 0) { //Go Directly after backing up fprintf(stderr, "robotAxis%f %f %f\n", robotAxis[0], robotAxis[1], (double)robotAxis.dot(goalStateAxis)); } else { fprintf(stderr, "robotAxis angle>90%f %f %f\n", robotAxis[0], robotAxis[1], (double)robotAxis.dot(goalStateAxis)); //Decide how to go? //Rotate robot axis by 90 degrees double temp = robotAxis[0]; //-sin(theta) robotAxis[0] = -robotAxis[1];//cos(theta) robotAxis[1] = temp; Eigen::Vector3d wayPoint; if(robotAxis.dot(goalStateAxis) >= 0) { wayPoint = Evaluation::GetGoalPositionToBall(_robots->at(i)->CurrentState()[2] + M_PI/2); } else { wayPoint = Evaluation::GetGoalPositionToBall(_robots->at(i)->CurrentState()[2] - M_PI/2); } wayPoint[0] -= 400 * cos(wayPoint[2]); wayPoint[1] -= 400 * sin(wayPoint[2]); fprintf(stderr, "Waypoint: %f, %f, %f\n", wayPoint[0], wayPoint[1], wayPoint[2] * 180 / M_PI); wayPointsToGoalState.push_back(wayPoint); } _robots->at(i)->setSpinner(false); //Go back along current axis goal = _robots->at(i)->CurrentState(); goal[0] -= 90 * cos(goal[2]); goal[1] -= 90 * sin(goal[2]); _robots->at(i)->goToLocation(1, goal); states[i] = 1; } else if (states[i] == 1) { // Going to location state if (_robots->at(i)->execute() == 1) { if(wayPointsToGoalState.size() > 0) { Eigen::Vector3d intermediateGoal = wayPointsToGoalState.back(); fprintf(stderr, "Going to waypoint: %f, %f, %f\n", intermediateGoal[0], intermediateGoal[1], intermediateGoal[2]); _robots->at(i)->goToLocation(1, intermediateGoal); wayPointsToGoalState.pop_back(); } else { states[i] = 2; } } } else if (states[i] == 2) { _robots->at(i)->setSpinner(true); if (_robots->at(i)->execute() == 1) { fprintf(stderr, "OneRobotOffensePlay::Execute kicking %f, %f, %f\n", _robots->at(i)->CurrentState()[0], _robots->at(i)->CurrentState()[1], _robots->at(i)->CurrentState()[2]); Eigen::Vector2d r = Eigen::Vector2d(_robots->at(i)->CurrentState()[0], _robots->at(i)->CurrentState()[1]); Eigen::Vector2d loc = Eigen::Vector2d(SoccerFieldInfo::Instance()->ball[0], SoccerFieldInfo::Instance()->ball[1]); double distance = (r - loc).norm(); fprintf(stderr, "Distance: %f\n", distance); if(distance > 120) { states[i]++; fprintf(stderr, "Last State\n"); } else { // Call Kick fprintf(stderr, "Kicking\n"); _robots->at(i)->setKickSpeed(2, 0); _robots->at(i)->sendVelocityCommands(); } } } else if (states[i] == 3) { continue; //_complete = true; // _complete should never be set true by a state machine } else { fprintf(stderr, "OneRobotOffensePlay::Execute Invalid state %d", states[i]); } } } }