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の姿勢角度を保存 */
/*
一つ目及び２つ目の隅を見つけた際にフィールドの縦横の長さを計算する
フィールドの形は平行四辺形との規定があるため、２辺の長さしか算出して
いない。
*/
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;
}
}
