This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
supportfuncs.h
- Committer:
- madcowswe
- Date:
- 2013-04-09
- Revision:
- 21:167dacfe0b14
- Parent:
- 20:70d651156779
- Child:
- 62:78d99b781f02
File content as of revision 21:167dacfe0b14:
#ifndef SUPPORTFUNCS_H #define SUPPORTFUNCS_H #include <cmath> #include "globals.h" #include "tvmet/Matrix.h" //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