2022_gorobo_Ateam / 2022_Ateam_MOTORprogram

足回り用プログラム(修正版)

Dependencies:   SBDBT arrc_mbed BNO055

Odmetry.hpp

Committer:
kazumayamanaka
Date:
2022-01-28
Revision:
5:e189d55ef292

File content as of revision 5:e189d55ef292:

#ifndef ODMETRY_H
#define ODMETRY_H

#include "mbed.h"
#define Co (108 * 3.1415926535897) / 512
#define PI 3.1415926535897

class Odmetry{
    public:
        Odmetry();
        void pass_pulse(double PULSE1,double PULSE2,double PULSE3,double PULSE4);
        void pass_angle(double THETA);
        void print_Pos();
        
    private:
        void Angular();
        void CalcPosition();
        void CalcSpd();
        void transform();
        
        double Radius;
        double Wradius;
        double Omega[4];
        double spd[4];
        double pulse_a[4],pulse_b[4],diff[4];
        double Xspd,Yspd;
        double Xpower,Ypower;
        double XPos,YPos,Theta;
        double dt;
};

#endif