gr_Robot_Command SkillSet::turnToPoint(const SParam ¶m, const BeliefState &state, int botID)
{
Vector2D<int> point(param.TurnToPointP.x, param.TurnToPointP.y);
Vector2D<int> botPos(state.homePos[botID].x, state.homePos[botID].y);
Vector2D<int> ballPos(state.ballPos.x, state.ballPos.y);
float finalSlope = Vector2D<int>::angle(point, botPos);
float turnAngleLeft = normalizeAngle(finalSlope - state.homePos[botID].theta); // Angle left to turn
float omega = turnAngleLeft * param.TurnToPointP.max_omega / (2 * PI); // Speedup turn
if(omega < MIN_BOT_OMEGA && omega > -MIN_BOT_OMEGA)
{
if(omega < 0) omega = -MIN_BOT_OMEGA;
else omega = MIN_BOT_OMEGA;
}
float v_x = omega*BOT_BALL_THRESH*1.5;
// comm.addCircle(state->homePos[botID].x, state->homePos[botID].y, 50);
float dist = Vector2D<int>::dist(ballPos, botPos);
if(dist < DRIBBLER_BALL_THRESH*1.2)
{
return getRobotCommandMessage(botID, v_x, 0, omega, 0, true);
}
else
{
return getRobotCommandMessage(botID, 0, 0, omega, 0, false);
}
}
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]);
}
}
}
}
void main() {
char columm = 0, display = 0, i, n, index, count = 0, shift = 1;
int posX2 = 100, posY2 = 50;
char data[] = "PENIS ";
struct SLider slider;
struct Ball Ball;
int ballSpeed = 0;
char key = 0;
init_uart(_UART0, _DEFFREQ, _DEFBAUD);
// initialis LED and clock
LED_init();
buttonInit();
//finde string length
Buffer(data);
n = sizeof(data)/sizeof(char)-1;
// Mainloop
clrscr();
window(1,1,posX2,posY2, "fsdf", 3 );
slidercreate(&slider, posX2,posY2);
ballInit(&Ball);
while(1){
sliderPos(&slider);
if (ballSpeed > 200){
ballPos(&Ball);
ballSpeed = 0;
}
if (Ball.y > posY2){
(Ball).x = 50;
(Ball).y = 3;
}
ballSpeed++;
gotoxy(15,15);
printf("%d", readKey());
printf("%d", slider.x2-slider.x1);
/* if (clock >=2){
shift++;
//reset the display
if (columm == 0){
PEOUT |= 0xE0;
PGOUT |= 0x80;
PEOUT = 0x1F;
PGOUT = 0;
}
LEDUpdate(columm, display);
//change display
if(columm == 4){
display = (display + 1) % 4;
columm = -1;
}
columm++;
clock=0;
}
// shift left
if (shift >= 90 ) {
LEDleft();
count++;
shift = 0;
//load new letter to "screen" 5
if (count == 6) {
loadBuffer( data[index], 4);
index = (index + 1) % n;
count = 0;
}
}*/
}
}