Cooper Liu / Mbed 2 deprecated Eurobot2013_Co-Processor

working version with calibration done

Fork of Eurobot2013 by Oskar Weigl

geometryfuncs/geometryfuncs.h

Committer:
xiaxia686
Date:
2013-04-07
Revision:
10:2bd9f4e02b74
Parent:
system/geometryfuncs.h@ 6:5a52c046d8f7

File content as of revision 10:2bd9f4e02b74:

#ifndef GEOMETRYFUNCS_H
#define GEOMETRYFUNCS_H

// rectifies angle to range -PI to PI
template <typename T>
T rectifyAng (T ang_in) {
    ang_in -= (floor(ang_in/(2*PI)))*2*PI;
    if (ang_in < -PI) {
        ang_in += 2*PI;
    }
    if (ang_in > PI) {
        ang_in -= 2*PI;
    }

    return ang_in;
}

#endif //GEOMETRYFUNCS_H