zjd.c

机器人足球教程footballteaching

#ifndef _ZJD_
#define _ZJD_
#include "HardwareInfo.c"
#include <GetCompassB.h>
#include "GetNewAngle.c"
#include <SetMotor.h>
#include <SetCentiS.h>

void zjd(unsigned int angle)
{
    unsigned int dg = 0;
    unsigned char a = 0;
    unsigned char b = 0;
    unsigned long shijian = 5;
    angle = angle + 360;
    while (1)
    {
        dg = GetCompassB(_COMPASS_1_);
        if ( dg > 360 )
        {
            continue;
        }
        dg = GetNewAngle(angle, dg);
        if ( dg==360&&b<10 )
        {
            b=b+1;
            continue;
        }
        if ( ((dg<(angle-5))&&(dg>(angle-180)))&&(a==0) )
        {
            a=1;
        }
        else
        {
            if ( ((dg<(angle+180))&&(dg>(angle+5)))&&(a==0) )
            {
                a=2;
            }
            else
            {
                if ( ((dg<(angle+5))&&(dg>(angle-5)))&&(a==0) )
                {
                    a=3;
                }
            }
        }
        if ( (dg<(angle+5))&&(dg>(angle-5)) )
        {
            break;
        }
        else
        {
            if ( (dg >= (angle - 90)) && (dg <(angle-5)) )
            {
                SetMotor(_MOTOR_zuo_, 0, 80);
                SetMotor(_MOTOR_you_, 2, 80);
            }
            else
            {
                if ( dg >= (angle - 180) && dg < (angle -90) )
                {
                    SetMotor(_MOTOR_zuo_, 0, 100);
                    SetMotor(_MOTOR_you_, 2, 100);
                }
                else
                {
                    if ( (dg > (angle+5)) && dg <= (angle + 90) )
                    {
                        SetMotor(_MOTOR_zuo_, 2, 80);
                        SetMotor(_MOTOR_you_, 0, 80);
                    }
                    else
                    {
                        if ( dg > (angle + 90) && dg <= (angle + 180) )
                        {
                            SetMotor(_MOTOR_zuo_, 2, 100);
                            SetMotor(_MOTOR_you_, 0, 100);
                        }
                    }
                }
            }
        }
    }
    if ( a == 1 )
    {
        SetMotor(_MOTOR_zuo_, 2, 100);
        SetMotor(_MOTOR_you_, 0, 100);
        SetCentiS(shijian);
    }
    else
    {
        if ( a==2 )
        {
            SetMotor(_MOTOR_zuo_, 0, 100);
            SetMotor(_MOTOR_you_, 2, 100);
            SetCentiS(shijian);
        }
    }
    SetMotor(_MOTOR_zuo_, 1, 100);
    SetMotor(_MOTOR_you_, 1, 100);
}
#endif