Shuto Naruse
/
Eurobot2012_Secondary
Eurobot2012_Secondary
Fork of Eurobot_2012_Secondary by
Kalman/Kalman.h
- Committer:
- narshu
- Date:
- 2012-04-20
- Revision:
- 0:fbfafa6bf5f9
File content as of revision 0:fbfafa6bf5f9:
#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(); } };