#ifndef _ANGLE_
#define _ANGLE_
#include "HardwareInfo.c"
#include <GetCompassB.h>
#include <SetMotor.h>
#include <SetTenthS.h>
#include <SetLCD3Char.h>

void angle(unsigned int angle)
{
    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, 30);
        SetMotor(_MOTOR_right_, 0, 27);
    }
    else
    {
        SetMotor(_MOTOR_left_, 0, 25);
        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;
                }
            }
        }
    }
    if ( rotate==1 )
    {
        SetMotor(_MOTOR_left_, 0, 40);
        SetMotor(_MOTOR_right_, 2, 40);
    }
    else
    {
        SetMotor(_MOTOR_left_, 2, 40);
        SetMotor(_MOTOR_right_, 0, 40);
    }
    SetTenthS(2);
    SetMotor(_MOTOR_left_, 1, 0);
    SetMotor(_MOTOR_right_, 1, 0);
    SetLCD3Char(1, degree);
}
#endif