自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
Diff: Odmetry.hpp
- Revision:
- 2:e189d55ef292
- Child:
- 7:a0b9b6e71e41
diff -r ea880e226e5a -r e189d55ef292 Odmetry.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Odmetry.hpp Fri Jan 28 15:12:04 2022 +0000 @@ -0,0 +1,33 @@ +#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 + \ No newline at end of file