#ifndef _TURNANGLE_
#define _TURNANGLE_
#include "HardwareInfo.c"
#include <GetCompassB.h>
#include <SetMotor.h>
#include "returnOut.c"
#include <SetCentiS.h>

void turnAngle(unsigned int angle, unsigned char yfy)
{
    unsigned int counter = 0;   //計數值。
    unsigned int angle2 = 0;   //存儲角度變量的值
    unsigned int degree = 0;   //指南針返回值。
    unsigned char rotate = 0;   //轉左？
    unsigned char i = 0;   //returnOut返回值
    while ( counter<2 )
    {
        degree = GetCompassB(_COMPASS_jd_);
        if ( degree>360 )
        {
            counter=0;
        }
        else
        {
            counter=counter+1;
        }
    }
    if ( angle>=0&&angle<180 )
    {
        angle2=angle+180;
    }
    else
    {
        angle2=angle-180;
    }
    if ( ((degree>angle) && (degree< angle2) && (angle<angle2)) || (((degree>angle)&&(degree<=360)||(degree>=0)&&(degree<angle2)) && (angle>angle2)) )
    {
        rotate=1;
    }
    if ( rotate==1 )
    {
        SetMotor(_MOTOR_left_, 2, 45);
        SetMotor(_MOTOR_right_, 0, 42);
    }
    else
    {
        SetMotor(_MOTOR_left_, 0, 35);
        SetMotor(_MOTOR_right_, 2, 15);
    }
    while (1)
    {
        counter=0;
        while ( counter<2 )
        {
            degree = GetCompassB(_COMPASS_jd_);
            if ( degree>360 )
            {
                counter=0;
            }
            else
            {
                counter=counter+1;
            }
        }
        if ( rotate>0 )
        {
            if ( (degree<=angle) && (angle-degree)>0 && (angle-degree)<5 )
            {
                break;
            }
            else
            {
                if ( ((360-degree+angle)>0) && ((360-degree+angle)<5) )
                {
                    break;
                }
            }
        }
        else
        {
            if ( (degree>=angle)&&((degree-angle)>0 && (degree-angle)<5) )
            {
                break;
            }
            else
            {
                if ( ((360-angle+degree)>0)&&((360-angle+degree)<5) )
                {
                    break;
                }
            }
        }
        i = returnOut(yfy);
        if ( i==1 )
        {
            return ;
        }
    }
    if ( rotate==1 )
    {
        SetMotor(_MOTOR_left_, 0, 50);
        SetMotor(_MOTOR_right_, 2, 50);
    }
    else
    {
        SetMotor(_MOTOR_left_, 2, 50);
        SetMotor(_MOTOR_right_, 0, 50);
    }
    SetCentiS(10);
    SetMotor(_MOTOR_left_, 1, 0);
    SetMotor(_MOTOR_right_, 1, 0);
}
#endif