Thomas Branch / Mbed 2 deprecated ICRSEurobot13

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