Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

supportfuncs.h

Committer:
rsavitski
Date:
2013-10-15
Revision:
92:4a1225fbb146
Parent:
62:78d99b781f02

File content as of revision 92:4a1225fbb146:

#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