defengsivefootball.c

一个机器人提足球的代码!基于神经网络! 一个机器人提足球的代码!基于神经网络!

#include "HardwareInfo.c"
#include <GetCompoI.h>
#include <SetMotor.h>
#include "turnAngle.c"
#include <GetUltrasound.h>
#include "goline.c"
#include "returnOut.c"
#include "comeBack.c"

int main(void)
{
    unsigned char fyMax = 0;   //当前复眼所有通道的最大光值。
    unsigned char maxIndex = 0;   //当前复眼最大光值的通道号。
    unsigned int distance_middle = 0;   //装在机器人后面的超声波测距模块的返回变量值。
    unsigned int distance_left = 0;   //装在机器人左面的超声波测距模块的返回变量值。
    unsigned char yfy = 70;   //远距离的光值，一般是指无球情况，用户要实测。
    unsigned char jfy = 140;   //近距离的光值，是指机器人与球距离多远的光值，用户要实测。
    unsigned int dis_middle = 30;   //机器人从场边走到场中央的距离，用户要实测。
    unsigned int dis_left = 29;   //机器人离球场边的距离，用户要实测。
    unsigned int dis_right = 40;   //机器人离球场边的距离，用户要实测。
    unsigned char sr = 0;   //右马达控制命令字，若为0，向前转，为1则停止。
    unsigned char sl = 0;   //右马达控制命令字，若为0，向前转，为1则停止。
    unsigned char i = 0;   //returnOut()的返回值。
    unsigned char fyMin = 0;   //当前复眼所有通道的最小光值。
    unsigned int distance = 31;   //转某角度直走，并走一段距离。
    unsigned char value = 50;   //复眼通道的最大值-复眼通道的最小值，其作用：判断是否发现球。
    while (1)
    {
        fyMax = GetCompoI(_COMPOUNDEYE_fy_, 9);
        fyMin = GetCompoI(_COMPOUNDEYE_fy_, 11);
        if ( (fyMax>yfy)||((fyMax-fyMin)>value) )
        {
            if ( fyMax>jfy )
            {
                maxIndex = GetCompoI(_COMPOUNDEYE_fy_, 8);
                if ( maxIndex==7 )
                {
                    SetMotor(_MOTOR_left_, 0, 80);
                    SetMotor(_MOTOR_right_, 2, 50);
                }
                else
                {
                    if ( maxIndex==6 )
                    {
                        SetMotor(_MOTOR_left_, 0, 80);
                        SetMotor(_MOTOR_right_, 2, 20);
                    }
                    else
                    {
                        if ( maxIndex==5 )
                        {
                            SetMotor(_MOTOR_left_, 0, 50);
                            SetMotor(_MOTOR_right_, 1, 100);
                        }
                        else
                        {
                            if ( maxIndex==1 )
                            {
                                SetMotor(_MOTOR_left_, 2, 50);
                                SetMotor(_MOTOR_right_, 0, 100);
                            }
                            else
                            {
                                if ( maxIndex==2 )
                                {
                                    SetMotor(_MOTOR_left_, 2, 20);
                                    SetMotor(_MOTOR_right_, 0, 100);
                                }
                                else
                                {
                                    if ( maxIndex==3 )
                                    {
                                        SetMotor(_MOTOR_left_, 1, 100);
                                        SetMotor(_MOTOR_right_, 0, 50);
                                    }
                                    else
                                    {
                                        SetMotor(_MOTOR_left_, 0, 100);
                                        SetMotor(_MOTOR_right_, 0, 70);
                                    }
                                }
                            }
                        }
                    }
                }
            }
            else
            {
                maxIndex = GetCompoI(_COMPOUNDEYE_fy_, 8);
                if ( maxIndex<4 )
                {
                    sl=1;
                    sr=0;
                    SetMotor(_MOTOR_left_, sl, 100);
                    SetMotor(_MOTOR_right_, sr, 100);
                }
                else
                {
                    if ( maxIndex>4 )
                    {
                        sl=0;
                        sr=1;
                        SetMotor(_MOTOR_left_, sl, 100);
                        SetMotor(_MOTOR_right_, sr, 100);
                    }
                    else
                    {
                        sl=0;
                        sr=0;
                        SetMotor(_MOTOR_left_, sl, 100);
                        SetMotor(_MOTOR_right_, sr, 100);
                    }
                }
            }
        }
        else
        {
            turnAngle(0, yfy);
            for (int _FOR_0_ = 0; _FOR_0_ < 3; _FOR_0_++ )
            {
                distance_left = GetUltrasound(_ULTRASOUND_left_);
                if ( distance_left<110 )
                {
                    break;
                }
            }
            for (int _FOR_1_ = 0; _FOR_1_ < 3; _FOR_1_++ )
            {
                distance_middle = GetUltrasound(_ULTRASOUND_middle_);
                if ( distance_middle<=110 )
                {
                    break;
                }
            }
            if ( distance_left<dis_left )
            {
                while ( (distance_middle<dis_middle) )
                {
                    goline(7, yfy, 90, distance);
                    distance_middle = GetUltrasound(_ULTRASOUND_middle_);
                    while ( distance_middle==888 )
                    {
                        distance_middle = GetUltrasound(_ULTRASOUND_middle_);
                        i = returnOut(yfy);
                        if ( i==1 )
                        {
                            break;
                        }
                    }
                    i = returnOut(yfy);
                    if ( i==1 )
                    {
                        break;
                    }
                }
                turnAngle(0, yfy);
            }
            else
            {
                if ( distance_left>dis_right )
                {
                    while ( (distance_middle<dis_middle) )
                    {
                        goline(7, yfy, 270, distance);
                        distance_middle = GetUltrasound(_ULTRASOUND_middle_);
                        while ( distance_middle==888 )
                        {
                            distance_middle = GetUltrasound(_ULTRASOUND_middle_);
                            i = returnOut(yfy);
                            if ( i==1 )
                            {
                                break;
                            }
                        }
                        i = returnOut(yfy);
                        if ( i==1 )
                        {
                            break;
                        }
                    }
                    turnAngle(0, yfy);
                }
            }
            comeBack(yfy, 7, 17);
            turnAngle(0, yfy);
        }
    }
    return 1;
}