Odmetry.hpp

00001 #ifndef ODMETRY_H
00002 #define ODMETRY_H
00003 
00004 #include "mbed.h"
00005 #define Co (108 * 3.1415926535897) / 512
00006 #define PI 3.1415926535897
00007 
00008 class Odmetry{
00009     public:
00010         Odmetry();
00011         void pass_pulse(double PULSE1,double PULSE2,double PULSE3,double PULSE4);
00012         void pass_angle(double THETA);
00013         void print_Pos();
00014         double obt_CurrentPosX();
00015         double obt_CurrentPosY();
00016         double obt_CurrentTheta();
00017         
00018     private:
00019         void Angular();
00020         void CalcPosition();
00021         void CalcSpd();
00022         void transform();
00023         
00024         double Radius;
00025         double Wradius;
00026         double Omega[4];
00027         double spd[4];
00028         double pulse_a[4],pulse_b[4],diff[4];
00029         double Xspd,Yspd;
00030         double Xpower,Ypower;
00031         double XPos,YPos,Theta;
00032         double dt;
00033 };
00034 
00035 #endif
00036