recoverytheball.c

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

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

int main(void)
{
    unsigned char MaxValue = 0;   //当前复眼所有通道的最大值返回值
    unsigned char var0 = 65;   //复眼阀值。
    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);
        if ( MaxValue>=var0 )
        {
            SetLED(_LED_1_, 1);
            if ( MaxNo==7 )
            {
                SetMotor(_MOTOR_left_, 0, 70);
                SetMotor(_MOTOR_right_, 2, 30);
            }
            else
            {
                if ( MaxNo==6 )
                {
                    SetMotor(_MOTOR_left_, 0, 70);
                    SetMotor(_MOTOR_right_, 2, 10);
                }
                else
                {
                    if ( MaxNo==5 )
                    {
                        SetMotor(_MOTOR_left_, 0, 50);
                        SetMotor(_MOTOR_right_, 1, 100);
                    }
                    else
                    {
                        if ( MaxNo==1 )
                        {
                            SetMotor(_MOTOR_left_, 2, 30);
                            SetMotor(_MOTOR_right_, 0, 70);
                        }
                        else
                        {
                            if ( MaxNo==2 )
                            {
                                SetMotor(_MOTOR_left_, 2, 10);
                                SetMotor(_MOTOR_right_, 0, 70);
                            }
                            else
                            {
                                if ( MaxNo==3 )
                                {
                                    SetMotor(_MOTOR_left_, 1, 100);
                                    SetMotor(_MOTOR_right_, 0, 50);
                                }
                                else
                                {
                                    SetMotor(_MOTOR_left_, 0, 100);
                                    SetMotor(_MOTOR_right_, 0, 100);
                                }
                            }
                        }
                    }
                }
            }
        }
        else
        {
            SetLED(_LED_1_, 0);
            SetMotor(_MOTOR_left_, 0, 50);
            SetMotor(_MOTOR_right_, 2, 50);
        }
    }
    return 1;
}