Diff: measuring_wheel.h

Revision:

8:8ea251946b2a

Parent:

3:30045028d27e

--- a/measuring_wheel.h	Mon Aug 27 06:14:20 2018 +0000
+++ b/measuring_wheel.h	Tue Sep 03 05:33:38 2019 +0000
@@ -4,53 +4,55 @@
 #include "mbed.h"
 #include "QEI.h"
 #include "R1370.h"
-//#include "OmniPosition.h"
-#define PI 3.141592653589793
+#include "pin_config.h"
+#include "SerialMultiByte.h"
+#include "Dense.h"
+
+#define FIRST_HEDDER 0xEE
+#define SECOND_HEDDER 0xFF
+#define BUFFER_SIZE 6
 
 class MeasuringWheel
 {
 public :
     MeasuringWheel(PinName channel1_1, PinName channel1_2, PinName channel2_1, PinName channel2_2, PinName channel3_1, PinName channel3_2);
-//    bool
 
-    bool wheelDiameter(float diameter1, float diameter2, float diameter3);
-
-    float getOutX();
-    float getOutY();
+    double getOutX();
+    double getOutY();
     void transmissionXY();
-    float getWheel1(),getWheel2(),getWheel3();
-    float getjyroAngle();
-//    OmniPosition r1370;
-
+    double getWheel1(),getWheel2(),getWheel3();
+    double getjyroAngle();
+    double yaw;
 private :
-    float diameter[3];
-    float radian[3];
-    float wheel[3],r,X,Y,subX,subY;
+    static const double PIII = 3.14159265358979;
+    double diameter[3];
+    double distance[3];
+    double radian[3];
+    Eigen::Matrix3d coefficient;
+    Eigen::Vector3d wheel;
+    Eigen::FullPivHouseholderQR<Eigen::Matrix3d> dec;
+    Eigen::Vector3d ans;
+    double miniX,miniY,X,Y,subX,subY,minusX,minusY,dphi, phi;
+    int loopCounter;
     void computeXY();
     void threadloop();
-    
-       
 
     QEI w1;
     QEI w2;
     QEI w3;
+    SerialMultiByte serial;
     R1370 r1370;
-    float Cdif, yaw, yawdegree;
-//    Ticker ticker;
+    double Cdif, yawdegree;
     Thread thread;
     uint16_t X_, Y_,ofsetX, ofsetY, yawdegree_;
     char upBitX, upBitY, upBitAngle;
     char downBitX,downBitY, downBitAngle;
-    Serial mainMicon;
     char get;
-    DigitalOut led;
-    void resetposition(); 
-    
-//    unsigned char upBitangle down;
-//    void rawAngle();
-    
-    
-    
+    void resetposition();
+    uint8_t txdata[BUFFER_SIZE];
+
+
+
 };
 
-#endif
\ No newline at end of file
+#endif