#ifndef _COMEBACK_
#define _COMEBACK_
#include "HardwareInfo.c"
#include "turnAngle.c"
#include <GetUltrasound.h>
#include <SetMotor.h>
#include "returnOut.c"
#include <SetCentiS.h>

void comeBack(unsigned char yfy, unsigned int middleDistance, unsigned int distance)
{
    unsigned int distance_middle = 0;   //裝在機器人后面的超聲波測距模塊的返回變量值。
    unsigned char fyMax = 0;   //當前復眼所有通道的最大光值。
    unsigned char i = 0;   //returnOut()的返回值。
    turnAngle(0, yfy);
    for (int _FOR_0_ = 0; _FOR_0_ < 3; _FOR_0_++ )
    {
        distance_middle = GetUltrasound(_ULTRASOUND_middle_);
        if ( distance_middle<100 )
        {
            break;
        }
    }
    if ( distance_middle>distance )
    {
        while ( distance_middle>distance )
        {
            SetMotor(_MOTOR_left_, 2, 100);
            SetMotor(_MOTOR_right_, 2, 56);
            distance_middle = GetUltrasound(_ULTRASOUND_middle_);
            while ( distance_middle==888 )
            {
                distance_middle = GetUltrasound(_ULTRASOUND_middle_);
                i = returnOut(yfy);
                if ( i==1 )
                {
                    return ;
                }
            }
            i = returnOut(yfy);
            if ( i==1 )
            {
                return ;
            }
        }
        SetMotor(_MOTOR_left_, 0, 100);
        SetMotor(_MOTOR_right_, 0, 100);
        SetCentiS(26);
    }
    else
    {
        if ( distance_middle<middleDistance )
        {
            while ( distance_middle<distance )
            {
                SetMotor(_MOTOR_left_, 0, 40);
                SetMotor(_MOTOR_right_, 0, 40);
                distance_middle = GetUltrasound(_ULTRASOUND_middle_);
                while ( distance_middle==888 )
                {
                    distance_middle = GetUltrasound(_ULTRASOUND_middle_);
                    i = returnOut(yfy);
                    if ( i==1 )
                    {
                        return ;
                    }
                }
                i = returnOut(yfy);
                if ( i==1 )
                {
                    return ;
                }
            }
            SetMotor(_MOTOR_left_, 2, 100);
            SetMotor(_MOTOR_right_, 2, 70);
            SetCentiS(20);
        }
        else
        {
            turnAngle(0, yfy);
            SetMotor(_MOTOR_left_, 1, 0);
            SetMotor(_MOTOR_right_, 1, 0);
        }
    }
}
#endif