yotaro morizumi
/
zoomy_customLibrary
my new gear...
control_theory/SLE.hpp@2:e7b09385d197, 2022-03-27 (annotated)
- Committer:
- yootee
- Date:
- Sun Mar 27 04:39:16 2022 +0000
- Revision:
- 2:e7b09385d197
- Parent:
- SLE.hpp@0:1456b6f84c75
- Child:
- 6:e7f2335456c8
arrc?
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yootee | 0:1456b6f84c75 | 1 | //--Self-localization Estimate--// |
yootee | 0:1456b6f84c75 | 2 | /* AMT102v*4つを用いてXY座標の自己位置推定(Self-localization Estimate)するライブラリ.4輪を前後左右4方向に均等に配置している場合のみ使える(要はベタな4輪オムニ自動機ならこれ使える).*/ |
yootee | 0:1456b6f84c75 | 3 | #pragma once |
yootee | 0:1456b6f84c75 | 4 | #include <mbed.h> |
yootee | 0:1456b6f84c75 | 5 | #include <portSet.hpp> |
yootee | 0:1456b6f84c75 | 6 | #include <rotary_inc_sp.hpp> |
yootee | 0:1456b6f84c75 | 7 | |
yootee | 0:1456b6f84c75 | 8 | #define NOW 0 |
yootee | 0:1456b6f84c75 | 9 | #define PREV 1 |
yootee | 0:1456b6f84c75 | 10 | #define X_DATA 0 |
yootee | 0:1456b6f84c75 | 11 | #define Y_DATA 1 |
yootee | 0:1456b6f84c75 | 12 | |
yootee | 0:1456b6f84c75 | 13 | class SLE |
yootee | 0:1456b6f84c75 | 14 | { |
yootee | 0:1456b6f84c75 | 15 | public: |
yootee | 0:1456b6f84c75 | 16 | SLE(Port enc0,Port enc1,Port enc2,Port enc3,float resolution,float ensyu,float interval); |
yootee | 0:1456b6f84c75 | 17 | void update(double theta); |
yootee | 0:1456b6f84c75 | 18 | void reset(); |
yootee | 0:1456b6f84c75 | 19 | void setPos(float x_pos,float y_pos); |
yootee | 0:1456b6f84c75 | 20 | float position[2]; |
yootee | 0:1456b6f84c75 | 21 | float velocity[2]; |
yootee | 0:1456b6f84c75 | 22 | RotaryInc odometer[4]; |
yootee | 0:1456b6f84c75 | 23 | private: |
yootee | 0:1456b6f84c75 | 24 | float wheel_ensyu; |
yootee | 0:1456b6f84c75 | 25 | float dt; |
yootee | 0:1456b6f84c75 | 26 | float Vx[2],Vy[2]; |
yootee | 0:1456b6f84c75 | 27 | }; |