Oskar Weigl / Mbed 2 deprecated Eurobot2013

We are going to win! wohoo

Dependencies:   mbed mbed-rtos

Kalman/Kalman.h@5:a229f40c1210, 2012-11-14 (annotated)

Committer:
madcowswe
Date:
Wed Nov 14 16:15:46 2012 +0000
Revision:
5:a229f40c1210
Parent:
1:6799c07fe510
Child:
6:5a52c046d8f7
Added "system"

Who changed what in which revision?

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