getnewangle.c

机器人足球教程footballteaching

#ifndef _GETNEWANGLE_
#define _GETNEWANGLE_
#include "HardwareInfo.c"

unsigned int GetNewAngle(unsigned int middleAngle, unsigned int oldAngle)
{
    unsigned int newAngle = 0;
    unsigned int min = 0;
    unsigned int max = 0;
    min = middleAngle - 180;
    max = middleAngle + 180;
    oldAngle = oldAngle + 360;
    newAngle = oldAngle;
    if ( min < 360 && oldAngle > max )
    {
        newAngle = oldAngle -360;
    }
    if ( max > 720 && oldAngle < min )
    {
        newAngle = oldAngle + 360;
    }
    return newAngle;
}
#endif