overhalfoftheballs.c

来自「机器人足球教程footballteaching」· C语言 代码 · 共 58 行

#ifndef _OVERHALFOFTHEBALLS_
#define _OVERHALFOFTHEBALLS_
#include "HardwareInfo.c"
#include <GetCompoI.h>
#include <GetADScable.h>
#include <GetCompassB.h>
#include <GetSysTime.h>
#include "TurnAngle.c"
#include <SetMotor.h>
#include <SetSysTime.h>

void OverHalfOfTheBalls(unsigned char GuanKan)
{
    unsigned char GuanMax = 0;   //当前所有通道的最大光值
    unsigned int b = 0;   //当前角度值
    unsigned long t = 0;   //当前系统时间
    unsigned char f = 0;   //当前灰度测量灰度值
    unsigned char ms = 0;   //是否处于到另半场找球的途中，0为从黑到白途中，2为从白到黑途中，100为不在途中
    GuanMax = GetCompoI(_COMPOUNDEYE_Guang_, 9);
    f = GetADScable(_SCABLEAD_FD_);
    b = GetCompassB(_COMPASS_1_);
    t = GetSysTime();
    if ( t>150 )
    {
        if ( f<80 )
        {
            ms=0;
            TurnAngle(0);
        }
        else
        {
            ms=2;
            TurnAngle(180);
        }
        SetMotor(_MOTOR_left_, 0, 100);
        SetMotor(_MOTOR_right_, 0, 100);
        SetSysTime(0);
        t=0;
        while ( (((f<80)&&(ms==0))||((f>50)&&(ms==2))) )
        {
            f = GetADScable(_SCABLEAD_FD_);
            t = GetSysTime();
            GuanMax = GetCompoI(_COMPOUNDEYE_Guang_, 9);
            if ( (GuanMax>GuanKan)||(t>150) )
            {
                break;
            }
        }
        SetMotor(_MOTOR_left_, 0, 50);
        SetMotor(_MOTOR_right_, 2, 50);
        t = 0;
        SetSysTime(0);
        ms=100;
    }
}
#endif