yotaro morizumi / Mbed 2 deprecated zoomy_customLibrary

my new gear...

Dependencies:   mbed

control_theory/SLE.hpp

Committer:
yootee
Date:
2022-03-27
Revision:
2:e7b09385d197
Parent:
SLE.hpp@ 0:1456b6f84c75
Child:
6:e7f2335456c8

File content as of revision 2:e7b09385d197:

//--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];
};