dualcompoundeye.c

机器人足球教程footballteaching

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

void DualCompoundEye(unsigned char guangkan)
{
    unsigned int chao = 0;   //右边超声测距模块的返回值
    unsigned char qianhao = 0;   //装在机器人前面复眼最大值的通道号
    unsigned int b = 0;   //指南针的返回值
    unsigned long t = 0;   //系统时间
    unsigned char houda = 0;   //装在机器人后面复眼的最大值
    unsigned char qianda = 0;   //装在机器人前面复眼的最大值
    unsigned long time = 50;   //所设的系统时间
    unsigned int dist = 60;   //机器人与场边的距离，需实测。
    b = GetCompassB(_COMPASS_1_);
    if ( b>140&&b<220 )
    {
        qianhao = GetCompoI(_COMPOUNDEYE_qian_, 8);
        if ( qianhao==7 )
        {
            SetSysTime(0);
            while (1)
            {
                SetMotor(_MOTOR_zuo_, 0, 100);
                SetMotor(_MOTOR_you_, 2, 10);
                qianhao = GetCompoI(_COMPOUNDEYE_qian_, 8);
                t = GetSysTime();
                if ( qianhao==6||t>time )
                {
                    break;
                }
            }
        }
        else
        {
            if ( qianhao==5 )
            {
                SetSysTime(0);
                while (1)
                {
                    SetMotor(_MOTOR_zuo_, 2, 10);
                    SetMotor(_MOTOR_you_, 0, 100);
                    qianhao = GetCompoI(_COMPOUNDEYE_qian_, 8);
                    t = GetSysTime();
                    if ( qianhao==6||t>time )
                    {
                        break;
                    }
                }
            }
            else
            {
                if ( qianhao==3 )
                {
                    SetSysTime(0);
                    while (1)
                    {
                        SetMotor(_MOTOR_zuo_, 0, 100);
                        SetMotor(_MOTOR_you_, 2, 10);
                        qianhao = GetCompoI(_COMPOUNDEYE_qian_, 8);
                        t = GetSysTime();
                        if ( qianhao==2||t>time )
                        {
                            break;
                        }
                    }
                }
                else
                {
                    if ( qianhao==1 )
                    {
                        SetSysTime(0);
                        while (1)
                        {
                            SetMotor(_MOTOR_zuo_, 2, 10);
                            SetMotor(_MOTOR_you_, 0, 100);
                            qianhao = GetCompoI(_COMPOUNDEYE_qian_, 8);
                            t = GetSysTime();
                            if ( qianhao==2||t>time )
                            {
                                break;
                            }
                        }
                    }
                    else
                    {
                        if ( qianhao==4 )
                        {
                            zjd(180);
                            chao = GetUltrasound(_ULTRASOUND_chao_);
                            if ( chao<=dist )
                            {
                                SetSysTime(0);
                                while ( qianhao!=5&&t<time )
                                {
                                    SetMotor(_MOTOR_zuo_, 2, 30);
                                    SetMotor(_MOTOR_you_, 0, 100);
                                    qianhao = GetCompoI(_COMPOUNDEYE_qian_, 8);
                                    t = GetSysTime();
                                }
                            }
                            else
                            {
                                SetSysTime(0);
                                while ( qianhao!=3&&t<time )
                                {
                                    SetMotor(_MOTOR_zuo_, 0, 100);
                                    SetMotor(_MOTOR_you_, 2, 30);
                                    qianhao = GetCompoI(_COMPOUNDEYE_qian_, 8);
                                    t = GetSysTime();
                                }
                            }
                        }
                    }
                }
            }
        }
        SetSysTime(0);
        while ( houda<guangkan )
        {
            SetMotor(_MOTOR_zuo_, 0, 100);
            SetMotor(_MOTOR_you_, 0, 100);
            houda = GetCompoI(_COMPOUNDEYE_hou_, 9);
            t = GetSysTime();
            if ( t>150 )
            {
                break;
            }
        }
        t=0;
    }
}
#endif