SLE.hpp

00001 //--Self-localization Estimate--//
00002 /* AMT102v*4つを用いてXY座標の自己位置推定(Self-localization Estimate)するライブラリ.4輪を前後左右４方向に均等に配置している場合のみ使える(要はベタな４輪オムニ自動機ならこれ使える).*/
00003 /*
00004     encoder position
00005       front
00006         0
00007     1       2
00008         3
00009 */
00010 #pragma once
00011 #include <mbed.h>
00012 #include <portSet.hpp>
00013 #include <rotary_inc_sp.hpp>
00014 
00015 #define NOW 0
00016 #define PREV 1
00017 #define X_DATA 0
00018 #define Y_DATA 1
00019 
00020 class SLE
00021 {
00022 public:
00023     SLE(Port enc0,Port enc1,Port enc2,Port enc3,int resolution,float ensyu,float interval);
00024     void update(double theta);
00025     void reset();
00026     void setPos(float x_pos,float y_pos);
00027     float position[2];
00028     float velocity[2];
00029     RotaryInc odometer[4];
00030 private:
00031     float wheel_ensyu;
00032     float dt;
00033     float Vx[2],Vy[2];
00034 };