yotaro morizumi
/
zoomy_customLibrary
my new gear...
SLE.hpp
- Committer:
- yootee
- Date:
- 2022-02-25
- Revision:
- 0:1456b6f84c75
File content as of revision 0:1456b6f84c75:
//--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]; };