Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

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?

UserRevisionLine numberNew 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