Cooper Liu / Mbed 2 deprecated Eurobot2013_Co-Processor

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of Eurobot2013 by Oskar Weigl

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