Revision 8:8ea251946b2a, committed 2019-09-03

Comitter:

ec30109b

Date:

Tue Sep 03 05:33:38 2019 +0000

Parent:

6:d4dba3255f35

Commit message:

keisokurin

Changed in this revision

--- a/measuring_wheel.cpp	Mon Aug 27 06:14:20 2018 +0000
+++ b/measuring_wheel.cpp	Tue Sep 03 05:33:38 2019 +0000
@@ -1,118 +1,135 @@
 #include "measuring_wheel.h"
 
 MeasuringWheel::MeasuringWheel(PinName channel1_1, PinName channel1_2, PinName channel2_1, PinName channel2_2, PinName channel3_1, PinName channel3_2):
-    w1(channel1_1, channel1_2, NC, 500, QEI::X4_ENCODING),
-    w2(channel2_1, channel2_2, NC, 500, QEI::X4_ENCODING),
-    w3(channel3_1, channel3_2, NC, 500, QEI::X4_ENCODING),
-    mainMicon(PC_12,PD_2, 115200),
-    r1370(PC_10, PC_11),
-    led(LED2)
+    w1(channel1_1, channel1_2, NC, 100, QEI::X4_ENCODING),
+    w2(channel2_1, channel2_2, NC, 100, QEI::X4_ENCODING),
+    w3(channel3_1, channel3_2, NC, 100, QEI::X4_ENCODING),
+    serial(mainTX, mainRX),
+    r1370(R1370TX, R1370RX)
+{
+    serial.baud(115200);
+    serial.setHeaders(FIRST_HEDDER, SECOND_HEDDER);
+    miniX = 0;
+    miniY = 0;
+    X = 0;
+    Y = 0;
+    phi = 0;
+    loopCounter = 0;
 
-{
+    /*けいそくりんの設定*/
+    //ホイールの向き
     radian[0] = 0.0;
-    radian[1] = -2.0*PI/3.0;
-    radian[2] = 2.0*PI/3.0;
+    radian[1] = -2.0*PIII/3.0;
+    radian[2] = 2.0*PIII/3.0;
+    //直径
+    diameter[0] = 49.0;
+    diameter[1] = 49.0;
+    diameter[2] = 49.0;
+    //中心からの距離
+    distance[0] = 124.2;
+    distance[1] = 124.2;
+    distance[2] = 124.2;
+
+    coefficient(0) = cos(radian[0]);
+    coefficient(1) = cos(radian[1]);
+    coefficient(2) = cos(radian[2]);
+    coefficient(3) = sin(radian[0]);
+    coefficient(4) = sin(radian[1]);
+    coefficient(5) = sin(radian[2]);
+    coefficient(6) = distance[0];
+    coefficient(7) = distance[1];
+    coefficient(8) = distance[2];
+
+    dec = coefficient.fullPivHouseholderQr();
+
     thread.start(callback(this, &MeasuringWheel::threadloop));
-//    ticker1.attach(callback(this, &MeasuringWheel::transmissionXY),0.1);
-    //mainMicon.attach(callback(this, &MeasuringWheel::resetposition));//受信割り込み
-//    angleTiker.attach(callback(this, &MeasuringWheel::rawAngle),0.1);
-}
-
-
-bool MeasuringWheel::wheelDiameter(float diameter1, float diameter2, float diameter3)
-{
-    diameter[0] = diameter1;
-    diameter[1] = diameter2;
-    diameter[2] = diameter3;
-    return 1;
 }
 
 void MeasuringWheel::computeXY()
 {
-
-    wheel[0] =  diameter[0]*PI/2000.0*w1.getPulses();
-    wheel[1] =  diameter[1]*PI/2000.0*w2.getPulses();
-    wheel[2] =  diameter[2]*PI/2000.0*w3.getPulses();
+    subY = 0;
+    subX = 0;
+    wheel(0) =  -diameter[0]*PIII/400.0*w1.getPulses();
+    wheel(1) =  -diameter[1]*PIII/400.0*w2.getPulses();
+    wheel(2) =  -diameter[2]*PIII/400.0*w3.getPulses();
     w1.reset();
     w2.reset();
     w3.reset();
-    r = (wheel[0] + wheel[1] + wheel[2])/3.0;
-    wheel[0] -= r;
-    wheel[1] -= r;
-    wheel[2] -= r;
-//    r1370.update();
     yawdegree = r1370.getAngle();
-    yaw = yawdegree;
-    yaw *= PI/180;
-//    yaw *= PI/180;
-    for(int i = 0; i < 3; i++) {
-        subX += wheel[i]*cos(radian[i]);
-        subY += wheel[i]*sin(radian[i]);
+    yaw = -1.0*yawdegree;
+    yaw *= PIII/180.0;
+
+    ans = dec.solve(wheel);
+
+    subX = ans(0);
+    subY = ans(1);
+    dphi = ans(2);
+
+    loopCounter++;
+    if(loopCounter > 1000000){
+        loopCounter = 0;
+        X += (miniX + subX*cos((double)yaw) - subY*sin((double)yaw));
+        Y += (miniY + subX*sin((double)yaw) + subY*cos((double)yaw));
+        miniX = 0;
+        miniY = 0;
+    }else{
+        miniX += subX*cos((double)yaw) - subY*sin((double)yaw);
+        miniY += subX*sin((double)yaw) + subY*cos((double)yaw);
     }
-    X += subX*cos((float)yaw) - subY*sin((float)yaw);
-    Y += subX*sin((float)yaw) + subY*cos((float)yaw);
-    X+=subX;
-    Y+=subY;
 
-    X_ = X + 32768;
-    Y_ = Y + 32768;
+    minusX = (X + miniX) * -1.0; // 2019AのはXとYがマイナスなので
+    minusY = (Y + miniY) * -1.0;
+    phi += dphi;
+
+    X_ = minusX + 32768;
+    Y_ = minusY + 32768;
     upBitX = (X_ >> 8) & 0xff;
     downBitX = X_ & 0xff;
     upBitY = (Y_ >> 8) & 0xff;
-    downBitY = Y_ & 0xff;   
-    
-    mainMicon.putc(72);
-    mainMicon.putc(42);
-    mainMicon.putc(upBitX);
-    mainMicon.putc(downBitX);
-    mainMicon.putc(upBitY);
-    mainMicon.putc(downBitY);
-    mainMicon.putc(r1370.upbit());
-    mainMicon.putc(r1370.downbit());
-    mainMicon.putc((unsigned char)(upBitX + downBitY + upBitY + downBitY + r1370.upbit() + r1370.downbit()));
-
+    downBitY = Y_ & 0xff;
+    txdata[0] = upBitX;
+    txdata[1] = downBitX;
+    txdata[2] = upBitY;
+    txdata[3] = downBitY;
+    txdata[4] = r1370.upbit();
+    txdata[5] = r1370.downbit();
+    serial.sendData(txdata, BUFFER_SIZE);
 }
 
 void MeasuringWheel::threadloop()
 {
-    while(true){
+    while(true) {
         computeXY();
-        }
     }
+}
 
-//void MeasuringWheel::rawAngle()
-//{
-//    r1370.update();
-//    yaw = r1370.getRate();
-//}
-
-float MeasuringWheel::getjyroAngle()
+double MeasuringWheel::getjyroAngle()
 {
-    
     return yawdegree;
+    //return phi;
 }
 
-float MeasuringWheel::getOutX()
+double MeasuringWheel::getOutX()
 {
-    return X;
+    return minusX;
 }
 
-float MeasuringWheel::getOutY()
+double MeasuringWheel::getOutY()
 {
-    return Y;
+    return minusY;
 }
 
-float MeasuringWheel::getWheel1()
+double MeasuringWheel::getWheel1()
 {
     return w1.getPulses();
 }
 
-float MeasuringWheel::getWheel2()
+double MeasuringWheel::getWheel2()
 {
     return w2.getPulses();
 }
 
-float MeasuringWheel::getWheel3()
+double MeasuringWheel::getWheel3()
 {
     return w3.getPulses();
 }
@@ -121,13 +138,8 @@
 
 void MeasuringWheel::resetposition()
 {
-    get = mainMicon.getc();
     if(get == 'R') {
         X = 0;
         Y = 0;
-        led = !led;
     }
-
-
 }
-

--- 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