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-09
Revision:
11:5ba926692210
Parent:
10:2bd9f4e02b74

File content as of revision 11:5ba926692210:

#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