Diff: Kalman/Kalman.cpp

Revision:

6:5a52c046d8f7

Parent:

1:6799c07fe510

--- a/Kalman/Kalman.cpp	Wed Nov 14 16:15:46 2012 +0000
+++ b/Kalman/Kalman.cpp	Wed Nov 14 16:49:10 2012 +0000
@@ -6,7 +6,7 @@
 #include "RFSRF05.h"
 #include "math.h"
 #include "globals.h"
-#include "motors.h"
+#include "encoders.h"
 #include "system.h"
 #include "geometryfuncs.h"
 
@@ -14,7 +14,7 @@
 #include <tvmet/Vector.h>
 using namespace tvmet;
 
-Kalman::Kalman(Motors &motorsin,
+Kalman::Kalman(Encoders &encodersin,
                UI &uiin,
                PinName Sonar_Trig,
                PinName Sonar_Echo0,
@@ -41,7 +41,7 @@
                    Sonar_SCK,
                    Sonar_NCS,
                    Sonar_NIRQ),
-        motors(motorsin),
+        encoders(encodersin),
         ui(uiin),
         predictthread(predictloopwrapper, this, osPriorityNormal, 512),
         predictticker( SIGTICKARGS(predictthread, 0x1) ),
@@ -83,7 +83,7 @@
 
 //Note: this init function assumes that the robot faces east, theta=0, in the +x direction
 void Kalman::KalmanInit() {
-    motors.stop();
+
     float SonarMeasuresx1000[3];
     float IRMeasuresloc[3];
     int beacon_cnt = 0;
@@ -199,11 +199,11 @@
         Thread::signal_wait(0x1);
         OLED1 = !OLED1;
 
-        int leftenc = motors.getEncoder1();
-        int rightenc = motors.getEncoder2();
+        int leftenc = encoders.getEncoder1();
+        int rightenc = encoders.getEncoder2();
 
-        float dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f;
-        float dright = motors.encoderToDistance(rightenc-lastright)/1000.0f;
+        float dleft = encoders.encoderToDistance(leftenc-lastleft)/1000.0f;
+        float dright = encoders.encoderToDistance(rightenc-lastright)/1000.0f;
 
         lastleft = leftenc;
         lastright = rightenc;