#include "HardwareInfo.c"
#include <SetSysTime.h>
#include <GetCompoI.h>
#include <GetCompassB.h>
#include <GetSysTime.h>
#include <SetMotor.h>
#include <SetCentiS.h>
#include "TurnAngle.c"
#include <GetUltrasound.h>

int main(void)
{
    unsigned char GuanMax = 0;   //當前所有通道的最大光值
    unsigned char Guan3 = 0;   //當前3號通道的光值
    unsigned char GuanKan = 55;   //遠距離光值，一般是沒有球的情況下的光值，再加上3-5就好了。
    unsigned char Guan4 = 0;   //當前4號通道光值
    unsigned char Guan5 = 0;   //當前5號通道光值
    unsigned char MaxNo = 0;   //當前最大光值的通道號
    unsigned int b = 0;   //當前角度值
    unsigned long t = 0;   //當前系統時間
    unsigned char q1 = 115;   //場地定義角
    unsigned char q2 = 245;   //場地定義角
    unsigned int p1 = 45;   //場地定義角
    unsigned int p2 = 315;   //場地定義角
    unsigned char JGuan = 180;   //較近距離的光值閥值，用在烏龍球的處理
    unsigned int you = 0;   //當前超聲測距讀數
    SetSysTime(0);
    while (1)
    {
        MaxNo = GetCompoI(_COMPOUNDEYE_Guang_, 8);
        GuanMax = GetCompoI(_COMPOUNDEYE_Guang_, 9);
        Guan3 = GetCompoI(_COMPOUNDEYE_Guang_, 3);
        Guan4 = GetCompoI(_COMPOUNDEYE_Guang_, 4);
        Guan5 = GetCompoI(_COMPOUNDEYE_Guang_, 5);
        b = GetCompassB(_COMPASS_1_);
        t = GetSysTime();
        if ( ((Guan3>JGuan)||(Guan4>JGuan)||(Guan5>JGuan))&&((b>=q1)&&(b<=q2)) )
        {
            SetMotor(_MOTOR_left_, 2, 100);
            SetMotor(_MOTOR_right_, 2, 100);
            SetCentiS(2);
            TurnAngle(180);
            while (1)
            {
                you = GetUltrasound(_ULTRASOUND_right_);
                if ( you!=9999&&you!=0 )
                {
                    break;
                }
            }
            if ( you<=55 )
            {
                SetMotor(_MOTOR_left_, 2, 100);
                SetMotor(_MOTOR_right_, 2, 40);
            }
            else
            {
                SetMotor(_MOTOR_left_, 2, 40);
                SetMotor(_MOTOR_right_, 2, 100);
            }
            t=0;
            SetSysTime(0);
            while ( ((b>5)&&(b<355))  )
            {
                b = GetCompassB(_COMPASS_1_);
                t = GetSysTime();
                if ( t>100 )
                {
                    break;
                }
            }
            SetMotor(_MOTOR_left_, 2, 100);
            SetMotor(_MOTOR_right_, 2, 100);
            b = GetCompassB(_COMPASS_1_);
            SetSysTime(0);
            t = GetSysTime();
            Guan3 = GetCompoI(_COMPOUNDEYE_Guang_, 3);
            Guan4 = GetCompoI(_COMPOUNDEYE_Guang_, 4);
            Guan5 = GetCompoI(_COMPOUNDEYE_Guang_, 5);
            while ( ((Guan3<125||Guan4<125 || Guan5 <125) && ((b<p1) ||(b>p2))) && t<60 )
            {
                b = GetCompassB(_COMPASS_1_);
                Guan3 = GetCompoI(_COMPOUNDEYE_Guang_, 3);
                Guan4 = GetCompoI(_COMPOUNDEYE_Guang_, 4);
                Guan5 = GetCompoI(_COMPOUNDEYE_Guang_, 5);
                t = GetSysTime();
            }
            SetSysTime(0);
        }
        if ( GuanMax>GuanKan )
        {
            SetSysTime(0);
            if ( MaxNo==7 )
            {
                SetMotor(_MOTOR_left_, 0, 80);
                SetMotor(_MOTOR_right_, 1, 50);
            }
            else
            {
                if ( MaxNo==6 )
                {
                    SetMotor(_MOTOR_left_, 0, 60);
                    SetMotor(_MOTOR_right_, 1, 25);
                }
                else
                {
                    if ( MaxNo==5 )
                    {
                        SetMotor(_MOTOR_left_, 0, 40);
                        SetMotor(_MOTOR_right_, 1, 100);
                    }
                    else
                    {
                        if ( MaxNo==1 )
                        {
                            SetMotor(_MOTOR_left_, 1, 50);
                            SetMotor(_MOTOR_right_, 0, 80);
                        }
                        else
                        {
                            if ( MaxNo==2 )
                            {
                                SetMotor(_MOTOR_left_, 1, 25);
                                SetMotor(_MOTOR_right_, 0, 60);
                            }
                            else
                            {
                                if ( MaxNo==3 )
                                {
                                    SetMotor(_MOTOR_left_, 1, 100);
                                    SetMotor(_MOTOR_right_, 0, 40);
                                }
                                else
                                {
                                    SetMotor(_MOTOR_left_, 0, 100);
                                    SetMotor(_MOTOR_right_, 0, 100);
                                }
                            }
                        }
                    }
                }
            }
        }
        else
        {
            SetMotor(_MOTOR_left_, 0, 50);
            SetMotor(_MOTOR_right_, 2, 50);
        }
    }
    return 1;
}