deathhandle.c

机器人足球教程footballteaching

#include "HardwareInfo.c"
#include <GetCompoI.h>
#include <GetCompassB.h>
#include <GetSysTime.h>
#include <SetLED.h>
#include <SetMotor.h>
#include <SetCentiS.h>
#include <SetTenthS.h>
#include <SetSysTime.h>

int main(void)
{
    unsigned char GuanMax = 0;   //当前所有通道的最大光值
    unsigned char Guan3 = 0;   //当前3号通道的光值
    unsigned char GuanKan = 60;   //远距离光值，一般是没有球的情况下的光值，再加上3-5就好了。
    unsigned char Guan4 = 0;   //当前4号通道光值
    unsigned char Guan5 = 0;   //当前5号通道光值
    unsigned char MaxNo = 0;   //当前最大光值的通道号
    unsigned int b = 0;   //当前角度值
    unsigned long t = 0;   //当前系统时间
    unsigned char JGuan = 246;   //近光值，贴近球的光值。
    while (1)
    {
        MaxNo = GetCompoI(_COMPOUNDEYE_Guang_, 8);
        GuanMax = GetCompoI(_COMPOUNDEYE_Guang_, 9);
        Guan3 = GetCompoI(_COMPOUNDEYE_Guang_, 3);
        Guan4 = GetCompoI(_COMPOUNDEYE_Guang_, 4);
        Guan5 = GetCompoI(_COMPOUNDEYE_Guang_, 5);
        b = GetCompassB(_COMPASS_1_);
        t = GetSysTime();
        while ( ((Guan3>=JGuan)||(Guan4>=JGuan)||(Guan5>=JGuan)) )
        {
            MaxNo = GetCompoI(_COMPOUNDEYE_Guang_, 8);
            GuanMax = GetCompoI(_COMPOUNDEYE_Guang_, 9);
            Guan3 = GetCompoI(_COMPOUNDEYE_Guang_, 3);
            Guan4 = GetCompoI(_COMPOUNDEYE_Guang_, 4);
            Guan5 = GetCompoI(_COMPOUNDEYE_Guang_, 5);
            t = GetSysTime();
            b = GetCompassB(_COMPASS_1_);
            if ( t>=200 )
            {
                SetLED(_LED_1_, 1);
                if ( ((b>=0)&&(b<90))  )
                {
                    SetMotor(_MOTOR_left_, 2, 100);
                    SetMotor(_MOTOR_right_, 0, 50);
                    SetCentiS(45);
                    SetMotor(_MOTOR_left_, 0, 100);
                    SetMotor(_MOTOR_right_, 0, 42);
                    SetTenthS(4);
                }
                else
                {
                    if ( ((b>=90)&&(b<180))  )
                    {
                        SetMotor(_MOTOR_left_, 2, 100);
                        SetMotor(_MOTOR_right_, 0, 100);
                        SetTenthS(2);
                    }
                    else
                    {
                        if ( b>=270 )
                        {
                            SetMotor(_MOTOR_left_, 0, 50);
                            SetMotor(_MOTOR_right_, 2, 100);
                            SetCentiS(23);
                            SetMotor(_MOTOR_left_, 0, 52);
                            SetMotor(_MOTOR_right_, 0, 100);
                            SetTenthS(4);
                        }
                        else
                        {
                            if ( (b>=180)&&(b<270) )
                            {
                                SetMotor(_MOTOR_left_, 0, 100);
                                SetMotor(_MOTOR_right_, 2, 100);
                                SetTenthS(2);
                            }
                        }
                    }
                }
                SetSysTime(0);
                t=0;
                break;
            }
        }
        SetLED(_LED_1_, 0);
        if ( GuanMax>GuanKan )
        {
            SetSysTime(0);
            if ( MaxNo<3 )
            {
                SetMotor(_MOTOR_left_, 1, 12);
                SetMotor(_MOTOR_right_, 0, 70);
            }
            else
            {
                if ( MaxNo==3 )
                {
                    SetMotor(_MOTOR_left_, 1, 5);
                    SetMotor(_MOTOR_right_, 0, 40);
                }
                else
                {
                    if ( MaxNo>5 )
                    {
                        SetMotor(_MOTOR_left_, 0, 70);
                        SetMotor(_MOTOR_right_, 1, 10);
                    }
                    else
                    {
                        if ( MaxNo==5 )
                        {
                            SetMotor(_MOTOR_left_, 0, 40);
                            SetMotor(_MOTOR_right_, 1, 5);
                        }
                        else
                        {
                            SetMotor(_MOTOR_left_, 0, 100);
                            SetMotor(_MOTOR_right_, 0, 100);
                        }
                    }
                }
            }
        }
        else
        {
            SetMotor(_MOTOR_left_, 0, 45);
            SetMotor(_MOTOR_right_, 2, 45);
        }
    }
    return 1;
}