measuring_wheel.h

00001 #ifndef MEASURING_WHEEL_H
00002 #define MEASURING_WHEEL_H
00003 
00004 #include "mbed.h"
00005 #include "QEI.h"
00006 #include "R1370.h"
00007 #include "Dense.h"
00008 
00009 #define FIRST_HEDDER 0xEE
00010 #define SECOND_HEDDER 0xFF
00011 #define PPR 500.0
00012 #define PIII 3.14159265358979
00013 
00014 class MeasuringWheel
00015 {
00016 public :
00017     MeasuringWheel(PinName channel1_1, PinName channel1_2, PinName channel2_1, PinName channel2_2, PinName channel3_1, PinName channel3_2,PinName R1370TX,PinName R1370RX);
00018 
00019     double getOutX();
00020     double getOutY();
00021     double getWheel1(),getWheel2(),getWheel3();
00022     double getjyroAngle();
00023     double yaw;
00024 private :
00025     double diameter[3];
00026     double distance[3];
00027     double radian[3];
00028     Eigen::Matrix3d coefficient;
00029     Eigen::Vector3d wheel;
00030     Eigen::FullPivHouseholderQR<Eigen::Matrix3d> dec;
00031     Eigen::Vector3d ans;
00032     double miniX,miniY,X,Y,subX,subY,minusX,minusY;
00033     int loopCounter;
00034     void computeXY();
00035     void threadloop();
00036 
00037     QEI w1;
00038     QEI w2;
00039     QEI w3;
00040     R1370 r1370;
00041     double Cdif, yawdegree;
00042     Thread thread;
00043     uint16_t X_, Y_,ofsetX, ofsetY, yawdegree_;
00044     void resetposition();
00045 };
00046 
00047 #endif