Oskar Weigl
/
Eurobot2013
We are going to win! wohoo
Embed:
(wiki syntax)
Show/hide line numbers
Kalman.h
00001 #ifndef KALMAN_H 00002 #define KALMAN_H 00003 00004 #include "globals.h" 00005 00006 00007 #include "rtos.h" 00008 //#include "Matrix.h" 00009 #include "encoders.h" 00010 #include "RFSRF05.h" 00011 #include "IR.h" 00012 #include "ui.h" 00013 00014 #include <tvmet/Matrix.h> 00015 #include <tvmet/Vector.h> 00016 using namespace tvmet; 00017 00018 00019 class Kalman { 00020 public: 00021 enum measurement_t {SONAR1 = 0, SONAR2, SONAR3, IR1, IR2, IR3}; 00022 static const measurement_t maxmeasure = IR3; 00023 00024 Kalman(Encoders &encodersin, 00025 UI &uiin, 00026 PinName Sonar_Trig, 00027 PinName Sonar_Echo0, 00028 PinName Sonar_Echo1, 00029 PinName Sonar_Echo2, 00030 PinName Sonar_Echo3, 00031 PinName Sonar_Echo4, 00032 PinName Sonar_Echo5, 00033 PinName Sonar_SDI, 00034 PinName Sonar_SDO, 00035 PinName Sonar_SCK, 00036 PinName Sonar_NCS, 00037 PinName Sonar_NIRQ); 00038 00039 void predict(); 00040 void runupdate(measurement_t type, float value, float variance); 00041 00042 //State variables 00043 Vector<float, 3> X; 00044 Matrix<float, 3, 3> P; 00045 Mutex statelock; 00046 00047 float SonarMeasures[3]; 00048 float IRMeasures[3]; 00049 float IR_Offset; 00050 float Sonar_Offset; 00051 Mutex InitLock; 00052 00053 bool Kalman_init; 00054 00055 //The IR is public so it's possible to print the offset in the print function 00056 IR ir; 00057 00058 //Initialises the kalman filter 00059 void KalmanInit(); 00060 00061 // reset kalman states 00062 void KalmanReset(); 00063 00064 private: 00065 00066 //Sensor interfaces 00067 RFSRF05 sonararray; 00068 Encoders& encoders; 00069 UI& ui; 00070 00071 Thread predictthread; 00072 void predictloop(); 00073 static void predictloopwrapper(void const *argument) { 00074 ((Kalman*)argument)->predictloop(); 00075 } 00076 RtosTimer predictticker; 00077 00078 // Thread sonarthread; 00079 // void sonarloop(); 00080 // static void sonarloopwrapper(void const *argument){ ((Kalman*)argument)->sonarloop(); } 00081 // RtosTimer sonarticker; 00082 00083 struct measurmentdata { 00084 measurement_t mtype; 00085 float value; 00086 float variance; 00087 } ; 00088 00089 Mail <measurmentdata, 16> measureMQ; 00090 00091 Thread updatethread; 00092 void updateloop(); 00093 static void updateloopwrapper(void const *argument) { 00094 ((Kalman*)argument)->updateloop(); 00095 } 00096 00097 00098 }; 00099 00100 #endif //KALMAN_H
Generated on Tue Jul 12 2022 19:50:03 by 1.7.2