Diff: Locate.cpp

Revision:

3:633ea1e7c681

Parent:

2:f20408555e83

Child:

6:835fbc52feb0

--- a/Locate.cpp	Sat Jul 02 02:36:39 2016 +0000
+++ b/Locate.cpp	Thu Aug 11 04:07:42 2016 +0000
@@ -1,18 +1,41 @@
-#include <mathe.h>
+#include <math.h>
 #include "Locate.h"
 
 
-Locate::Locate () {
-    theta = 0;
-    x = 0;
-    y = 0;
+Locate::Locate (int fx, int fy) 
+{
+    x = fx;
+    y = fy;
+}
+
+void Locate::setup(int r, int l)
+{
+    pr = dr = r;    //(今(最初)のステップ数) = (初期ズレ) = (エンコーダから受け取るステップ数)
+    pl = dl = l;    //上の説明は意味不明なので無視してください
 }
 
-void Locate::update (short int r, short int l) {
-    theta = (r - l) * ROUND;
-    v = (r + l) / 2;
-    x += v * sin(theta);
-    y += v * cos(theta);
 
-    //r = 0; l = 0; //受け取らずにループしても変化しないようにするため
+void Locate::update (int r, int l) 
+{
+    r -= dr; l -= dl;   //ズレを修正
+    
+    theta = (r - l) * ROUND;   
+    v = (r-pr + l-pl) / 2;     //r-pr = 前回から右車輪が進んだステップ数
+    
+    x += v * (sin(theta));    
+    y += v * (cos(theta));
+    
+    pr = r;                 //今回のステップ数を保存
+    pl = l;
+}
+
+
+short Locate::getX()
+{
+    return x * LOCATE_STEP;   
+}
+
+short Locate::getY()
+{
+    return y * LOCATE_STEP;   
 }
\ No newline at end of file