ICRS Eurobot 2013
Dependencies: mbed mbed-rtos Servo QEI
Diff: supportfuncs.h
- Revision:
- 16:52250d8d8fce
- Child:
- 19:4b993a9a156e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/supportfuncs.h Sun Apr 07 16:50:36 2013 +0000 @@ -0,0 +1,15 @@ +#ifndef SUPPORTFUNCS_H +#define SUPPORTFUNCS_H + +#define _USE_MATH_DEFINES +#include <cmath> + +//Constrains agles to +/- pi +inline float constrainAngle(float x){ + x = fmod(x + M_PI, 2*M_PI); + if (x < 0) + x += 2*M_PI; + return x - M_PI; +} + +#endif //SUPPORTFUNCS_H \ No newline at end of file