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();
}
}
