#ifndef _COMEORBACK_
#define _COMEORBACK_
#include "HardwareInfo.c"
#include "turnAngle.c"
#include <GetUltrasound.h>
#include <SetSysTime.h>
#include <SetMotor.h>
#include <GetSysTime.h>
#include "returnOut.c"
#include <GetCompoI.h>

void comeORback(unsigned char yfy, unsigned int backDistance, unsigned int distance, unsigned long time)
{
    unsigned char fyMax = 0;   //當前所有通道的最大光值
    unsigned long t = 0;   //系統時間
    unsigned int distance_middle = 0;   //后超聲波測距模塊的返回值
    unsigned char i = 0;
    turnAngle(0, yfy);
    for (int _FOR_0_ = 0; _FOR_0_ < 4; _FOR_0_++ )
    {
        distance_middle = GetUltrasound(_ULTRASOUND_middle_);
        if ( distance_middle<100 )
        {
            break;
        }
    }
    if ( distance_middle>distance )
    {
        turnAngle(180, yfy);
        SetSysTime(0);
        t=0;
        distance_middle = GetUltrasound(_ULTRASOUND_middle_);
        while ( distance_middle<backDistance )
        {
            SetMotor(_MOTOR_left_, 0, 80);
            SetMotor(_MOTOR_right_, 0, 100);
            distance_middle = GetUltrasound(_ULTRASOUND_middle_);
            t = GetSysTime();
            i = returnOut(yfy);
            if ( (i==1)||(t>time) )
            {
                return ;
            }
        }
    }
    else
    {
        SetSysTime(0);
        t=0;
        SetMotor(_MOTOR_left_, 0, 55);
        SetMotor(_MOTOR_right_, 0, 80);
        while ( distance_middle<backDistance )
        {
            distance_middle = GetUltrasound(_ULTRASOUND_middle_);
            fyMax = GetCompoI(_COMPOUNDEYE_fy_, 9);
            t = GetSysTime();
            if ( (fyMax>yfy)||(t>time) )
            {
                return ;
            }
        }
    }
}
#endif