Shuto Naruse / Mbed 2 deprecated Eurobot2012_Secondary

Eurobot2012_Secondary

Fork of Eurobot_2012_Secondary by Shuto Naruse

Eurobot_shared/Kalman/Kalman.h@1:cc2a9eb0bd55, 2012-10-17 (annotated)

Committer:
narshu
Date:
Wed Oct 17 22:25:31 2012 +0000
Revision:
1:cc2a9eb0bd55
Commit before publishing

Who changed what in which revision?

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