2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
supportfuncs.h
- Committer:
- rsavitski
- Date:
- 2013-10-15
- Revision:
- 92:4a1225fbb146
- Parent:
- 62:78d99b781f02
File content as of revision 92:4a1225fbb146:
#ifndef SUPPORTFUNCS_H #define SUPPORTFUNCS_H #include <cmath> #include "globals.h" #include "tvmet/Matrix.h" template <typename T> int sgn(T val) { return (T(0) < val) - (val < T(0)); } //Constrains agles to +/- pi inline float constrainAngle(float x){ x = fmod(x + PI, 2*PI); if (x < 0) x += 2*PI; return x - PI; } template <typename T> tvmet::Matrix <T, 2, 2> Rotmatrix(T theta) { tvmet::Matrix <T, 2, 2> outmatrix; outmatrix = cos(theta), -sin(theta), sin(theta), cos(theta); return outmatrix; } #endif //SUPPORTFUNCS_H