Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Kalman/Kalman.h@1:bbabbd997d21, 2012-04-20 (annotated)

Committer:
narshu
Date:
Fri Apr 20 21:56:15 2012 +0000
Revision:
1:bbabbd997d21
Parent:
0:f3bf6c7e2283 
copied everything from secondary;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
narshu 0:f3bf6c7e2283 1 #include "rtos.h"
narshu 0:f3bf6c7e2283 2 //#include "Matrix.h"
narshu 0:f3bf6c7e2283 3 #include "motors.h"
narshu 0:f3bf6c7e2283 4 #include "RFSRF05.h"
narshu 0:f3bf6c7e2283 5
narshu 0:f3bf6c7e2283 6 #include <tvmet/Matrix.h>
narshu 0:f3bf6c7e2283 7 #include <tvmet/Vector.h>
narshu 0:f3bf6c7e2283 8 using namespace tvmet;
narshu 0:f3bf6c7e2283 9
narshu 0:f3bf6c7e2283 10
narshu 0:f3bf6c7e2283 11 class Kalman {
narshu 0:f3bf6c7e2283 12 public:
narshu 1:bbabbd997d21 13 enum measurement_t {SONAR1 = 0, SONAR2, SONAR3, IR1, IR2, IR3};
narshu 0:f3bf6c7e2283 14 static const measurement_t maxmeasure = IR3;
narshu 0:f3bf6c7e2283 15
narshu 0:f3bf6c7e2283 16 Kalman(Motors &motorsin);
narshu 0:f3bf6c7e2283 17
narshu 0:f3bf6c7e2283 18 void predict();
narshu 0:f3bf6c7e2283 19 void runupdate(measurement_t type, float value, float variance);
narshu 0:f3bf6c7e2283 20
narshu 0:f3bf6c7e2283 21 //State variables
narshu 0:f3bf6c7e2283 22 Vector<float, 3> X;
narshu 0:f3bf6c7e2283 23 Matrix<float, 3, 3> P;
narshu 0:f3bf6c7e2283 24 Mutex statelock;
narshu 0:f3bf6c7e2283 25
narshu 0:f3bf6c7e2283 26 float SonarMeasures[3];
narshu 0:f3bf6c7e2283 27 float IRMeasures[3];
narshu 0:f3bf6c7e2283 28
narshu 0:f3bf6c7e2283 29 private:
narshu 0:f3bf6c7e2283 30
narshu 1:bbabbd997d21 31 //Matrix<float, 3, 3> Q; //perhaps calculate on the fly? dependant on speed etc?
narshu 0:f3bf6c7e2283 32
narshu 0:f3bf6c7e2283 33 RFSRF05 sonararray;
narshu 0:f3bf6c7e2283 34 Motors& motors;
narshu 0:f3bf6c7e2283 35
narshu 0:f3bf6c7e2283 36 Thread predictthread;
narshu 0:f3bf6c7e2283 37 void predictloop();
narshu 0:f3bf6c7e2283 38 static void predictloopwrapper(void const *argument){ ((Kalman*)argument)->predictloop(); }
narshu 0:f3bf6c7e2283 39 RtosTimer predictticker;
narshu 0:f3bf6c7e2283 40
narshu 0:f3bf6c7e2283 41 // Thread sonarthread;
narshu 0:f3bf6c7e2283 42 // void sonarloop();
narshu 0:f3bf6c7e2283 43 // static void sonarloopwrapper(void const *argument){ ((Kalman*)argument)->sonarloop(); }
narshu 0:f3bf6c7e2283 44 // RtosTimer sonarticker;
narshu 0:f3bf6c7e2283 45
narshu 0:f3bf6c7e2283 46 struct measurmentdata{
narshu 0:f3bf6c7e2283 47 measurement_t mtype;
narshu 0:f3bf6c7e2283 48 float value;
narshu 0:f3bf6c7e2283 49 float variance;
narshu 0:f3bf6c7e2283 50 } ;
narshu 0:f3bf6c7e2283 51
narshu 0:f3bf6c7e2283 52 Mail <measurmentdata, 16> measureMQ;
narshu 0:f3bf6c7e2283 53
narshu 0:f3bf6c7e2283 54 Thread updatethread;
narshu 0:f3bf6c7e2283 55 void updateloop();
narshu 0:f3bf6c7e2283 56 static void updateloopwrapper(void const *argument){ ((Kalman*)argument)->updateloop(); }
narshu 0:f3bf6c7e2283 57
narshu 0:f3bf6c7e2283 58
narshu 0:f3bf6c7e2283 59 };