Thomas Branch / Mbed 2 deprecated ICRSEurobot13

ICRS Eurobot 2013

Dependencies:   mbed mbed-rtos Servo QEI

supportfuncs.h@20:70d651156779, 2013-04-09 (annotated)

Committer:
madcowswe
Date:
Tue Apr 09 15:33:36 2013 +0000
Revision:
20:70d651156779
Parent:
19:4b993a9a156e 
Predict loop running, update loop not done.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
madcowswe 16:52250d8d8fce 1 #ifndef SUPPORTFUNCS_H
madcowswe 16:52250d8d8fce 2 #define SUPPORTFUNCS_H
madcowswe 16:52250d8d8fce 3
madcowswe 16:52250d8d8fce 4 #include <cmath>
madcowswe 19:4b993a9a156e 5 #include "globals.h"
madcowswe 20:70d651156779 6 #include "tvmet/Matrix.h"
madcowswe 16:52250d8d8fce 7
madcowswe 16:52250d8d8fce 8 //Constrains agles to +/- pi
madcowswe 16:52250d8d8fce 9 inline float constrainAngle(float x){
madcowswe 19:4b993a9a156e 10 x = fmod(x + PI, 2*PI);
madcowswe 16:52250d8d8fce 11 if (x < 0)
madcowswe 19:4b993a9a156e 12 x += 2*PI;
madcowswe 19:4b993a9a156e 13 return x - PI;
madcowswe 16:52250d8d8fce 14 }
madcowswe 16:52250d8d8fce 15
madcowswe 20:70d651156779 16 template <typename T>
madcowswe 20:70d651156779 17 tvmet::Matrix <T, 2, 2> Rotmatrix(T theta) {
madcowswe 20:70d651156779 18 tvmet::Matrix <T, 2, 2> outmatrix;
madcowswe 20:70d651156779 19 outmatrix = cos(theta), -sin(theta),
madcowswe 20:70d651156779 20 sin(theta), cos(theta);
madcowswe 20:70d651156779 21 return outmatrix;
madcowswe 20:70d651156779 22 }
madcowswe 20:70d651156779 23
madcowswe 16:52250d8d8fce 24 #endif //SUPPORTFUNCS_H