Cooper Liu
/
Eurobot2013_Co-Processor
working version with calibration done
Fork of Eurobot2013 by
Diff: geometryfuncs/geometryfuncs.h
- Revision:
- 10:2bd9f4e02b74
- Parent:
- 6:5a52c046d8f7
diff -r 08552997b544 -r 2bd9f4e02b74 geometryfuncs/geometryfuncs.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/geometryfuncs/geometryfuncs.h Sun Apr 07 16:30:49 2013 +0000 @@ -0,0 +1,18 @@ +#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 \ No newline at end of file