goangle.c

机器人足球教程footballteaching

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

void GoAngle(unsigned int angle)
{
    // extern global var
    extern unsigned char g_Speed;
    extern unsigned char g_ModifySpeed;
    extern unsigned int g_Small;
    extern unsigned int g_Big;

    unsigned int dg = 0;
    unsigned char halfSpeed = 0;
    halfSpeed = g_Speed / 2;
    angle = angle + 360;
    dg = GetCompassB(_COMPASS_1_);
    if ( dg > 360 )
    {
        return ;
    }
    dg = GetNewAngle(angle, dg);
    if ( dg == angle )
    {
        SetMotor(_MOTOR_left_, 0, g_Speed);
        SetMotor(_MOTOR_right_, 0, g_Speed);
    }
    else
    {
        if ( (dg >= (angle - g_Small)) && dg < angle )
        {
            SetMotor(_MOTOR_left_, 0, g_Speed);
            SetMotor(_MOTOR_right_, 0, g_ModifySpeed);
        }
        else
        {
            if ( dg > angle && dg <= (angle + g_Small) )
            {
                SetMotor(_MOTOR_left_, 0, g_ModifySpeed);
                SetMotor(_MOTOR_right_, 0, g_Speed);
            }
            else
            {
                if ( (dg >= (angle - g_Big)) && dg < (angle- g_Small) )
                {
                    SetMotor(_MOTOR_left_, 0, g_Speed);
                    SetMotor(_MOTOR_right_, 1, 0);
                }
                else
                {
                    if ( dg > (angle + g_Small) && dg <= (angle + g_Big) )
                    {
                        SetMotor(_MOTOR_left_, 1, 0);
                        SetMotor(_MOTOR_right_, 0, g_Speed);
                    }
                    else
                    {
                        if ( dg >= (angle - 180) && dg < (angle - g_Big) )
                        {
                            SetMotor(_MOTOR_left_, 0, g_Speed);
                            SetMotor(_MOTOR_right_, 2, g_Speed);
                        }
                        else
                        {
                            if ( dg > (angle + g_Big) && dg <= (angle + 180) )
                            {
                                SetMotor(_MOTOR_left_, 2, g_Speed);
                                SetMotor(_MOTOR_right_, 0, g_Speed);
                            }
                        }
                    }
                }
            }
        }
    }
}
#endif