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