Target zone_Search_Handler_Plastic(VideoCopy *image, Target obj)
{
Target zone;
redZoneCnt = blueZoneCnt = greenZoneCnt = 0;
while(1){
zone = findZone(image, obj);
printf("obj flag : %d\n", zone.flag);
if(zone.flag == MILK)
break;
else if(zone.flag == 5) // red
{
turn_Right_Grabbing_Plastic();
usleep(200000);
turn_Right_Grabbing_Plastic();
usleep(200000);
redZoneCnt++;
}
else if(zone.flag == 4) // green
{
turn_Left_Grabbing_Plastic();
usleep(200000);
blueZoneCnt++;
}
else if(zone.flag > 3)
{
printf("zone not found\n");
music_G();
usleep(200000);
}
}
return zone;
}
void* doMission(void) {
if(goal == MILK)
{
while (stopFlag == 0) {
switch (step) {
case 1: // 우유팩만 탐색
// 우유팩만 찾으면 됨
//t = findTarget(&bufCopy);
t = findTarget(&bufCopy, COKE);
break;
case 2: // 우유팩에 접근
while(1) {
switch (approach2Milk(&bufCopy, t)) {
case 2:
exit(0);
break;
case 0:
goto STOP_APPROACH_2_MILK;
break;
default:
usleep(2000000);
break;
}
}
STOP_APPROACH_2_MILK:
break;
case 3: // 우유팩 줍기
// 우유팩을 위한 겟타겟
t.flag = MILK;
getTarget(t);
// 180도 돌 것인가?
break;
case 4: // 중간까지 나오기
// go_MiddlePos();
break;
case 5: // 목표장소 탐색
// 고동완이 만들 목적지 탐색 비교 함수 필요
// 목적지를 찾을 수 있게 좀 나와야함
// findZone implementation
t.flag = MILK;
z = findZone(&bufCopy, t);
// zoneHandler(&minorStep, z);
break;
case 6: // 우유팩 장소에 접근
// 우유팩 존에 다가갈 함수 필요
approach2Zone(&bufCopy, z);
break;
case 7: // 던지기
// 우유팩 던지는것 필요
putTarget();
// stopFlag = 1;
// 물건 놓았으면 적외선 감지 끝
// terminate_infra();
break;
default:
break;
}
step++;
if(step >= 10)
step = 1;
}
}
/*
else if(goal == PLASTIC)
{
while (stopFlag == 0) {
switch (minorStep) {
case 1:
// 플라스틱 찾을 함수 필요
//t = findTarget(&bufCopy);
t = search_Step();
break;
case 2: // 타겟 찾기 예외처리
break;
case 3:
// 플라스틱에 특화된 접근 필요
stopCnt = 0;
while(stopCnt < 2){
approach2Target(&bufCopy, t);
stopCnt++;
}
break;
case 4: // 물체 접근 예외처리
break;
case 5:
// 플라스틱 목적지 탐색하면
// 플라스틱 걷어차는 함수 필요
getTarget(t);
// 성공하였을 경우 적외선 감지 시작
// generate_Infra();
break;
case 6: // 물체 집기 예외처리
break;
case 7: // 다음 단계 전환 전 준비
break;
default:
break;
}
minorStep++;
}
}
*/
// 다 끝나면 캔에 대해서 작업
}
Target zone_Search_Handler(VideoCopy *image, Target obj)
{
Target zone;
redZoneCnt = blueZoneCnt = greenZoneCnt = 0;
while(1){
zone = findZone(image, obj);
printf("obj flag : %d\n", zone.flag);
if(zone.flag == MILK)
{
if(zone.x_point < 50)
turn_Right_Grabbing();
else if(zone.x_point > 270)
turn_Left_Grabbing();
break;
}
else if(zone.flag == 7) // red
{
turn_Right_Grabbing();
usleep(200000);
redZoneCnt++;
}
else if(zone.flag == 8) // blue
{
turn_Right_Grabbing();
usleep(200000);
blueZoneCnt++;
}
else if(zone.flag > 3)
{
printf("zone not found\n");
right_Kick();
usleep(200000);
}
}
if(redZoneCnt > 0){
moveStraightGrabbing();
usleep(15000000);
moveStop();
usleep(200000);
}
/*
else if(blueZoneCnt > 0){
moveStraightGrabbing();
usleep(5000000);
moveStop();
usleep(200000);
}
*/
// while(1){
zone = findZone(image, obj);
printf("obj flag : %d\n", zone.flag);
if(zone.flag == MILK)
{
if(zone.x_point < 50)
turn_Right_Grabbing();
else if(zone.x_point > 270)
turn_Left_Grabbing();
// break;
}
else if(zone.flag == 7) // red
{
turn_Right_Grabbing();
usleep(200000);
redZoneCnt++;
}
else if(zone.flag == 8) // blue
{
turn_Right_Grabbing();
usleep(200000);
blueZoneCnt++;
}
else if(zone.flag > 3)
{
printf("zone not found\n");
// turn_Left_Grabbing();
usleep(200000);
}
// }
return zone;
}
void rotate_To_Zone(VideoCopy *image, Target target)
{
int i;
while(1){
Target zone = findZone(image, target);
printf("zone flag : %d\n", zone.flag);
printf("x : %d\n", zone.x_point);
if(zone.flag == target.flag)
{
//music_G();
// 가운데 있다고 판단 되면
if(zone.x_point < maxX && zone.x_point >minX)
{
music_G();
break;
}
else if(zone.x_point <=minX)
{
for(i=0; i<1; i++){
rotateOnRight();
//usleep(200000);
turnCnt--;
}
}
else if(zone.x_point >=maxX)
{
for(i=0;i<1; i++){
rotateOnLeft();
//usleep(200000);
turnCnt++;
}
}
}
else if(zone.flag == 4) // see left
{
for(i=0;i<2; i++){
rotateOnLeft();
//usleep(200000);
turnCnt++;
}
}
else if(zone.flag == 5) // see right
{
for(i=0; i<2; i++){
rotateOnRight();
//usleep(200000);
turnCnt--;
}
}
else if(zone.flag == 6)
{
music_D();
printf("zone not found\n");
}
}
}
void searchZone_n_Kick(VideoCopy *image, Target target)
{
int kickCnt = 0;
int i,j;
int sideFlag_Cnt = 3; // 사이드인지 아닌지 판별할 플래그의 카운트 기준수
while(1)
{
int kickFlag = 0;
Target zone = findZone(image, target);
printf("zone flag : %d\n", zone.flag);
printf("x : %d\n", zone.x_point);
if(zone.flag == target.flag)
{
// 가운데 있다고 판단 되면
if(zone.x_point < maxX && zone.x_point >minX)
{
kickFlag = 1;
printf("kickFlag = 1\n");
}
else if(zone.x_point <=minX)
{
for(i=0; i<1; i++){
rotateOnRight();
usleep(200000);
turnCnt--;
}
}
else if(zone.x_point >=maxX)
{
for(i=0;i<1; i++){
rotateOnLeft();
usleep(200000);
turnCnt++;
}
}
}
else if(zone.flag == 4) // see left
{
for(i=0;i<1; i++){
rotateOnLeft();
usleep(200000);
turnCnt++;
}
}
else if(zone.flag == 5) // see right
{
for(i=0; i<1; i++){
rotateOnRight();
usleep(200000);
turnCnt--;
}
}
else if(zone.flag == 6)
{
printf("zone not found\n");
}
// 세부조정
while(1){
printf("see down\n");
// 내려다 보기
seeDown();
usleep(200000); // delay 1sec, camera buffer update
// 내려다 본 결과로 판단
Target obj = findTarget(&bufCopy, target.flag);
printf("x : %d, y : %d, flag : %d\n", obj.x_point, obj.y_point, obj.flag);
printf("see up\n");
// 몸을 다시 일으키고
seeUp();
usleep(200000);
// 못 찾았을 경우에 대한 예외처리가 없음 현재는
if(obj.flag == target.flag){
if(obj.x_point < detail_minX)
{
rightOneStep();
usleep(200000);
}
else if(obj.x_point >detail_maxX)
{
leftOneStep();
usleep(200000);
}
else if(obj.y_point > detail_Y)
{
frontOneStep();
usleep(200000);
}
else
{
/*
if(kickFlag == 0){
gathered_Touch(4); // 아무의미없는 인자
usleep(200000);
}
*/
break;
}
}
}
// 킥 플래그 서 있으면 찬다
if(kickFlag == 1)
{
gathered_Touch(PLASTIC);
usleep(200000);
left_Kick();
usleep(200000);
// 물체앞까지 의무걸음
moveStraight();
usleep(5500000);
moveStop();
usleep(200000);
kickCnt++;
if(plastic_Start_Pos == 2)
{
if(kickCnt >= side_Cnt)
break;
}
else if(plastic_Start_Pos == 1)
{
if(kickCnt >= straight_Cnt)
break;
}
}
if(kickCnt == 1)
{
if(abs(turnCnt) >= sideFlag_Cnt)
{
plastic_Start_Pos = 2; // 먼 곳에서 시작
}
else
{
plastic_Start_Pos = 1; // 가까운곳, 즉 가운데서 시작
}
}
}
}
int * PlaceJoueur::placeJoueur(int ** carte, int largeur,int hauteur, int* peuple, int nbJoueurs)
{
vector< vector<Coordonnees> > tabZones;
for(int i=0;i<nbJoueurs;i++)
{
vector<Coordonnees> tmp = findZone(carte,largeur,hauteur,peuple[i]);
tabZones.push_back(tmp);
}
vector<Coordonnees> zoneAccessible = getUnion(tabZones);
int * coord = new int[2*nbJoueurs];
Coordonnees coordJ1, coordJ2;
int max=0;
AStar * star = new AStar();
vector<Coordonnees>::iterator it = zoneAccessible.begin();
for(;it!=zoneAccessible.end();it++)
{
vector<Coordonnees>::iterator it2 = zoneAccessible.begin();
for(;it2!=zoneAccessible.end();it2++)
{
if (!(*it==*it2))
{
if (verif[peuple[0]][carte[(*it).x()][(*it).y()]])
{
vector<Node*>* res = star->pathFinding(tabToVecor(carte,largeur,hauteur),peuple[0],largeur,hauteur,(*it),(*it2));
if (res->size()>max)
{
max=res->size();
coordJ1 = (*it);
coordJ2 = (*it2);
}
}
}
}
}
coord[0]=coordJ1.x();
coord[1]=coordJ1.y();
coord[2] = coordJ2.x();
coord[3] = coordJ2.y();
Coordonnees coordJ3;
if (nbJoueurs>=3)
{
bool ok = false;
int sigmaRef = 2;
while (!ok)
{
double max=0;
vector<Coordonnees>::iterator it = zoneAccessible.begin();
for(;it!=zoneAccessible.end();it++)
{
if (!((*it) == coordJ1 || (*it) == coordJ2))
{
if (verif[peuple[2]][carte[(*it).x()][(*it).y()]])
{
vector<Node*>* res = star->pathFinding(tabToVecor(carte,largeur,hauteur),peuple[2],largeur,hauteur,(*it),(coordJ1));
vector<Node*>* res2 = star->pathFinding(tabToVecor(carte,largeur,hauteur),peuple[2],largeur,hauteur,(*it),(coordJ2));
if (std::abs((int)res->size()-(int)res2->size())<=2 && (res->size()+res2->size())/2>max)
{
max=(res->size()+res2->size())/2;
coordJ3 = (*it);
ok = true;
}
}
}
}
sigmaRef++;
}
coord[4] = coordJ3.x();
coord[5] = coordJ3.y();
if (nbJoueurs==4)
{
Coordonnees coordJ4;
bool ok = false;
int sigmaRef = 3;
while (!ok)
{
double max=0;
vector<Coordonnees>::iterator it = zoneAccessible.begin();
for(;it!=zoneAccessible.end();it++)
{
if (!((*it) == coordJ1 || (*it) == coordJ2 || (*it) == coordJ3))
{
if (verif[peuple[3]][carte[(*it).x()][(*it).y()]])
{
vector<Node*>* res = star->pathFinding(tabToVecor(carte,largeur,hauteur),peuple[3],largeur,hauteur,(*it),(coordJ1));
vector<Node*>* res2 = star->pathFinding(tabToVecor(carte,largeur,hauteur),peuple[3],largeur,hauteur,(*it),(coordJ2));
vector<Node*>* res3 = star->pathFinding(tabToVecor(carte,largeur,hauteur),peuple[3],largeur,hauteur,(*it),(coordJ3));
int sigtmp1 = std::abs((int)res->size()-(int)res2->size());
int sigtmp2 = std::abs((int)res2->size()-(int)res3->size());
int sigtmp3 = std::abs((int)res->size()-(int)res3->size());
if ((sigtmp1+sigtmp2+sigtmp3)/3<=sigmaRef && (res->size()+res2->size()+res3->size())/3>max)
{
max=(res->size()+res2->size()+res3->size())/3;
coordJ4 = (*it);
ok = true;
}
}
}
}
sigmaRef++;
}
coord[6] = coordJ4.x();
coord[7] = coordJ4.y();
}
}
delete star;
/*
int nbJoueur = compteJoueurs(tabJoueurs,largeur,hauteur);
srand (time(NULL));
int * coord = new int[2];
if (nbJoueur==0)
{
coord[0]=rand()%2*(largeur-1);
coord[1]=rand()%2*(hauteur-1);
int * coord2 = new int[2];
coord2[0] = (((largeur-1)-coord[0])/4)*4;
coord2[1] = (((hauteur-1)-coord[1])/4)*4;
AStar * star = new AStar();
vector<Node*>* res = star->pathFinding(tabToVecor(carte,largeur,hauteur),peuple,largeur,hauteur,Coordonnees(coord[0],coord[1]),Coordonnees(coord2[0],coord2[1]));
delete star;
coord[0]=res->back()->getCoord().x();
coord[1]=res->back()->getCoord().y();
delete res;
}
else if(nbJoueur==1)
{
coord = getCoordJoueur1(tabJoueurs,largeur,hauteur);
int * coord2 = new int[2];
coord2[0] = (((largeur-1)-coord[0])/4)*4;
coord2[1] = (((hauteur-1)-coord[1])/4)*4;
AStar * star = new AStar();
vector<Node*>* res = star->pathFinding(tabToVecor(carte,largeur,hauteur),peuple,largeur,hauteur,Coordonnees(coord[0],coord[1]),Coordonnees(coord2[0],coord2[1]));
delete star;
coord[0]=res->back()->getCoord().x();
coord[1]=res->back()->getCoord().y();
delete res;
}
else
{
do
{
coord[0]=rand()%2*(largeur-1);
coord[1]=rand()%2*(hauteur-1);
}
while (tabJoueurs[coord[0]][coord[1]]!=0);
}
*/
return coord;
}
