/**
* Second main function
* @return
*/
int main_2() {
// Initializations
Position start, destination;
Path_element *path = NULL;
Path_element *robot_path;
int x, y;
#ifdef CHALLENGE_AB
int initial_facing_direction;
int i;
static int first_run = 1;
if (first_run) {
first_run = 0;
// Get which places to user wants to visit
getPlacesToVisit();
}
// Reset no_total_path_possible
no_total_path_possible = 0;
#endif
// Unvisit all places
unvisitAll();
// Seed the rand() function
srand(time(NULL));
#ifdef CHALLENGE_AB
// Randomize facing direction
initial_facing_direction = NORTH;// = rand() % 4 + 1;
#endif
// Initialize grid
generateGrid();
#if MINES_ACTIVE == 1
// Create mines
// createMines();
// removeConnection(0,0,1,0);
// removeConnection(1,0,2,0);
// removeConnection(0,2,1,2);
// removeConnection(2,2,3,2);
// removeConnection(3,3,4,3);
// removeConnection(0,0,0,1);
// removeConnection(1,0,1,1);
// removeConnection(1,3,1,4);
// removeConnection(2,1,2,2);
// removeConnection(2,3,2,4);
// removeConnection(3,0,3,1);
// removeConnection(4,0,4,1);
// removeConnection(4,1,4,2);
// removeConnection(0,0,0,1);
// removeConnection(1,3,1,2);
// removeConnection(2,2,2,3);
// removeConnection(4,1,4,2);
#endif
// Define the start position in the bottom left
start.x = destination.x = 1;
start.y = destination.y = 0;
#if MINES_ACTIVE == 1
// Discover mines at start location
// discoverMines(start.x, start.y);
#endif
#ifdef CHALLENGE_AB
int key, q;
goto go2;
frombeginning2:
cls_2();
printf("Place the robot at the beginning, type '1' to continue...\n");
key = 0;
while (!key) { scanf("%d",&key); }
go2:
curDir = 0;
setCommand(new_sck, MOVE, 1, 1, 0);
wait_till_ready(new_sck);
setCommand(new_sck, POS, 0, start.x+1, start.y+1);
wait_till_ready(new_sck);
// Get first destination
destination.x = places_to_visit[getTargetPlace(start.x, start.y, initial_facing_direction, 0)][0];
destination.y = places_to_visit[getTargetPlace(start.x, start.y, initial_facing_direction, 0)][1];
// Create the robot_path linked list
robot_path = malloc(sizeof (Path_element));
robot_path->x = start.x;
robot_path->y = start.y;
robot_path->facing_direction = initial_facing_direction;
robot_path->next = NULL;
for (q = 0; q < NUM_NODES; q++) getNodeById(q)->mark = 0;
// Mark places to visit
for (i = 0; i < NUMBER_OF_PLACES_TO_VISIT; i++) {
if (!places_to_visit[i][2])
grid[places_to_visit[i][0]][places_to_visit[i][1]]->mark = 49 + i;
}
// Find initial path
path = findShortestPath(start.x, start.y, initial_facing_direction, destination.x, destination.y);
// If path == NULL, there was no path found
if (!path || no_total_path_possible) {
// Clear screen, print the grid, display a message, sleep for a while and then rerun the program
cls_2();
printGrid(robot_path);
printf("\nCouldn't find a intial path...\n");
// getchar();
// main();
return 0;
}
// Record start
x = start.x;
y = start.y;
// Display first move
cls_2();
printGrid(robot_path);
// getchar();
int moveCorrect = 1;
while (1) {
if (kbhit()) goto frombeginning2;
if (moveCorrect) path = path->next;
// addToEndOfPath(&robot_path, path->x, path->y);
if (x == path->x && y == path->y) {
path = path->next;
}
int prevfd = path->facing_direction;
if (makeMove(x,y,path->x,path->y)) {
moveCorrect = 1;
x = path->x;
y = path->y;
} else {
printf("Returned mine between (%d,%d)<curr pos> and (%d,%d)\n",x,y,path->x,path->y);
removeConnection(x,y,path->x,path->y);
moveCorrect = 0;
// prevfd = reverseDirection(prevfd);
}
// Move and save position and step weight, also add the position to the path
if (moveCorrect) {
path = path->next;
addToEndOfPath(&robot_path, x, y);
}
// printf("VISIT\n");
visit(x, y);
// printf("VISITED ALL PLACES\n");
if (visitedAllPlaces()) break;
// printf("getTargetPlace\n");
// Get destination
destination.x = places_to_visit[getTargetPlace(x, y, prevfd, 0)][0];
destination.y = places_to_visit[getTargetPlace(x, y, prevfd, 0)][1];
// #if MINES_ACTIVE == 1
// Check for mines
// discoverMines(x, y);
// Now filter combs for discovered mines
// #endif
// Calculate new path, mines couldve changed something
// printf("New path %d,%d %d %d, %d\n",x,y,prevfd,destination.x,destination.y);
path = findShortestPath(
x, y,
prevfd,
destination.x,
destination.y
);
// printf("kay\n");
// If path == NULL, there was no path found
if (!path || no_total_path_possible) {
cls_2();
printGrid(robot_path);
printf("Could not find a path!\n");
getchar();
return 0;
}
// Display path and add step weight to path weight
cls_2();
printGrid(robot_path);
getTargetPlace(x, y, path->facing_direction, DEBUG);
// getchar();
}
// #if MINES_ACTIVE == 1
// // Reveal mines
// revealMines();
// #endif
// Display final screen
cls_2();
printGrid(robot_path);
printf("\nDone.\n");
getchar();
#endif
#ifdef CHALLENGE_C
// Find initial path
int *combs;
int index;
int q;
// clear grid markings
// Some unreadable shit which makes things work like they should
cls_2();
printf("Press enter to start...\n"); getchar();
int antiMine = 0;
rerun:
goto go3;
antimine:
antiMine = 1;
go3:
stepsTakenSize = 0;
index = 0;
int key;
goto go2;
frombeginning:
cls_2();
printf("Place the robot at the beginning, type '1' to continue, '9' to restart or '5' to restart in anti-mine mode...\n");
key = 0;
while (key==0) { scanf("%d",&key); }
if(key==9) goto rerun;
if(key==5) goto antimine;
go2:
curDir = 0;
setCommand(new_sck, MOVE, 1, 1, 0);
wait_till_ready(new_sck);
for (q = 0; q < NUM_NODES; q++) getNodeById(q)->mark = 0;
robot_path = malloc(sizeof (Path_element));
robot_path->x = start.x;
robot_path->y = start.y;
robot_path->next = NULL;
cls_2();
printGrid(robot_path);
setCommand(new_sck, POS, 0, start.x+1, start.y+1);
wait_till_ready(new_sck);
x = start.x;
y = start.y;
// Find initial path
combs = exploreArea(grid[start.x][start.y]->id);
// If path == NULL, there was no path found
if (!combs) {
// Clear screen, print the grid, display a message, sleep for a while and then rerun the program
printf("\nCouldn't find a intial path...\n");
getchar();
// main();
return 0;
}
while (1) {
// Start by removing the path walked from the current combs
int i;//, fins = 1;
for (i = 0; combs[i] != -2; i+=2) {
if (inStepsTaken(combs[i],combs[i+1])) {
combs[i] = combs[i+1] = -1;
}
}
// Check if there is a comb available
if (combs[index] == -1) {
index += 2;
continue;
}
if (combs[index] == -2) break;
// First walk to first node
int nodeNow = combs[index];
int nodeTo = combs[index+1];
//printf
path = findShortestPath(x, y,
(path?path->facing_direction:NORTH),
// 1,
getNodeById(nodeNow)->positionGrid.x,
getNodeById(nodeNow)->positionGrid.y);
int failed = 0;
int intcpt = 0;
while (!(x == getNodeById(nodeNow)->positionGrid.x && y == getNodeById(nodeNow)->positionGrid.y)) {
if (kbhit()) goto frombeginning;
if (path && x == path->x && y == path->y) path = path->next;
else if (!path) {
printf("FAIL!\n");
getchar();
intcpt = 1;
failed = 1;
break;
}
// printf("Making first MOVE!\n");
if (makeMove(x,y,path->x,path->y)) {
stepTaken(grid[x][y]->id,grid[path->x][path->y]->id);
// printf("Step correct! (from %d to %d)\n", grid[x][y]->id,grid[path->x][path->y]->id);getchar();
x = path->x;
y = path->y;
path = path->next;
addToEndOfPath(&robot_path, x, y);
} else {
if (antiMine) goto frombeginning;
removeConnection(x,y,path->x,path->y);
failed = 1;
break;
}
cls_2();
printGrid(robot_path);
}
if (failed) { // Soooo it failed, boohoo.
if (!intcpt) islandSN(grid[x][y]->id);
//cls_2();
printf("Recalculating...\n");
combs = exploreArea(grid[x][y]->id);
index = 0;
cls_2();
printGrid(robot_path);
continue;
}
// Okay now walk to second node
path = findShortestPath(x, y,
(path?path->facing_direction:NORTH),
getNodeById(nodeTo)->positionGrid.x,
getNodeById(nodeTo)->positionGrid.y);
while (!(x == getNodeById(nodeTo)->positionGrid.x && y == getNodeById(nodeTo)->positionGrid.y)) {
if (kbhit()) goto frombeginning;
if (path && x == path->x && y == path->y) path = path->next;
else if (!path) {
printf("FAIL!\n");
getchar();
intcpt = 1;
failed = 1;
break;
}
if (makeMove(x,y,path->x,path->y)) {
stepTaken(grid[x][y]->id,grid[path->x][path->y]->id);
// printf("Step correct! (from %d to %d)\n", grid[x][y]->id,grid[path->x][path->y]->id);getchar();
x = path->x;
y = path->y;
path = path->next;
addToEndOfPath(&robot_path, x, y);
} else {
if (antiMine) goto frombeginning;
removeConnection(x,y,path->x,path->y);
failed = 1;
break;
}
cls_2();
printGrid(robot_path);
}
if (failed) { // Soooo it failed, boohoo.
if (!intcpt) islandSN(grid[x][y]->id);
//cls_2();
printf("Recalculating...\n");
combs = exploreArea(grid[x][y]->id);
index = 0;
// islandSN(grid[x][y]->id);
cls_2();
printGrid(robot_path);
continue;
}
// Guess everything went okay
index += 2;
}
printf("Aaaaaaaaaaaaaand we're done, type '1' to rerun and '5' to rerun in anti-mine mode!\n");
key = 0;
while (key==0) { scanf("%d",&key); }
if (key==1) goto rerun;
else if(key==5) goto antimine;
#endif
return EXIT_SUCCESS;
}
/**
* This method will get called on idle. This method should be responsible with doing the work that the application needs to do
* (i.e. run simulations, and so on).
*/
void ControllerEditor::processTask(){
double simulationTime = 0;
double maxRunningTime = 0.98/Globals::desiredFrameRate;
Vector3d backupInput;
bool once = false;
//if we still have time during this frame, or if we need to finish the physics step, do this until the simulation time reaches the desired value
while (simulationTime/maxRunningTime < Globals::animationTimeToRealTimeRatio){
simulationTime += SimGlobals::dt;
double phi = conF->getController()->getPhase();
lastFSMState = conF->getController()->getFSMState();
double signChange = (conF->getController()->getStance() == RIGHT_STANCE)?-1:1;
Globals::targetPosePhase = phi;
Tcl_UpdateLinkedVar( Globals::tclInterpreter, "targetPosePhase" );
// tprintf("d = %2.4lf, v = %2.4lf\n", conF->con->d.x, conF->con->v.x);
avgSpeed += conF->getCharacter()->getHeading().getComplexConjugate().rotate(conF->getCharacter()->getRoot()->getCMVelocity()).z;
timesVelSampled++;
if (conF) {
if( phi < dTrajX.getMaxPosition() ) {
dTrajX.clear();
dTrajZ.clear();
vTrajX.clear();
vTrajZ.clear();
}
Vector3d d = conF->getController()->d;
Vector3d v = conF->getController()->v;
dTrajX.addKnot( phi, d.x * signChange);
dTrajZ.addKnot( phi, d.z );
vTrajX.addKnot( phi, v.x * signChange );
vTrajZ.addKnot( phi, v.z );
bool newStep = conF->advanceInTime(SimGlobals::dt);
//int sign = -1;
double maxChange = 0.2;
char * s = "out\\reducedCharacterState.rs";
if (!once)
{
backupInput = conF->getCharacter()->getRoot()->getCMVelocity();
once = true;
}
if (newStep) {
avgSpeed /= timesVelSampled;
Vector3d v = conF->getLastStepTaken();
tprintf("step: %lf %lf %lf (phi = %lf, speed = %lf)\n", v.x, v.y, v.z, phi, avgSpeed);
//Globals::animationRunning = false;
//conF->getCharacter()->setHeading(.2);
RigidBody* bBall = InteractiveWorld::getBall();
if (bBall != NULL)
{
double q1 = bBall->getCMPosition().getZ();
double q2 = bBall->getCMPosition().getX();
double m1 = conF->getCharacter()->getRoot()->getCMPosition().getZ();
double m2 = conF->getCharacter()->getRoot()->getCMPosition().getX();
double q = (q2 - m2)/(q1 - m1);
double curr = SimGlobals::desiredHeading;
double diff = ( q - diff);
double dist = sqrt(pow((q2 - m2), 2) + pow((q1 - m1), 2));
if (dist < .4)
{
//s = "out\\reducedCharacterState1.rs";
tprintf("state changed once");
//Vector3d input=conF->getCharacter()->getRoot()->getCMVelocity() * 1.2;
//conF->getCharacter()->getRoot()->setCMVelocity(input);
//Vector3d d = conF->getController()->d;
//Vector3d v = conF->getController()->v;
//dTrajX.addKnot(phi, d.x * signChange);
//dTrajZ.addKnot(phi, d.z*2);
//vTrajX.addKnot(phi, v.x * signChange);
//vTrajZ.addKnot(phi, v.z*2);
//Character* bip = conF->getCharacter();
//SimBiController* con = new SimBiController(bip);
//char * fline = "../data/characters/fJog.rbs";
//FILE *f = fopen(fline, "r");
//SimBiConState* tempState;
//con->loadFromFile(line);
}
else
{
Vector3d input = backupInput;
conF->getCharacter()->getRoot()->setCMVelocity(input);
//s = "out\\reducedCharacterState.rs";
}
/*if (q > 0)
{
if (abs(diff) > .2)
{
conF->getCharacter()->setHeading(curr + maxChange);
//conF->getCharacter()->setHeading(q/3);
SimGlobals::desiredHeading = (curr + maxChange);
}
else
{
conF->getCharacter()->setHeading(q / 2);
//conF->getCharacter()->setHeading(q/3);
SimGlobals::desiredHeading = (q / 2);
}
}
else
{
if (abs(diff) > .2)
{
conF->getCharacter()->setHeading(curr - maxChange);
//conF->getCharacter()->setHeading(q/3);
SimGlobals::desiredHeading = (curr - maxChange);
}
else
{*/
q = atan(q);
conF->getCharacter()->setHeading(q );
//conF->getCharacter()->setHeading(q/3);
SimGlobals::desiredHeading = (q);
/*}
}*/
tprintf("step: %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",dist ,curr, q, q1, q2, m1, m2, bBall->getOrientation().getV().getX(),bBall->getOrientation().getV().getZ());
}
avgSpeed = 0;
timesVelSampled = 0;
stepTaken();
//get the reversed new state...
DynamicArray<double> newState;
if (conF->getController()->getStance() == RIGHT_STANCE)
conF->getCharacter()->getReverseStanceState(&newState);
else
conF->getCharacter()->getState(&newState);
ReducedCharacterState rNew(&newState);
conF->getCharacter()->saveReducedStateToFile(s, newState);
}
// if (conF->getController()->isBodyInContactWithTheGround()){
// tprintf("Lost control of the biped...\n");
// Globals::animationRunning = false;
// break;
// }
// break;
}
}
}