Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Diff: Kalman/Kalman.h
- Revision:
- 2:cffa347bb943
- Parent:
- 1:bbabbd997d21
- Child:
- 3:429829612cf9
--- a/Kalman/Kalman.h Fri Apr 20 21:56:15 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,59 +0,0 @@ -#include "rtos.h" -//#include "Matrix.h" -#include "motors.h" -#include "RFSRF05.h" - -#include <tvmet/Matrix.h> -#include <tvmet/Vector.h> -using namespace tvmet; - - -class Kalman { -public: - enum measurement_t {SONAR1 = 0, SONAR2, SONAR3, IR1, IR2, IR3}; - static const measurement_t maxmeasure = IR3; - - Kalman(Motors &motorsin); - - void predict(); - void runupdate(measurement_t type, float value, float variance); - - //State variables - Vector<float, 3> X; - Matrix<float, 3, 3> P; - Mutex statelock; - - float SonarMeasures[3]; - float IRMeasures[3]; - -private: - - //Matrix<float, 3, 3> Q; //perhaps calculate on the fly? dependant on speed etc? - - RFSRF05 sonararray; - Motors& motors; - - Thread predictthread; - void predictloop(); - static void predictloopwrapper(void const *argument){ ((Kalman*)argument)->predictloop(); } - RtosTimer predictticker; - -// Thread sonarthread; -// void sonarloop(); -// static void sonarloopwrapper(void const *argument){ ((Kalman*)argument)->sonarloop(); } -// RtosTimer sonarticker; - - struct measurmentdata{ - measurement_t mtype; - float value; - float variance; - } ; - - Mail <measurmentdata, 16> measureMQ; - - Thread updatethread; - void updateloop(); - static void updateloopwrapper(void const *argument){ ((Kalman*)argument)->updateloop(); } - - -}; \ No newline at end of file