void stateGoingToCube::processData(){
//Wait to get a still image
static int cubeFound;
static double distance;
static double angle;
static int color;
startProcessData();
if((getTimeMicroseconds()-startTimeStateMicroseconds)/1000 <= mywaitTimeMS){
stop();
cubeFound=foundCube();
distance = myImageProcessor->getNearestCubeDist()+GO_TO_CUBE_OVERSHOOT_DISTANCE;
angle = myImageProcessor->getNearestCubeAngle();
color = myImageProcessor->getNearestCubeColor();
}
else if(cubeFound){
goToPoint(distance,angle);
if (finishedGoingToPoint){
nextState = new stateCollectingCube(this);//(this,color); //->pass camera color
//PASS NO COLOR PARAMETER TO GET DATA FROM THE COLOR SENSOR
}
}
else{
nextState=new stateLookingForBlocks(this);
}
finishProcessData();
}
void
SSLGamePlanner::kickOff ()
{
if (ssl_game_state.kickoff ())
{
if (ssl_game_state.ourKickoff ())
{
goToBall ((uint8_t)KICK_OFF_ROBOT, 0.0 + (int)OUR_FIELD * ssl::math::PI);
}
else
{
geometry_msgs::Point32 p;
p.x = -(0.5 + ssl::config::ROBOT_RADIUS) + 2 * (0.5 + ssl::config::ROBOT_RADIUS) * (int)OUR_FIELD;
p.y = 0;
p.z = 0;
goToPoint ((uint8_t)KICK_OFF_ROBOT, 0.0 + (int)OUR_FIELD * ssl::math::PI, p);
}
}
geometry_msgs::Point32 p;
p.x = -2.975 + 2 * 2.975 * (int)OUR_FIELD;
p.y = 0;
p.z = 0;
goToPoint ((uint8_t)GOALIE_ROBOT, 0 + (int)OUR_FIELD * ssl::math::PI, p);
p.x = -1.000 + 2 * 1.0 * (int)OUR_FIELD;
p.y = -1.000;
p.z = 0;
goToPoint ((uint8_t)2, 0, p);
p.y = 0.0;
goToPoint ((uint8_t)3, 0, p);
p.y = 1.0;
goToPoint ((uint8_t)4, 0, p);
}
void traverse2(int previousNode, int currentNode, Location* currentLocation, int* orientation, Location mazeCoords[], int visited[], int dijkstraPath[]) {
int i, j;
// If graph uninitialised, initialise it
if (!graphInitialised) {
for (i = 0; i < 17; i++) {
for (j = 0; j < 17; j++) {
graph[i][j] = 0;
}
}
graphInitialised = 1;
}
int adjacentNodes[3];
findAdjacentNodes(currentNode, *orientation, adjacentNodes);
visited[currentNode] = 1;
/*
printf("Visited nodes:");
for (i = 0; i < 17; i++) {
if (visited[i]) {
printf(" %d", i);
}
}
printf("\n");
*/
graph[0][1] = 1;
graph[1][0] = 1;
for (i = 0; i < 3; i++) {
if (adjacentNodes[i] >= 0) {
graph[currentNode][adjacentNodes[i]] = 1;
graph[adjacentNodes[i]][currentNode] = 1;
}
}
for (i = 0; i < 3; i++) {
if (adjacentNodes[i] >= 0 && visited[adjacentNodes[i]] == 0) {
updateOrientation(orientation, currentNode, adjacentNodes[i]);
printf("Traverse \n");
printf("Current node: %d \n", currentNode);
printf("Orientation: %d \n", *orientation);
printf("Current location: x = %f; y = %f \n", currentLocation->x, currentLocation->y);
printf("Target location: x = %f; y = %f \n", mazeCoords[adjacentNodes[i]].x, mazeCoords[adjacentNodes[i]].y);
goToPoint(currentLocation, &mazeCoords[adjacentNodes[i]]);
correctLocation(adjacentNodes[i], currentLocation, orientation, mazeCoords);
traverse2(currentNode, adjacentNodes[i], currentLocation, orientation, mazeCoords, visited, dijkstraPath);
}
}
updateOrientation(orientation, currentNode, previousNode);
goToPoint(currentLocation, &mazeCoords[previousNode]);
correctLocation(previousNode, currentLocation, orientation, mazeCoords);
printf("Current location: x = %f; y = %f \n", currentLocation->x, currentLocation->y);
int explored = 1;
for (i = 0; i < 17; i++) {
if (visited[i] == 0) {
explored = 0;
}
}
if (explored) {
//printgraph();
dijkstra(0, dijkstraPath);
}
}
int umain (void) {
//while (1)
//{
/*
motor_set_vel(0, -100);
motor_set_vel(1, -90);
pause(3000);
motor_set_vel(0, 0);
motor_set_vel(1, 0);
pause(3000);
motor_set_vel(0, 70);
motor_set_vel(1, -70); //turns right
pause(1000);
brake();
*/
//Point pt = {10, 10, 10};
//update();
//Point pt = {game.coords[1].x, game.coords[1].y};
//Point pt = {0, 0};
goToPoint();
/*
//left side of board
motor_set_vel(0, -90);
motor_set_vel(1, -100);
pause(500);
motor_set_vel(0, -70); //turns left
motor_set_vel(1, 70);
pause(700);
motor_set_vel(0, -90);
motor_set_vel(1, -100);
pause(1000);
motor_set_vel(0, 70);
motor_set_vel(1, -70);
pause(700);
motor_set_vel(0, -90);
motor_set_vel(1, -100);
pause(5000);
brake();
*/
/*
//right side of board
motor_set_vel(0, -90);
motor_set_vel(1, -100);
pause(500);
motor_set_vel(0, 70); //turns right
motor_set_vel(1, -70);
pause(800);
motor_set_vel(0, -90);
motor_set_vel(1, -100);
pause(1500);
motor_set_vel(0, -70);
motor_set_vel(1, 70);
pause(900);
motor_set_vel(0, -90);
motor_set_vel(1, -100);
pause(6000);
brake();
*/
/*
float deg = gyro_get_degrees();
if (deg < 0) {
motor_set_vel(0, 90);
motor_set_vel(1, 90);
} else {
motor_set_vel(0, 90);
motor_set_vel(1, 90);
}
*/
//}
return 0;
}
