turnangle.c

机器人足球教程footballteaching

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

void TurnAngle(unsigned int angle)
{
    unsigned char counter = 0;
    unsigned int dg = 0;
    unsigned int min = 0;
    unsigned int max = 0;
    unsigned char isLeft = 0;
    unsigned int angle2 = 0;
    unsigned char turnSpeed = 50;
    unsigned char var1 = 0;
    while ( counter < 2 )
    {
        dg = GetCompassB(_COMPASS_1_);
        if ( dg > 360 )
        {
            counter = 0;
        }
        else
        {
            counter = counter + 1;
        }
    }
    if ( (angle>=0) && (angle<180) )
    {
        angle2=angle + 180;
    }
    else
    {
        angle2=angle - 180;
    }
    if ( ((dg>angle) && (dg<= angle2) && (angle2> angle)) || (((dg>=angle)&&(dg<=360)||(dg>=0)&&(dg<angle2)) && (angle>angle2)) )
    {
        isLeft=1;
    }
    if ( isLeft == 1 )
    {
        SetMotor(_MOTOR_left_, 2, turnSpeed);
        SetMotor(_MOTOR_right_, 0, turnSpeed);
    }
    else
    {
        SetMotor(_MOTOR_left_, 0, turnSpeed);
        SetMotor(_MOTOR_right_, 2, turnSpeed);
    }
    while (1)
    {
        counter = 0;
        while ( counter < 2 )
        {
            dg = GetCompassB(_COMPASS_1_);
            if ( dg > 360 )
            {
                counter = 0;
            }
            else
            {
                counter = counter + 1;
            }
        }
        if ( isLeft>0 )
        {
            if ( (dg<=angle) && (angle-dg)>0 && (angle-dg)<5 )
            {
                break;
            }
            else
            {
                if ( ((360-dg+angle)>0) && ((360-dg+angle)<5) )
                {
                    break;
                }
            }
        }
        else
        {
            if ( (dg>=angle)&&((dg-angle)>0 && (dg-angle)<5) )
            {
                break;
            }
            else
            {
                if ( ((360-angle+dg)>0)&&((360-angle+dg)<5) )
                {
                    break;
                }
            }
        }
    }
    if ( isLeft == 1 )
    {
        SetMotor(_MOTOR_left_, 0, turnSpeed);
        SetMotor(_MOTOR_right_, 2, turnSpeed);
    }
    else
    {
        SetMotor(_MOTOR_left_, 2, turnSpeed);
        SetMotor(_MOTOR_right_, 0, turnSpeed);
    }
    SetCentiS(10);
    SetMotor(_MOTOR_left_, 1, 0);
    SetMotor(_MOTOR_right_, 1, 0);
}
#endif