Cooper Liu / Mbed 2 deprecated ICRSEurobot13

Colour sensors calibrated

Dependencies:   mbed-rtos mbed Servo QEI

Fork of ICRSEurobot13 by Thomas Branch

supportfuncs.h@46:adcd57a5e402, 2013-04-12 (annotated)

Committer:
xiaxia686
Date:
Fri Apr 12 20:59:18 2013 +0000
Revision:
46:adcd57a5e402
Parent:
20:70d651156779 
Colours Sensors fixed

Who changed what in which revision?

UserRevisionLine numberNew 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 16:52250d8d8fce 8 //Constrains agles to +/- pi
madcowswe 16:52250d8d8fce 9 inline float constrainAngle(float x){
madcowswe 19:4b993a9a156e 10 x = fmod(x + PI, 2*PI);
madcowswe 16:52250d8d8fce 11 if (x < 0)
madcowswe 19:4b993a9a156e 12 x += 2*PI;
madcowswe 19:4b993a9a156e 13 return x - PI;
madcowswe 16:52250d8d8fce 14 }
madcowswe 16:52250d8d8fce 15
madcowswe 20:70d651156779 16 template <typename T>
madcowswe 20:70d651156779 17 tvmet::Matrix <T, 2, 2> Rotmatrix(T theta) {
madcowswe 20:70d651156779 18 tvmet::Matrix <T, 2, 2> outmatrix;
madcowswe 20:70d651156779 19 outmatrix = cos(theta), -sin(theta),
madcowswe 20:70d651156779 20 sin(theta), cos(theta);
madcowswe 20:70d651156779 21 return outmatrix;
madcowswe 20:70d651156779 22 }
madcowswe 20:70d651156779 23
madcowswe 16:52250d8d8fce 24 #endif //SUPPORTFUNCS_H