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]);
}
}
}
}