Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Eurobot_shared/Kalman/Kalman.h@9:377560539b74, 2012-04-28 (annotated)

Committer:
narshu
Date:
Sat Apr 28 17:21:24 2012 +0000
Revision:
9:377560539b74
Restructured project to have a single shared lib; Also raised the RF baud rate

Who changed what in which revision?

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