Diff: measuring_wheel.cpp

Revision:

8:8ea251946b2a

Parent:

3:30045028d27e

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