#ifndef _TURN_
#define _TURN_
#include "HardwareInfo.c"
#include <SetSysTime.h>
#include <GetCompassB.h>
#include <GetSysTime.h>
#include <SetMotor.h>
#include <GetCompoI.h>

void turn(unsigned char jGuan)
{
    unsigned int degree = 0;   //指南針返回值
    unsigned long t = 0;   //系統時間
    unsigned char fy3 = 0;   //當前3號復眼通道
    unsigned char fy4 = 0;   //當前4號復眼通道
    unsigned char fy5 = 0;   //當前5號復眼通道
    unsigned int p1 = 45;   //場定義角
    unsigned int p2 = 315;   //場定義角
    t=0;
    SetSysTime(0);
    while ( ((degree>5)&&(degree<355))  )
    {
        degree = GetCompassB(_COMPASS_jd_);
        t = GetSysTime();
        if ( t>100 )
        {
            break;
        }
    }
    SetMotor(_MOTOR_left_, 2, 100);
    SetMotor(_MOTOR_right_, 2, 80);
    degree = GetCompassB(_COMPASS_jd_);
    t=0;
    SetSysTime(0);
    fy3 = GetCompoI(_COMPOUNDEYE_fy_, 3);
    fy4 = GetCompoI(_COMPOUNDEYE_fy_, 4);
    fy5 = GetCompoI(_COMPOUNDEYE_fy_, 5);
    while ( ((fy3<jGuan||fy4<jGuan || fy5 <jGuan) && ((degree<p1) ||(degree>p2))) && (t<60) )
    {
        degree = GetCompassB(_COMPASS_jd_);
        fy3 = GetCompoI(_COMPOUNDEYE_fy_, 3);
        fy4 = GetCompoI(_COMPOUNDEYE_fy_, 4);
        fy5 = GetCompoI(_COMPOUNDEYE_fy_, 5);
        t = GetSysTime();
    }
    t=0;
    SetSysTime(0);
}
#endif