ICRS Eurobot 2013
Dependencies: mbed mbed-rtos Servo QEI
Diff: supportfuncs.h
- Revision:
- 20:70d651156779
- Parent:
- 19:4b993a9a156e
diff -r 4b993a9a156e -r 70d651156779 supportfuncs.h --- a/supportfuncs.h Sun Apr 07 19:26:07 2013 +0000 +++ b/supportfuncs.h Tue Apr 09 15:33:36 2013 +0000 @@ -3,7 +3,7 @@ #include <cmath> #include "globals.h" - +#include "tvmet/Matrix.h" //Constrains agles to +/- pi inline float constrainAngle(float x){ @@ -13,4 +13,12 @@ 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 \ No newline at end of file