Diff: Kalman.cpp
- Revision:
- 2:8aa491f77a0b
- Parent:
- 1:4964fa534202
--- a/Kalman.cpp Thu Apr 26 19:28:32 2012 +0000
+++ b/Kalman.cpp Thu Apr 26 20:11:48 2012 +0000
@@ -15,32 +15,36 @@
using namespace tvmet;
Kalman::Kalman(Motors &motorsin,
- PinName Sonar_Trig,
- PinName Sonar_Echo0,
- PinName Sonar_Echo1,
- PinName Sonar_Echo2,
- PinName Sonar_Echo3,
- PinName Sonar_Echo4,
- PinName Sonar_Echo5,
- PinName Sonar_SDI,
- PinName Sonar_SDO,
- PinName Sonar_SCK,
- PinName Sonar_NCS,
- PinName Sonar_NIRQ) :
- ir(*this),
+ UI &uiin,
+ PinName Sonar_Trig,
+ PinName Sonar_Echo0,
+ PinName Sonar_Echo1,
+ PinName Sonar_Echo2,
+ PinName Sonar_Echo3,
+ PinName Sonar_Echo4,
+ PinName Sonar_Echo5,
+ PinName Sonar_SDI,
+ PinName Sonar_SDO,
+ PinName Sonar_SCK,
+ PinName Sonar_NCS,
+ PinName Sonar_NIRQ,
+ PinName IR_Tx,
+ PinName IR_Rx) :
+ ir(*this,IR_Tx,IR_Rx),
sonararray(Sonar_Trig,
- Sonar_Echo0,
- Sonar_Echo1,
- Sonar_Echo2,
- Sonar_Echo3,
- Sonar_Echo4,
- Sonar_Echo5,
- Sonar_SDI,
- Sonar_SDO,
- Sonar_SCK,
- Sonar_NCS,
- Sonar_NIRQ),
+ Sonar_Echo0,
+ Sonar_Echo1,
+ Sonar_Echo2,
+ Sonar_Echo3,
+ Sonar_Echo4,
+ Sonar_Echo5,
+ Sonar_SDI,
+ Sonar_SDO,
+ Sonar_SCK,
+ Sonar_NCS,
+ Sonar_NIRQ),
motors(motorsin),
+ ui(uiin),
predictthread(predictloopwrapper, this, osPriorityNormal, 512),
predictticker( SIGTICKARGS(predictthread, 0x1) ),
// sonarthread(sonarloopwrapper, this, osPriorityNormal, 256),
@@ -84,7 +88,7 @@
int beacon_cnt = 0;
// set initiating flag to false
Kalman_init = false;
-
+
// init the offset array
for (int k = 0; k < 3; k ++) {
SonarMeasure_Offset[k] = 0;
Shuto Naruse