Diff: control_theory/SLE.hpp

Revision:

2:e7b09385d197

Parent:

0:1456b6f84c75

Child:

6:e7f2335456c8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/control_theory/SLE.hpp	Sun Mar 27 04:39:16 2022 +0000
@@ -0,0 +1,27 @@
+//--Self-localization Estimate--//
+/* AMT102v*4つを用いてXY座標の自己位置推定(Self-localization Estimate)するライブラリ.4輪を前後左右４方向に均等に配置している場合のみ使える(要はベタな４輪オムニ自動機ならこれ使える).*/
+#pragma once
+#include <mbed.h>
+#include <portSet.hpp>
+#include <rotary_inc_sp.hpp>
+
+#define NOW 0
+#define PREV 1
+#define X_DATA 0
+#define Y_DATA 1
+
+class SLE
+{
+public:
+    SLE(Port enc0,Port enc1,Port enc2,Port enc3,float resolution,float ensyu,float interval);
+    void update(double theta);
+    void reset();
+    void setPos(float x_pos,float y_pos);
+    float position[2];
+    float velocity[2];
+    RotaryInc odometer[4];
+private:
+    float wheel_ensyu;
+    float dt;
+    float Vx[2],Vy[2];
+};
\ No newline at end of file