void Master::checkDistance(float linear, float angular){ if (linear > 0) { forward(); errorMsg(0); //return ; } else if (linear < 0){ backward(); errorMsg(1); //return ; } if (angular < 0){ turn_Right(); errorMsg(2); //return ; } else if (angular > 0) { turn_Left(); errorMsg(3); //return ; } if( (linear == 0) && (angular == 0) ){ ok = true; } }
void milk_Turning() { // 양수는 오른쪽으로 회전했었다 // 음수는 왼쪽 int i; if(redZoneCnt > 0) { /*for(i=0; i <3; i++) { turn_Right(); usleep(200000); }*/ turn_Right_60(); usleep(200000); turn_Right_45(); usleep(200000); go_Plastic(); usleep(3800000); } else if(blueZoneCnt > 0) { for(i=0; i <3; i++) { turn_Right_45(); usleep(500000); } go_Plastic(); usleep(3500000); } else { for(i=0; i <2; i++) { turn_Right_90(); usleep(500000); } go_Plastic(); usleep(3000000); } // 의무 보행하여 가운데쪽으로 좀 나오자 //moveStraight(); //usleep(8000000); // 15 걸음 의도 moveStop(); usleep(200000); /* for(i=0; i <2; i++) { turn_Left(); usleep(200000); } // 의무 보행하여 가운데쪽으로 좀 나오자 moveStraight(); usleep(4000000); // moveStop(); usleep(200000); */ // 여기서부터 재탐색 int isFind = 0; isFind = left_front_right_search(MILK); if(isFind == 0) { turn_Right(); usleep(200000); turn_Right(); usleep(200000); go_Plastic(); usleep(2000000); moveStop(); usleep(200000); isFind = left_front_right_search(MILK); } if(isFind == 0) { turn_Right(); usleep(200000); go_Plastic(); usleep(2000000); moveStop(); usleep(200000); isFind = left_front_right_search(MILK); } }
void plastic_Turning() { int i; if(turnCnt <= -1) // 내가 왼쪽에서 시작했다고 판단 { for(i=0; i <4; i++) { turn_Right(); usleep(200000); } // turn_Right_60(); // usleep(200000); go_Plastic(); usleep(2700000); // moveStop(); usleep(200000); } else if(turnCnt >= 1) // 내가 오른쪽에서 시작했다고 판단 { for(i=0; i < 4; i++) { turn_Left(); usleep(200000); } // turn_Left_60(); // usleep(200000); go_Plastic(); usleep(2700000); // moveStop(); usleep(200000); } else { music_D(); for(i=0; i < 2; i++) { turn_Left_90(); usleep(500000); } go_Plastic(); usleep(3200000); // moveStop(); usleep(200000); } Target second_Plastic; int isfind = 0; // 가운데라면 if(turnCnt == 0) { isfind = left_front_right_search(PLASTIC); if(isfind == 1 || isfind == 5) { go_Plastic(); usleep(1000000); moveStop(); usleep(200000); } } else if(turnCnt <0) { isfind = left_front_right_search(PLASTIC); if(isfind ==0){ turn_Left_45(); usleep(200000); go_Plastic(); usleep(1000000); moveStop(); usleep(200000); int isfind2 = left_front_right_search(PLASTIC);; if(isfind2 > 0) { isfind = 4; } } } else if(turnCnt >0) { isfind = left_front_right_search(PLASTIC); if(isfind ==0){ turn_Right_45(); usleep(200000); go_Plastic(); usleep(1000000); moveStop(); usleep(200000); int isfind2 = left_front_right_search(PLASTIC);; if(isfind2 > 0) { isfind = 4; } } } if(isfind == 0) { left_Kick(); } }