Cooper Liu / Mbed 2 deprecated Eurobot2013_Co-Processor

working version with calibration done

Fork of Eurobot2013 by Oskar Weigl

geometryfuncs/geometryfuncs.h@11:5ba926692210, 2013-04-09 (annotated)

Committer:
xiaxia686
Date:
Tue Apr 09 15:32:47 2013 +0000
Revision:
11:5ba926692210
Parent:
10:2bd9f4e02b74 
woking version (calibrated)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
madcowswe 6:5a52c046d8f7 1 #ifndef GEOMETRYFUNCS_H
madcowswe 6:5a52c046d8f7 2 #define GEOMETRYFUNCS_H
madcowswe 6:5a52c046d8f7 3
madcowswe 6:5a52c046d8f7 4 // rectifies angle to range -PI to PI
madcowswe 6:5a52c046d8f7 5 template <typename T>
madcowswe 6:5a52c046d8f7 6 T rectifyAng (T ang_in) {
madcowswe 6:5a52c046d8f7 7 ang_in -= (floor(ang_in/(2*PI)))*2*PI;
madcowswe 6:5a52c046d8f7 8 if (ang_in < -PI) {
madcowswe 6:5a52c046d8f7 9 ang_in += 2*PI;
madcowswe 6:5a52c046d8f7 10 }
madcowswe 6:5a52c046d8f7 11 if (ang_in > PI) {
madcowswe 6:5a52c046d8f7 12 ang_in -= 2*PI;
madcowswe 6:5a52c046d8f7 13 }
madcowswe 6:5a52c046d8f7 14
madcowswe 6:5a52c046d8f7 15 return ang_in;
madcowswe 6:5a52c046d8f7 16 }
madcowswe 6:5a52c046d8f7 17
madcowswe 6:5a52c046d8f7 18 #endif //GEOMETRYFUNCS_H