getnewangle.c

来自「机器人足球教程footballteaching」· C语言 代码 · 共 26 行

#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