![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
supportfuncs.h@92:4a1225fbb146, 2013-10-15 (annotated)
- Committer:
- rsavitski
- Date:
- Tue Oct 15 12:19:32 2013 +0000
- Revision:
- 92:4a1225fbb146
- Parent:
- 62:78d99b781f02
touch: ripped out 2013-specific bits. Need to address "2014" comments. Rewrite AI layer and other deleted parts.
Who changed what in which revision?
User | Revision | Line number | New 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 | 62:78d99b781f02 | 8 | template <typename T> int sgn(T val) { |
madcowswe | 62:78d99b781f02 | 9 | return (T(0) < val) - (val < T(0)); |
madcowswe | 62:78d99b781f02 | 10 | } |
madcowswe | 62:78d99b781f02 | 11 | |
madcowswe | 16:52250d8d8fce | 12 | //Constrains agles to +/- pi |
madcowswe | 16:52250d8d8fce | 13 | inline float constrainAngle(float x){ |
madcowswe | 19:4b993a9a156e | 14 | x = fmod(x + PI, 2*PI); |
madcowswe | 16:52250d8d8fce | 15 | if (x < 0) |
madcowswe | 19:4b993a9a156e | 16 | x += 2*PI; |
madcowswe | 19:4b993a9a156e | 17 | return x - PI; |
madcowswe | 16:52250d8d8fce | 18 | } |
madcowswe | 16:52250d8d8fce | 19 | |
madcowswe | 20:70d651156779 | 20 | template <typename T> |
madcowswe | 20:70d651156779 | 21 | tvmet::Matrix <T, 2, 2> Rotmatrix(T theta) { |
madcowswe | 20:70d651156779 | 22 | tvmet::Matrix <T, 2, 2> outmatrix; |
madcowswe | 20:70d651156779 | 23 | outmatrix = cos(theta), -sin(theta), |
madcowswe | 20:70d651156779 | 24 | sin(theta), cos(theta); |
madcowswe | 20:70d651156779 | 25 | return outmatrix; |
madcowswe | 20:70d651156779 | 26 | } |
madcowswe | 20:70d651156779 | 27 | |
madcowswe | 16:52250d8d8fce | 28 | #endif //SUPPORTFUNCS_H |