task main() { displayCenteredTextLine(0, "Station"); displayCenteredBigTextLine(1, "RoboTaxi"); displayCenteredTextLine(3, "Using A*"); displayCenteredTextLine(5, "Ashesi"); displayCenteredTextLine(6, "Fall 2015"); sleep(5000); eraseDisplay(); //store input from the numeric keypad base_station(); //displays values received as location and destination displayTextLine(1, "Test Values: "); displayTextLine(2, "Xval: %d", passenger_location_x); displayTextLine(3, "Yval: %d", passenger_location_y); displayTextLine(4, "XDest: %d", passenger_destination_x); displayTextLine(5, "YDest: %d", passenger_destination_y); wait1Msec(100); goal_x = passenger_destination_x; goal_y = passenger_destination_y; //find goal with taxi1 in mind findGoal(taxi1base_x, taxi1base_y, passenger_location_x, passenger_location_y); //get distance from taxi1 to passenger int dist1 = get_distance(taxi1base_x, taxi1base_y, passenger_location_x, passenger_location_y); //find goal with taxi2 in mind findGoal(taxi2base_x, taxi2base_y, passenger_location_x, passenger_location_y); //get distance from taxi2 to passenger int dist2 = get_distance(taxi2base_x, taxi2base_y, passenger_location_x, passenger_location_y); //compare distances between taxis and passengers. select taxi with shorter distance if (dist1 < dist2){//if taxi1 is shorter eraseDisplay(); displayTextLine(1, "taxi 1 "); wait1Msec(500); sendMessageWithParm(TAXI1LOC,passenger_location_x,passenger_location_y);//11 is location to taxi1 wait1Msec(500); displayTextLine(1, "to taxi 1.1 "); sendMessageWithParm(TAXI1DEST,passenger_destination_x,passenger_destination_y);//12 is destination to taxi1 wait1Msec(500); displayTextLine(1, "to taxi 1.2 "); } else {//if taxi2 is shorter displayTextLine(1, "taxi 2 "); wait1Msec(500); sendMessageWithParm(TAXI2LOC,passenger_location_x,passenger_location_y);//21 is location to taxi2 wait1Msec(500); displayTextLine(1, "to taxi 2.1 "); sendMessageWithParm(TAXI2DEST,passenger_destination_x,passenger_destination_y);//22 is destination to taxi2 wait1Msec(500); displayTextLine(1, "to taxi 2.2 "); } }
void ContProgress::Run(){ //履歴を移動 for(int i = 4; i > 1; i--){ this->innerMemory->setFBoard(i, this->innerMemory->getFBoard(i - 1)); } //最新状態を更新 this->innerMemory->setFBoard(0, this->getInput(0)); int step = this->getIBoard(0); if(step == NO_SIGNAL){ this->setIBoard(0, 0); }else{ this->setIBoard(0, step + 1); } //初期値はNO_SIGNAL float signalX = NO_SIGNAL; #ifdef IMPORTANCE_BASED this->importance = NO_SIGNAL; #endif //IMPORTANCE_BASED //現在地 float pos = this->getInput(0); //進度 float progress = pos / (float)step; if(progress < PROGRESS_LOW){ signalX = (float)MAX_DRIVE; #ifdef IMPORTANCE_BASED //進捗が遅いほど,重要とする this->importance = 1.0f * this->calcImportance(1.0f - progress / PROGRESS_LOW); #endif //IMPORTANCE_BASED }else{ signalX = NO_SIGNAL; #ifdef IMPORTANCE_BASED this->importance = NO_SIGNAL; #endif //IMPORTANCE_BASED } int goal = findGoal(); if(goal != -1){ if(goal != 0){ signalX = (float)MAX_DRIVE; #ifdef IMPORTANCE_BASED this->importance = 2.0f; #endif //IMPORTANCE_BASED } } //出力 this->setOutput(0, signalX); }
int simplehero::selectNeighbor(GraphMap* map, int cur_x, int cur_y) { // printf("Selecting neighbor\n"); int x, y, a, b; Pos* goal = findGoal(map, cur_x, cur_y); int g; if(goal == 0) { printf("Couldn't find goal\n"); return 0; } g = map->getVertex(goal->getX(), goal->getY()); int toGo = BFSearch(map, cur_x, cur_y, g); //printf("Done searching\n"); if(toGo == -1) { // printf("No target found\n"); delete goal; return 0; } /* if(p == 0 || p[1]->getX() < 0) { printf("ERRRORRORROR\n"); goal = findGoal(map, cur_x, cur_y); toGo = BFSearch(map ,cur_x, cur_y, goal); }*/ map->getPosition(toGo, x, y); for(int i = 0; i < map->getNumNeighbors(cur_x, cur_y); i++) { map->getNeighbor(cur_x, cur_y, i, a, b); if(x == a && y == b) { delete goal; return i; } } printf("Shouldn't get here"); return 0; }