yotaro morizumi
/
zoomy_customLibrary
my new gear...
Diff: control_theory/SLE.hpp
- Revision:
- 2:e7b09385d197
- Parent:
- 0:1456b6f84c75
- Child:
- 6:e7f2335456c8
diff -r 744e1f66d5dc -r e7b09385d197 control_theory/SLE.hpp --- /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輪を前後左右4方向に均等に配置している場合のみ使える(要はベタな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