Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Eurobot_shared/Kalman/Kalman.h@26:0995f61cb7b8, 2012-10-17 (annotated)
- Committer:
- narshu
- Date:
- Wed Oct 17 22:22:47 2012 +0000
- Revision:
- 26:0995f61cb7b8
- Parent:
- 24:7a3906c2f5d5
Eurobot 2012 Primary;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
narshu | 24:7a3906c2f5d5 | 1 | #ifndef KALMAN_H |
narshu | 24:7a3906c2f5d5 | 2 | #define KALMAN_H |
narshu | 24:7a3906c2f5d5 | 3 | |
narshu | 24:7a3906c2f5d5 | 4 | #include "globals.h" |
narshu | 24:7a3906c2f5d5 | 5 | |
narshu | 24:7a3906c2f5d5 | 6 | |
narshu | 24:7a3906c2f5d5 | 7 | #include "rtos.h" |
narshu | 24:7a3906c2f5d5 | 8 | //#include "Matrix.h" |
narshu | 24:7a3906c2f5d5 | 9 | #include "motors.h" |
narshu | 24:7a3906c2f5d5 | 10 | #include "RFSRF05.h" |
narshu | 24:7a3906c2f5d5 | 11 | #include "IR.h" |
narshu | 24:7a3906c2f5d5 | 12 | #include "ui.h" |
narshu | 24:7a3906c2f5d5 | 13 | |
narshu | 24:7a3906c2f5d5 | 14 | #include <tvmet/Matrix.h> |
narshu | 24:7a3906c2f5d5 | 15 | #include <tvmet/Vector.h> |
narshu | 24:7a3906c2f5d5 | 16 | using namespace tvmet; |
narshu | 24:7a3906c2f5d5 | 17 | |
narshu | 24:7a3906c2f5d5 | 18 | |
narshu | 24:7a3906c2f5d5 | 19 | class Kalman { |
narshu | 24:7a3906c2f5d5 | 20 | public: |
narshu | 24:7a3906c2f5d5 | 21 | enum measurement_t {SONAR1 = 0, SONAR2, SONAR3, IR1, IR2, IR3}; |
narshu | 24:7a3906c2f5d5 | 22 | static const measurement_t maxmeasure = IR3; |
narshu | 24:7a3906c2f5d5 | 23 | |
narshu | 24:7a3906c2f5d5 | 24 | Kalman(Motors &motorsin, |
narshu | 24:7a3906c2f5d5 | 25 | UI &uiin, |
narshu | 24:7a3906c2f5d5 | 26 | PinName Sonar_Trig, |
narshu | 24:7a3906c2f5d5 | 27 | PinName Sonar_Echo0, |
narshu | 24:7a3906c2f5d5 | 28 | PinName Sonar_Echo1, |
narshu | 24:7a3906c2f5d5 | 29 | PinName Sonar_Echo2, |
narshu | 24:7a3906c2f5d5 | 30 | PinName Sonar_Echo3, |
narshu | 24:7a3906c2f5d5 | 31 | PinName Sonar_Echo4, |
narshu | 24:7a3906c2f5d5 | 32 | PinName Sonar_Echo5, |
narshu | 24:7a3906c2f5d5 | 33 | PinName Sonar_SDI, |
narshu | 24:7a3906c2f5d5 | 34 | PinName Sonar_SDO, |
narshu | 24:7a3906c2f5d5 | 35 | PinName Sonar_SCK, |
narshu | 24:7a3906c2f5d5 | 36 | PinName Sonar_NCS, |
narshu | 24:7a3906c2f5d5 | 37 | PinName Sonar_NIRQ); |
narshu | 24:7a3906c2f5d5 | 38 | |
narshu | 24:7a3906c2f5d5 | 39 | void predict(); |
narshu | 24:7a3906c2f5d5 | 40 | void runupdate(measurement_t type, float value, float variance); |
narshu | 24:7a3906c2f5d5 | 41 | |
narshu | 24:7a3906c2f5d5 | 42 | //State variables |
narshu | 24:7a3906c2f5d5 | 43 | Vector<float, 3> X; |
narshu | 24:7a3906c2f5d5 | 44 | Matrix<float, 3, 3> P; |
narshu | 24:7a3906c2f5d5 | 45 | Mutex statelock; |
narshu | 24:7a3906c2f5d5 | 46 | |
narshu | 24:7a3906c2f5d5 | 47 | float SonarMeasures[3]; |
narshu | 24:7a3906c2f5d5 | 48 | float IRMeasures[3]; |
narshu | 24:7a3906c2f5d5 | 49 | float IR_Offset; |
narshu | 24:7a3906c2f5d5 | 50 | float Sonar_Offset; |
narshu | 24:7a3906c2f5d5 | 51 | Mutex InitLock; |
narshu | 24:7a3906c2f5d5 | 52 | |
narshu | 24:7a3906c2f5d5 | 53 | bool Kalman_init; |
narshu | 24:7a3906c2f5d5 | 54 | |
narshu | 24:7a3906c2f5d5 | 55 | //The IR is public so it's possible to print the offset in the print function |
narshu | 24:7a3906c2f5d5 | 56 | IR ir; |
narshu | 24:7a3906c2f5d5 | 57 | |
narshu | 24:7a3906c2f5d5 | 58 | //Initialises the kalman filter |
narshu | 24:7a3906c2f5d5 | 59 | void KalmanInit(); |
narshu | 24:7a3906c2f5d5 | 60 | |
narshu | 24:7a3906c2f5d5 | 61 | // reset kalman states |
narshu | 24:7a3906c2f5d5 | 62 | void KalmanReset(); |
narshu | 24:7a3906c2f5d5 | 63 | |
narshu | 24:7a3906c2f5d5 | 64 | private: |
narshu | 24:7a3906c2f5d5 | 65 | |
narshu | 24:7a3906c2f5d5 | 66 | //Sensor interfaces |
narshu | 24:7a3906c2f5d5 | 67 | RFSRF05 sonararray; |
narshu | 24:7a3906c2f5d5 | 68 | Motors& motors; |
narshu | 24:7a3906c2f5d5 | 69 | UI& ui; |
narshu | 24:7a3906c2f5d5 | 70 | |
narshu | 24:7a3906c2f5d5 | 71 | Thread predictthread; |
narshu | 24:7a3906c2f5d5 | 72 | void predictloop(); |
narshu | 24:7a3906c2f5d5 | 73 | static void predictloopwrapper(void const *argument) { |
narshu | 24:7a3906c2f5d5 | 74 | ((Kalman*)argument)->predictloop(); |
narshu | 24:7a3906c2f5d5 | 75 | } |
narshu | 24:7a3906c2f5d5 | 76 | RtosTimer predictticker; |
narshu | 24:7a3906c2f5d5 | 77 | |
narshu | 24:7a3906c2f5d5 | 78 | // Thread sonarthread; |
narshu | 24:7a3906c2f5d5 | 79 | // void sonarloop(); |
narshu | 24:7a3906c2f5d5 | 80 | // static void sonarloopwrapper(void const *argument){ ((Kalman*)argument)->sonarloop(); } |
narshu | 24:7a3906c2f5d5 | 81 | // RtosTimer sonarticker; |
narshu | 24:7a3906c2f5d5 | 82 | |
narshu | 24:7a3906c2f5d5 | 83 | struct measurmentdata { |
narshu | 24:7a3906c2f5d5 | 84 | measurement_t mtype; |
narshu | 24:7a3906c2f5d5 | 85 | float value; |
narshu | 24:7a3906c2f5d5 | 86 | float variance; |
narshu | 24:7a3906c2f5d5 | 87 | } ; |
narshu | 24:7a3906c2f5d5 | 88 | |
narshu | 24:7a3906c2f5d5 | 89 | Mail <measurmentdata, 16> measureMQ; |
narshu | 24:7a3906c2f5d5 | 90 | |
narshu | 24:7a3906c2f5d5 | 91 | Thread updatethread; |
narshu | 24:7a3906c2f5d5 | 92 | void updateloop(); |
narshu | 24:7a3906c2f5d5 | 93 | static void updateloopwrapper(void const *argument) { |
narshu | 24:7a3906c2f5d5 | 94 | ((Kalman*)argument)->updateloop(); |
narshu | 24:7a3906c2f5d5 | 95 | } |
narshu | 24:7a3906c2f5d5 | 96 | |
narshu | 24:7a3906c2f5d5 | 97 | |
narshu | 24:7a3906c2f5d5 | 98 | }; |
narshu | 24:7a3906c2f5d5 | 99 | |
narshu | 24:7a3906c2f5d5 | 100 | #endif //KALMAN_H |