closetotheball.c

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

#include "HardwareInfo.c"
#include <GetCompoI.h>
#include <SetLED.h>
#include <SetMotor.h>

int main(void)
{
    unsigned char MaxValue = 0;   //当前复眼所有通道的最大值返回值
    unsigned char var0 = 60;   //复眼阀值。
    unsigned char var2 = 200;   //靠近球的光值
    unsigned char MaxNo = 0;   //复眼最大值的通道号
    unsigned char NO3 = 0;   //当前复眼的第三通道号返回值
    unsigned char NO4 = 0;   //当前复眼的第四通道号返回值
    unsigned char NO5 = 0;   //当前复眼的第五通道号返回值
    while (1)
    {
        MaxValue = GetCompoI(_COMPOUNDEYE_1_, 9);
        MaxNo = GetCompoI(_COMPOUNDEYE_1_, 8);
        NO3 = GetCompoI(_COMPOUNDEYE_1_, 3);
        NO4 = GetCompoI(_COMPOUNDEYE_1_, 4);
        NO5 = GetCompoI(_COMPOUNDEYE_1_, 5);
        if ( NO3>=var2||NO4>=var2||NO5>=var2 )
        {
            SetLED(_LED_1_, 1);
            if ( MaxNo>4 )
            {
                SetMotor(_MOTOR_left_, 0, 40);
                SetMotor(_MOTOR_right_, 1, 100);
            }
            else
            {
                if ( MaxNo<4 )
                {
                    SetMotor(_MOTOR_left_, 1, 100);
                    SetMotor(_MOTOR_right_, 0, 40);
                }
                else
                {
                    SetMotor(_MOTOR_left_, 0, 100);
                    SetMotor(_MOTOR_right_, 0, 100);
                }
            }
        }
        else
        {
            if ( MaxValue>=var0 )
            {
                SetLED(_LED_1_, 0);
                if ( MaxNo==7 )
                {
                    SetMotor(_MOTOR_left_, 0, 60);
                    SetMotor(_MOTOR_right_, 2, 30);
                }
                else
                {
                    if ( MaxNo==6 )
                    {
                        SetMotor(_MOTOR_left_, 0, 60);
                        SetMotor(_MOTOR_right_, 2, 10);
                    }
                    else
                    {
                        if ( MaxNo==5 )
                        {
                            SetMotor(_MOTOR_left_, 0, 40);
                            SetMotor(_MOTOR_right_, 1, 100);
                        }
                        else
                        {
                            if ( MaxNo==1 )
                            {
                                SetMotor(_MOTOR_left_, 2, 30);
                                SetMotor(_MOTOR_right_, 0, 60);
                            }
                            else
                            {
                                if ( MaxNo==2 )
                                {
                                    SetMotor(_MOTOR_left_, 2, 10);
                                    SetMotor(_MOTOR_right_, 0, 60);
                                }
                                else
                                {
                                    if ( MaxNo==3 )
                                    {
                                        SetMotor(_MOTOR_left_, 1, 100);
                                        SetMotor(_MOTOR_right_, 0, 40);
                                    }
                                    else
                                    {
                                        SetMotor(_MOTOR_left_, 0, 100);
                                        SetMotor(_MOTOR_right_, 0, 100);
                                    }
                                }
                            }
                        }
                    }
                }
            }
            else
            {
                SetMotor(_MOTOR_left_, 0, 40);
                SetMotor(_MOTOR_right_, 2, 20);
            }
        }
    }
    return 1;
}