int main() { startOI("/dev/ttyUSB0"); while(getBumpsAndWheelDrops() != 2) { traveling(); } stopOI(); return 0; }
struct field_outline_t form_getFieldFormInfo (struct self_pos_t *absolute_locate, const struct wheel_speed_t go_forward){ struct xy_coord_t current_bump_pos, before_bump_pos; struct xy_coord_t start_corner_pos, first_corner_pos, second_corner_pos; struct field_outline_t field_info; int corner_counter = 0; int loop_counter = 0; int judge_bump; int judge_corner_flag; int approach_corner_angle[CORNER_NUM]; int draw_count = 0; char *fname1 = "field_form_coord.txt"; FILE *fp1; /* 外壁情報取得開始時の隅の座標の取得 */ start_corner_pos.x = absolute_locate->pos.x; start_corner_pos.y = absolute_locate->pos.y; /* 衝突位置座標の初期化 */ before_bump_pos.x = current_bump_pos.x = 0; before_bump_pos.y = current_bump_pos.y = 0; while(1){ /* createを直進させる */ directDrive(go_forward.vel_left, go_forward.vel_right); getCurrentSelfPos(absolute_locate, go_forward); if(loop_counter % DRAW_COUNT_RANGE == 0){ filePrintCoord(&fp1, fname1, "a", absolute_locate->pos.x, absolute_locate->pos.y); printf("%d, %d %d\n", absolute_locate->pos.x, absolute_locate->pos.y, absolute_locate->theta); } /* createが衝突したかを判定する */ /* getBumpsAndWheelDropsはcreateが衝突したならば以下の値を返す */ /* 右側のbumpsensorが衝突を検知->1 左側のbumpsensor->2 両方のbumpsensorに衝突を検知->3 */ judge_bump = getBumpsAndWheelDrops(); if(judge_bump == 1 || judge_bump == 2 || judge_bump == 3){ current_bump_pos = form_getBumpPos(absolute_locate); /*隅に到達したならば、judge_corner_flagを立てる*/ judge_corner_flag = form_judgeCorner(current_bump_pos, before_bump_pos); if(judge_corner_flag == TRUE){ corner_counter++; printf("corner_num = %d\n\n\n\n", corner_counter); form_resetAbsoluteTheta(corner_counter, absolute_locate); approach_corner_angle[corner_counter] = absolute_locate->theta;/* 隅に進入時のcreateの姿勢角度を保存 */ /* 一つ目及び2つ目の隅を見つけた際にフィールドの縦横の長さを計算する フィールドの形は平行四辺形との規定があるため、2辺の長さしか算出して いない。 */ switch(corner_counter){ case 1: first_corner_pos = form_getCornerPos(absolute_locate); field_info.virtical_length = form_getFieldLength(start_corner_pos, first_corner_pos); break; case 2: second_corner_pos = form_getCornerPos(absolute_locate); field_info.side_length = form_getFieldLength(first_corner_pos, second_corner_pos); break; case CORNER_NUM: field_info.corner_angle = form_setEachCornerAngle(approach_corner_angle); robot_Stop_Back_Stop(1, BACK_DISTANCE, absolute_locate); absolute_locate->theta = robot_Turn_Stop(TARGET_ANGLE_5, CORRECTION_ANGLE_5, turn_clockwise_100, absolute_locate->theta); return field_info; default: break; } } robot_Stop_Back_Stop(0.2, BACK_DISTANCE, absolute_locate); absolute_locate->theta = robot_Turn_Stop(TARGET_ANGLE_5, CORRECTION_ANGLE_5, turn_clockwise_100, absolute_locate->theta); before_bump_pos = current_bump_pos; } countCicleTime(); draw_count++; } }
void Controller::checkState() { Coordinate create_coord; bool Bumper_Hit = false; std::cout << "total_distance:" << this->create.getTotalDistance() << std::endl; std::cout << "angle:" << this->create.getTotalAngle() << std::endl; std::cout << "(" << this->create.getCurrentCoordinate().x << ", " << this->create.getCurrentCoordinate().y << ")" << std::endl; std::cout << "direction:"<< this->create.direction << std::endl; std::cout << "current mesh:" << this->block.getCurrentMeshNum(this->create.getCurrentCoordinate()) << std::endl; // 1.壁探索 if(this->search_flag == WALL) { int bumper_hit = getBumpsAndWheelDrops(); Coordinate wall_coord; //1-1.バンパーに衝突したかを判定 // 1-1-1.バンパセンサ反応無し → 現在座標、超音波センサの観測座標を記録 this->block.showMesh(); if(bumper_hit == 0) { float soner_distance; // 1-1-1-1各座表値の計算 this->create.doNormalMode(create_coord, wall_coord, soner_distance); // 1-1-1-2座標値の記録 → Map this->map.push_back_CreatePointList( create_coord ); //createの現在座標を記録(プッシュバック) if(soner_distance < RECORD_OBSTACLE_TH) // RECORD_OBSTACLE_TH 以上離れた障害物は記録しない { this->map.push_back_WallPointList( wall_coord ); // 障害物の座標を記録(プッシュバック) } this->create.run(); } // 1-1-2.バンパセンサに衝突 else if( bumper_hit != 0) { // 1-1-2-1.現在座標とバンパーヒット座標を計算 & 方向転換 → Createクラスへ this->create.doBumperHitMode(bumper_hit, create_coord, wall_coord); // 1-1-2-2.座標値の記録 → Mapクラスへ this->map.push_back_CreatePointList( create_coord ); this->map.push_back_WallPointList( wall_coord ); } //1-2.メッシュの更新 this->block.setMeshMark( create_coord, Bumper_Hit ); //1-3.壁探索終了の判定 Coordinate start_coord; int total_distance = this->create.getTotalDistance(); // 1個目ドッキング検知 if(this->create.getTotalDistance() > 4000 && this->create.isDockFound() && !this->LinePointSet) { this->create.stopRun(); // 壁探索が終わったら、即Createを止める std::cout << "2line point set!" << std::endl; std::vector<Coordinate> tmp_obstacle_list; std::vector<Coordinate> SOC_list; // search obstacle create list 障害物を探索する時のcreateの座標値リスト this->map.set2LinePoint(); // 2辺分の座標値をwall_point_list2に入れる this->output_WallList2(); // this->showWall2(); this->LinePointSet = true; tmp_obstacle_list = create.searchObstacle(SOC_list); // 衝突した障害物の周りを回る for(int i=0;i<SOC_list.size();i++) { this->map.push_back_CreatePointList( SOC_list[i] ); this->map.push_back_ObstaclePointList( SOC_list[i] ); } this->search_flag = DOCK; this->showWall2(); } else if(this->create.getTotalDistance() > 10000 && this->create.isDockFound()) // 2個目ドッキング検知 { std::vector<Coordinate> tmp_obstacle_list; std::vector<Coordinate> SOC_list; // search obstacle create list 障害物を探索する時のcreateの座標値リスト this->create.stopRun(); // 壁探索が終わったら、即Createを止める tmp_obstacle_list = create.searchObstacle(SOC_list); // 衝突した障害物の周りを回る for(int i=0;i<SOC_list.size();i++) { this->map.push_back_CreatePointList( SOC_list[i] ); this->map.push_back_ObstaclePointList( SOC_list[i] ); } std::cout << "Docking!" << std::endl; this->search_flag = DOCK; } } //2.障害物探索 else if(this->search_flag == OBSTACLE) // 障害物探索 { int goal; std::vector<Coordinate> move_point_list; // Createが辿る座標を格納したリスト std::vector<Coordinate> tmp_create_list; std::vector<Coordinate> tmp_obstacle_list; this->block.showMesh(); bool Bumper = false; // 2-1.createの現在座標を取得 create_coord = this->create.getCurrentCoordinate(); // 2-2.createの現在座標から向かうメッシュを計算し、辿る座標リストを得る goal = this->block.getNextMeshNum(); move_point_list = this->block.getMovePointList(create_coord, this->create.direction, goal);// → Createの現在座標を元に、ゴールまでに辿る座標のリストを得る // 2-3.座標リスト通りに進む for(int i=0;i<move_point_list.size();i++) { // 2-3-1.次の座標値を渡して、移動し、各座表値の計算を行う this->create.runNextPoint(move_point_list[i], Bumper, tmp_create_list, tmp_obstacle_list); if(Bumper) // runNextPoint()中に障害物に衝突したら { this->create.changeDirection(); break; #if 1 std::vector<Coordinate> SOC_list; // search obstacle create list 障害物を探索する時のcreateの座標値リスト Bumper = false; tmp_obstacle_list = create.searchObstacle(SOC_list); // 衝突した障害物の周りを回る for(int i=0;i<tmp_obstacle_list.size();i++) { this->map.push_back_ObstaclePointList( tmp_obstacle_list[i] ); this->block.setMeshMark(tmp_obstacle_list[i], true); //obstacle_list.push_back(tmp_obstacle_list[i]); } for(int i=0;i<SOC_list.size();i++) { this->map.push_back_CreatePointList( SOC_list[i] ); this->block.setMeshMark(SOC_list[i], false);// //create_list.push_back(create_coord[i]); } #endif } // 2-3-2 移動後の座標値を渡して、メッシュを更新 for(int j=0;j<tmp_create_list.size();j++) { this->block.setMeshMark(tmp_create_list[j], false);// // 2-3-3各座表の情報を渡して、map,obstacleリストを更新 this->map.push_back_CreatePointList( tmp_create_list[j] ); } for(int j=0;j<tmp_obstacle_list.size();j++) { // this->block.setMeshMark(tmp_obstacle_list[j], true);// this->map.push_back_ObstaclePointList( tmp_obstacle_list[j] ); } this->block.showMesh(); } // 2-4.全部のメッシュをチェックし終えたかを判定 if( !this->block.checkAllSearchEnd() ) { this->search_flag = DOCK; } } // 3.ドッキングステーションへゴール else if(this->search_flag == DOCK) { this->create.stopRun(); runDemo(DEMO_COVER_AND_DOCK); this->finished = true; std::cout << "finished!" << std::endl; } }