This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
supportfuncs.h
- Committer:
- madcowswe
- Date:
- 2013-04-14
- Revision:
- 67:be3ea5450cc7
- Parent:
- 62:78d99b781f02
File content as of revision 67:be3ea5450cc7:
#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